unstaged changes + pit mayhem

build.gradle

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

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

@@ -27,9 +27,9 @@ public static void init() {
     auxStick = new Joystick(RobotMap.auxillaryStick);
 
     Button moveArmToPos = new JoystickButton(leftDriveStick, 9);
-    Button movePivotDown = new JoystickButton(auxStick, 4);
-    Button movePivotUp = new JoystickButton(auxStick, 5);
+    Button moveArmToHighestRocket = new JoystickButton(leftDriveStick, 8);
 
+    moveArmToHighestRocket.whileHeld(new MoveToHighRocket());
     moveArmToPos.whileHeld(new MoveTo47());
   }
 

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

@@ -105,9 +105,6 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
     //CommandBase.grabber.toggle();
   }
 

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

@@ -18,12 +18,9 @@ public class AutoDriveToTarget extends CommandBase {
 
   public AutoDriveToTarget() {
     super("AutoDrive");
-    System.out.println("wew");
-    // Use requires() here to declare subsystem dependencies
     requires(driveTrain);
     requires(vision);
     requires(climber);
-    System.out.println("lad");
 
     limelight = NetworkTableInstance.getDefault().getTable("limelight");
   }

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

@@ -45,12 +45,12 @@ protected void execute() {
       System.out.println("Toggle grabber");
     }
 
-    if (rightStick.getRawButtonPressed(9)) {
-      climber.driveBack(0.5);
+    if (auxJoystick.getRawButtonPressed(4)) {
+      climber.driveBack(0.25);
     }
 
-    if (auxJoystick.getRawButtonPressed(2)) {
-      climber.driveBack(1);
+    if (auxJoystick.getRawButtonPressed(5)) {
+      climber.driveBack(-0.25);
     }
 
     if (Math.abs(auxJoystick.getY()) <= 0.05) // Arm is disabled, just drives the climb motors here

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

@@ -11,7 +11,6 @@
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Grabber;
-import frc.robot.subsystems.Vacuum;
 import frc.robot.subsystems.Vision;
 import frc.robot.OI;
 
@@ -23,7 +22,6 @@ public class CommandBase extends Command {
   public static Vision vision;
   public static Climber climber;
   public static Grabber grabber;
-  public static Vacuum vacuum;
 
   // Init all subsystems
   public static void init() {
@@ -33,7 +31,6 @@ public static void init() {
     driveTrain = new DriveTrain();
     vision = new Vision();
     grabber = new Grabber();
-    vacuum = new Vacuum();
 
     OI.init();
   }

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

@@ -16,8 +16,6 @@ public class MoveTo47 extends CommandBase {
   public MoveTo47() {
     super("MoveTo47Inches");
     requires(climber);
-    // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
   }
 
   // Called just before this Command runs the first time
@@ -40,6 +38,7 @@ public boolean isFinished() { // Do not worry about this method. It will be inte
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    climber.driveArm(0);
   }
 
   // Called when another command which requires one or more of the same

src/main/java/frc/robot/commands/LiftToHeight.java → src/main/java/frc/robot/commands/MoveToHighRocket.java

@@ -7,38 +7,44 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.RobotMap;
 
-public class LiftToHeight extends Command {
-  public LiftToHeight() {
-    // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
+public class MoveToHighRocket extends CommandBase {
+
+  boolean upThere = false;
+
+  public MoveToHighRocket() {
+    super("MoveToHighRocket");
+    requires(climber);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    System.out.println("Move to high rocket started");
+    climber.setArmPos(-15);
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    System.out.println("high rocket pid exec");
   }
 
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
+  public boolean isFinished() { // Do not worry about this method. It will be interrupted when the button is released (2)
     return false;
   }
 
   // Called once after isFinished returns true
   @Override
   protected void end() {
+      climber.driveArm(0);
   }
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    System.out.println("Move to high rocket interrupted");
   }
 }

src/main/java/frc/robot/subsystems/Climber.java

@@ -45,6 +45,8 @@ public Climber() {
         arm = new CANSparkMax(5, MotorType.kBrushless);
         armEncoder = new CANEncoder(arm);
         armPID = new CANPIDController(arm);
+        armEncoder.getVelocity();
+        armEncoder.getPosition();
 
         backDriveRight.setInverted(true);