Not tested zhiqin

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.2.1"
+    id "edu.wpi.first.GradleRIO" version "2019.4.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"

src/main/java/frc/robot/OI.java

@@ -7,13 +7,19 @@
 
 package frc.robot;
 
+import java.awt.event.KeyListener;
+import java.util.ResourceBundle.Control;
+
+import edu.wpi.first.wpilibj.Controller;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import frc.robot.commands.ArmToNextPosition;
-import frc.robot.commands.ClawReverse;
-import frc.robot.commands.MoveClaw;
-import frc.robot.commands.SetSpeed;
+import edu.wpi.first.wpilibj.buttons.Trigger;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.*;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.CameraI2c;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -49,19 +55,44 @@ public class OI {
   // button.whenReleased(new ExampleCommand());
 
   public static XboxController xBoxControl = new XboxController(0);
-
+  public static Joystick xBoxControlArm = new Joystick(1);
+  Button pistonExtend = new JoystickButton(xBoxControl, 8);
+  Button pistonRetract = new JoystickButton(xBoxControl, 7);
+  Button lowRocket_c = new JoystickButton(xBoxControlArm, 1);
+  Button midRocket_c = new JoystickButton(xBoxControlArm, 2);
+  Button button = new JoystickButton(xBoxControl, 2);
+  Button climb = new JoystickButton(xBoxControl, 1);
+  Button highRocket_c = new JoystickButton(xBoxControlArm, 3);
+  Button lowRocket_h = new JoystickButton(xBoxControlArm, 4);
+  Button cargoShip_h = new JoystickButton(xBoxControlArm, 6);
+  Button midRocket_h = new JoystickButton(xBoxControlArm, 5);
   public OI() {
-   // Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
-    Button b1 = new JoystickButton(xBoxControl, 6);
-    b1.whenPressed(new SetSpeed(true));
-    //b1.whenReleased(new SetSpeed(false));
-
-    Button b2 = new JoystickButton(xBoxControl, 5);
-    b2.whenPressed(new SetSpeed(false));
-    b2.close();
-    Button aButton = new JoystickButton(xBoxControl, 1);
-    aButton.whenPressed(new ArmToNextPosition());
-    aButton.close();
+    //Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
 
+    pistonExtend.whenPressed(new ExtendPiston());
+
+    pistonRetract.whenPressed(new RetractPiston());
+    climb.whenPressed(new ClimbStair());
+    button.whenPressed(new TurnDegrees());
+
+
+    lowRocket_c.whenPressed(new MoveArmTo(1));
+
+    midRocket_c.whenActive(new MoveArmTo(2));
+
+    highRocket_c.whenActive(new MoveArmTo(3));
+
+
+    lowRocket_h.whenPressed(new MoveArmTo(4));
+
+    midRocket_h.whenPressed(new MoveArmTo(5));
+    //not possible to do high rocket hatch due to no teleArm
+
+
+    cargoShip_h.whenPressed(new MoveArmTo(6));
+
+    /*Button aButton = new JoystickButton(xBoxControl, 1);
+    aButton.whenPressed(new ExtendPiston());
+    aButton.whenReleased(new StopTeleArm());*/
   }
 }

src/main/java/frc/robot/Robot.java

@@ -9,26 +9,26 @@
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.Compressor;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.ExampleCommand;
-import frc.robot.commands.GetDistance;
-import frc.robot.commands.PneumaticsDrive;
 import frc.robot.commands.Teleop;
+import frc.robot.commands.TurnDegrees;
 import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.ArmLimitSwitch;
-import frc.robot.subsystems.BaseLimitSwitch;
 import frc.robot.subsystems.CameraI2c;
 import frc.robot.subsystems.Claw;
+import frc.robot.subsystems.Climb;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.LaserFinder;
+import frc.robot.subsystems.Photoresistor;
 import frc.robot.subsystems.pneumatics;
-import frc.robot.subsystems.SerialDistance;
 import frc.robot.subsystems.UltraSonic;
+import frc.robot.subsystems.Wrist;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -42,30 +42,34 @@ public class Robot extends TimedRobot {
   public static pneumatics m_pneumatics;
   public static OI m_oi;
   public static DriveTrain m_driveTrain;
+  //public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static Teleop m_teleop;
   Command m_autonomousCommand;
   Compressor com = new Compressor(0);
   SendableChooser<Command> m_chooser = new SendableChooser<>();
-  public static SerialDistance m_serialPort;
   public static UltraSonic m_ultraSonic;
   public static Claw claw;
   public static CameraI2c camera;
   public static Arm arm;
-  public static ArmLimitSwitch m_limitSwitch;
-  public static BaseLimitSwitch m_baseSwitch;
+  public static Photoresistor resistor;
+  public static Timer m_timer;
+  public static LaserFinder m_laser;
+  public static Wrist m_wrist;
+  public static Climb m_climb;
+  //public static Ramps m_ramps;
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
    * 
    */
   @Override
   public void robotInit() {
+    SmartDashboard.putBoolean("Arm moving",false);
     com.setClosedLoopControl(true);
     com.start();
+    m_climb = new Climb();
     claw = new Claw();
     arm = new Arm();
-    m_baseSwitch = new BaseLimitSwitch();
-    m_limitSwitch = new ArmLimitSwitch();
     camera = new CameraI2c();
     m_driveTrain = new DriveTrain();
     m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
@@ -74,11 +78,21 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture(0);
     CameraServer.getInstance().startAutomaticCapture(1);
     Scheduler.getInstance().add(new Teleop());
-   // m_serialPort = new SerialDistance();
-   // m_ultraSonic = new UltraSonic();
-    Scheduler.getInstance().add(new GetDistance());
+    resistor = new Photoresistor();
+    m_timer = new Timer();
+    m_laser = new LaserFinder();
+    SmartDashboard.putBoolean("TankDrive", false);
+    m_wrist = new Wrist();
+    m_climb.dropFront();
+    m_climb.dropBack();
+    m_ultraSonic = new UltraSonic();
+
+    //Scheduler.getInstance().add(new GetDistance());
     //OI must be init last
+    SmartDashboard.putNumber("Claw Speed", claw.speed);
+    SmartDashboard.putNumber("Offset", TurnDegrees.offset); 
     m_oi = new OI();
+    m_timer.start();
   }
 
   /**
@@ -91,7 +105,44 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+    SmartDashboard.putNumber("Wrist Encoder", m_wrist.getAngle());
+    SmartDashboard.putNumber("Reed switch", arm.readPos());
+    SmartDashboard.putNumber("String pot", arm.readPos());
+    SmartDashboard.putNumber("UltraSonic", m_ultraSonic.getDistance());
+    //--------------------------Do 10 times per Second --------------------------------------------------
+    if(m_timer.hasPeriodPassed(.1)){
+      CameraI2c.read();
+      m_laser.read();
+      m_timer.reset();
+    }
+    ////////////////////////////////////////////////////////////////////////////////////////////////////
+    //m_serialPort.getString();  //REMOVE LATER LSKJDFLKSDFLJAFHSDGJOEWJ9248&(*#@!^%(*!@&$(*@&$37)))
+    //CameraI2c.read();
+    /*
+    SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
+    SmartDashboard.putNumber("Gyro-Y", m_driveTrain.getGyroY());
+    SmartDashboard.putN
+    umber("Gyro-Z", m_driveTrain.getGyroZ());
+    */
+    /*
+    TurnDegrees.offset = SmartDashboard.getNumber("Offset",  TurnDegrees.offset);
+    SmartDashboard.putNumber("Offset", TurnDegrees.offset);
+    /*
+    SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
+    SmartDashboard.putNumber("Gyro-Y", imu.getAngleY());
+    SmartDashboard.putNumber("Gyro-Z", imu.getAngleZ());
+
+    SmartDashboard.putNumber("Accel-X", imu.getAccelX());
+    SmartDashboard.putNumber("Accel-Y", imu.getAccelY());
+    SmartDashboard.putNumber("Accel-Z", imu.getAccelZ());
+
+    SmartDashboard.putNumber("Pitch", imu.getPitch());
+    SmartDashboard.putNumber("Roll", imu.getRoll());
+    SmartDashboard.putNumber("Yaw", imu.getYaw());
 
+    SmartDashboard.putNumber("Pressure: ", imu.getBarometricPressure());
+    SmartDashboard.putNumber("Temperature: ", imu.getTemperature()); 
+    */
   }
   @Override
   public void disabledInit() {
@@ -159,6 +210,7 @@ public void teleopInit() {
    */
   @Override
   public void teleopPeriodic() {
+    //CameraI2c.read();
     Scheduler.getInstance().run();
   }
 

src/main/java/frc/robot/RobotMap.java

@@ -24,11 +24,42 @@ public class RobotMap {
   // number and the module. For example you with a rangefinder:
   // public static int rangefinderPort = 1;
   // public static int rangefinderModule = 1;
-  public static int UltraSonic = 1;
-  public static int ArmLimitSwitch = 2;
-  public static int leftMotor1 = 0;
-  public static int leftMotor2 = 1;
-  public static int leftMotor3 = 2;
-  public static int leftMotor4 = 3;
-  public static int arm = 4;
+
+  //Analog
+  public final static int PHOTORESISTOR = 0;  
+  public final static int ULTRASONIC = 3;
+  public static final int STRING_POT = 2;
+  //public final static int ARMLIMITSWITCH = 2;
+
+  //DIO
+  public final static int ARM_REED_2 = 2;  
+  public final static int ARM_REED_3 = 3;
+  public final static int ARM_SWITCHBOTTOM = 0;
+  public final static int ARM_REED_1 = 1; //UNUSED(?)
+  //public final static int ULTRASONIC = 4;
+  public final static int ULTRASONIC_ECHO = 5;
+  public final static int WRIST_ENCODER_A = 6;
+  public final static int WRIST_ENCODER_B = 7;
+
+  //CAN
+  public final static int ARM_L = 4;//
+  public final static int ARM_R = 5;//
+  public final static int CLAW_1 = 6;//
+  public final static int CLAW_2 = 8;//
+  public final static int LEFTMOTOR_1 = 3;  //
+  public final static int LEFTMOTOR_2 = 1;//
+  public final static int RIGHTMOTOR_1 = 2;//
+  public final static int RIGHTMOTOR_2 = 0; //
+  public final static int WRISTMOTOR = 7;//
+
+   //Pneumatics
+  public final static int PNEUMATIC_COMPRESSOR = 0;
+  public final static int DOUBLESOL_FORWARD = 0;
+  public final static int DOUBLESOL_REVERSE = 1;
+  public final static int DOUBLESOL_FORWARD1 = 2;
+  public static final int DOUBLESOL_REVERSE1 = 3;
+  public static final int CLIMBSOL_FRONT_FORWARD = 7;
+  public static final int CLIMBSOL_FRONT_REVERSE = 6;
+  public static final int CLIMBSOL_BACK_FORWARD = 4;
+  public static final int CLIMBSOL_BACK_REVERSE =5;
 }

src/main/java/frc/robot/commands/ArmDrive.java

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.OI;
+import frc.robot.Robot;
+
+public class ArmDrive extends Command {
+  public static final double WRIST_POSISTION_STRAIGHT = 15;
+  int count = 1;
+  public ArmDrive() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.arm);
+    requires(Robot.m_wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    SmartDashboard.putBoolean("Swtich one", Robot.arm.switchOne());
+    SmartDashboard.putBoolean("Switch two", Robot.arm.switchTwo());
+    SmartDashboard.putBoolean("Switch three", Robot.arm.switchThree());
+    SmartDashboard.putNumber("Arm position", Robot.arm.readPos());
+    if(OI.xBoxControl.getBumper(Hand.kLeft)){
+      Robot.arm.move();
+      SmartDashboard.putBoolean("Arm moving", true);
+    }else if(OI.xBoxControl.getBumper(Hand.kRight)){
+      Robot.arm.moveDown();
+      SmartDashboard.putBoolean("Arm moving", true);
+    }else{
+      SmartDashboard.putBoolean("Arm moving", false);
+      Robot.arm.stop();
+    }
+    if(OI.xBoxControl.getY(Hand.kLeft) > .1 || OI.xBoxControl.getY(Hand.kLeft) < .1){
+      Robot.m_wrist.raise(OI.xBoxControl.getY(Hand.kLeft));
+    }
+    if(OI.xBoxControl.getStickButton(Hand.kLeft)){
+      Robot.m_wrist.setTarget(WRIST_POSISTION_STRAIGHT);
+      Robot.m_wrist.move();
+    }
+
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}