Merge pull request #12 from Robocubs/develop

Release Version 2 (Codename "Milford")

src/main/java/org/usfirst/frc/team1701/robot/OI.java

@@ -33,7 +33,6 @@ public class OI {
   public static JoystickButton manualWristUp;
   public static JoystickButton manualWristDown;
   public static JoystickButton gear;
-
   public static JoystickButton manualWristReset;
 
   public static Joystick operation;
@@ -75,13 +74,13 @@ public OI() {
     climbPosition = new JoystickButton(operation, 6);
     climbPosition.whenPressed(new ClimbPosition());
     climb = new JoystickButton(operation, 5);
-    climb.whenPressed(new Climb());
+    climb.whileHeld(new Climb());
     climb.whenReleased(new StopClimb());
     manualWinchUp = new JoystickButton(operation, 3);
-    manualWinchUp.whenPressed(new WinchDown());
+    manualWinchUp.whileHeld(new WinchUp());
     manualWinchUp.whenReleased(new WinchStop());
     manualWinchDown = new JoystickButton(operation, 10);
-    manualWinchDown.whenPressed(new WinchUp());
+    manualWinchDown.whileHeld(new WinchDown());
     manualWinchDown.whenReleased(new WinchStop());
     manualWristUp = new JoystickButton(operation, 7);
     manualWristUp.whenPressed(new WristDown());
@@ -90,9 +89,10 @@ public OI() {
     manualWristDown.whenPressed(new WristUp());
     manualWristDown.whenReleased(new WristStop());
     gear = new JoystickButton(drive_FB,3);
-    gear.whenPressed(new HighGear());
+    gear.whenPressed(new AutoGear());
     gear.whenReleased(new LowGear());
 
+
     manualWristReset = new JoystickButton(operation,15);
     manualWristReset.whenPressed(new ResetWrist());
   }

src/main/java/org/usfirst/frc/team1701/robot/Robot.java

@@ -13,7 +13,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
-import org.usfirst.frc.team1701.robot.commands.AutoCommandGroup;
+import org.usfirst.frc.team1701.robot.commands.*;
 import org.usfirst.frc.team1701.robot.subsystems.*;
 
 /*
@@ -39,44 +39,28 @@ public class Robot extends IterativeRobot {
    */
   private DriverStation ds = DriverStation.getInstance();
   private String gameCode = "";
-  public static SendableChooser<Number> autonomousLocation;
-  public static SendableChooser<Number> action;
   public static OI oi;
   public static DriveTrain driveTrain;
   public static LiftArm liftArm;
   public static Vision vision;
   public static Position position;
+  public static Shuffleboard shuffleboard;
   /*
    * This function is run when the robot is first started up.
    */
   public void robotInit() {
     RobotMap.init(); // Initialize our RobotMap.
+    Shuffleboard.init();
     driveTrain = new DriveTrain();
     vision = new Vision();
     liftArm = new LiftArm();
     position = new Position();
     oi = new OI();
-    autonomousLocation = new SendableChooser<>();
-    autonomousLocation.addObject("Left",1);
-    autonomousLocation.addObject("Left-Switch",4);
-    autonomousLocation.addDefault("Middle",2);
-    autonomousLocation.addObject("Right-Switch",5);
-    autonomousLocation.addObject("Right",3);
-    SmartDashboard.putData("Autonomous Location", autonomousLocation);
-    action = new SendableChooser<>();
-    action.addDefault("Defualt Autonomous", 1);
-    action.addObject("Forward Autonomous", 2);
-    SmartDashboard.putData("Autonomous Chooser",action);
-    SmartDashboard.putBoolean("Reversed", false);
-    SmartDashboard.putString("Current Gear","");
-    SmartDashboard.putNumber("Arm",0);
-    SmartDashboard.putBoolean("Arm Down", false);
-    SmartDashboard.putNumber("D", 0);
 
     vision.setPIPMode(2);
     driveTrain.resetEncoders();
-
-
+    driveTrain.setBrakeMode();
+    driveTrain.setLowGear();
   }
   /*
    * This function is called when the robot has been disabled.
@@ -99,7 +83,7 @@ public void disabledPeriodic() {
    */
   public void autonomousInit() {
     if(gameCode.length() == 3) {
-      CommandGroup autonomousCommand = new AutoCommandGroup(gameCode,action.getSelected(),autonomousLocation.getSelected());
+      CommandGroup autonomousCommand = new AutoCommandGroup(gameCode, Shuffleboard.action.getSelected(), Shuffleboard.autonomousLocation.getSelected());
       autonomousCommand.start();
     }
   }
@@ -128,4 +112,6 @@ public void teleopPeriodic() {
    * This function is called periodically during test mode.
    */
   public void testPeriodic() {}
+
+
 }

src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java

@@ -10,15 +10,13 @@
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-/*import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;*/
+
 
 public class RobotMap {
   /*
    * Create all static variables, most (exception of logger) filled
    * by the init() method below.
    */
-  //private static final Logger logger = LogManager.getLogger();
   public static WPI_TalonSRX _leftFrontMotor;
   public static WPI_TalonSRX _leftBackMotor;
   public static WPI_TalonSRX _rightFrontMotor;
@@ -36,18 +34,18 @@ public class RobotMap {
   public static DoubleSolenoid winchBrake;
   public static DoubleSolenoid wristBrake;
   public static AnalogInput liftArmEncoder;
+  public static AnalogInput wristEncoder;
   public static DigitalInput cubeSensor;
   public static DigitalInput armSensor;
-  public static Spark _led0;
-  public static Spark _led1;
   public static AHRS _navx;
   public static int encPidIdx;
-
+  public static double armSpeed = 1.0;
+
   /**
    * Initialize the public values above.
    */
   public static void init() {
-    _leftFrontMotor = new WPI_TalonSRX(0); //0
+    _leftFrontMotor = new WPI_TalonSRX(3); //0
     _leftBackMotor = new WPI_TalonSRX(1); //1
     _rightFrontMotor = new WPI_TalonSRX(15); //15
     _rightBackMotor = new WPI_TalonSRX(14); //14
@@ -60,6 +58,7 @@ public static void init() {
      * Initialize all analog and digital objects
      */
     liftArmEncoder = new AnalogInput(0);
+    wristEncoder = new AnalogInput(1);
     cubeSensor = new DigitalInput(0);
     armSensor = new DigitalInput(1);
 
@@ -88,12 +87,6 @@ public static void init() {
      * Instantiate NavX.
      */
     _navx = new AHRS(SerialPort.Port.kUSB1);
-    /**
-     * LED controllers, follows Spark motor controller pattern.
-     * @see https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf, page 7
-     */
-    _led0 = new Spark(0);
-    _led1 = new Spark(1);
-
   }
+
 }

src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java

@@ -1,45 +1,56 @@
 package org.usfirst.frc.team1701.robot.commands.Auto;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.usfirst.frc.team1701.robot.Robot;
 
 public class Drive extends Command {
     boolean isFinished = false;
     double distance = 0;
     double currentSpeed;
+    double startAngle;
     boolean setHighGear = false;
 
     public Drive(double distance,double speed) {
+        Robot.driveTrain.driveSpeed = 0;
+
         requires(Robot.driveTrain);
         this.distance = distance;
         this.currentSpeed = speed;
     }
     public Drive(double distance, double speed, boolean setHighGear) {
-        this.distance = distance;
-        this.currentSpeed = speed;
+        this(distance,speed);
         this.setHighGear = setHighGear;
-
     }
     protected void initialize() {
+        startAngle = Robot.driveTrain.getNavxAngle();
         Robot.driveTrain.resetEncoders();
     }
     protected void execute() {
 
-        if(setHighGear) {
+        if(!setHighGear) {
             Robot.driveTrain.setLowGear();
         } else {
             Robot.driveTrain.setHighGear();
         }
 
+        SmartDashboard.putNumber("Drive Train", Robot.driveTrain.getRightDistance());
+
         if(Robot.driveTrain.getRightDistance() < distance) {
-            Robot.driveTrain.teleopControl(-currentSpeed,0);
+            Robot.driveTrain.driveSpeed = -currentSpeed;
+            Robot.driveTrain.setAngle(startAngle);
+            Robot.driveTrain.startPID();
         } else {
+            Robot.driveTrain.stopPID();
+            Robot.driveTrain.stopMotors();
             isFinished = true;
         }
     }
     protected boolean isFinished() {
         return isFinished;
     }
-    protected void end() {}
+    protected void end() {
+        Robot.driveTrain.stopPID();
+    }
     protected void interrupted() {}
 }