deeprun final code

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

@@ -28,9 +28,13 @@ public static void init() {
 
     Button moveArmToPos = new JoystickButton(leftDriveStick, 9);
     Button moveArmToHighestRocket = new JoystickButton(leftDriveStick, 8);
+    Button driveClimberForward = new JoystickButton(rightDriveStick, 4);
+    Button driveClimberBackwards = new JoystickButton(rightDriveStick, 5);
 
     moveArmToHighestRocket.whileHeld(new MoveToHighRocket());
     moveArmToPos.whileHeld(new MoveTo47());
+    driveClimberForward.whileHeld(new DriveClimberForward());
+    driveClimberBackwards.whileHeld(new DriveClimberBackwards());
   }
 
   public static Joystick getRightDriveStick() {

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

@@ -42,7 +42,7 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture();
   }
 
-  /**
+  /**h
    * This function is called every robot packet, no matter the mode. Use
    * this for items like diagnostics that you want ran during disabled,
    * autonomous, teleoperated and test.
@@ -89,6 +89,7 @@ public void autonomousInit() {
      * autonomousCommand = new ExampleCommand(); break; }
      */
     System.out.println("auto init");
+    CommandBase.grabber.setForward();
   }
 
   /**

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

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

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

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+public class DriveClimberBackwards extends CommandBase {
+  public DriveClimberBackwards() {
+    super("driveclimberforward");
+    // Use requires() here to declare subsystem dependencies
+    requires(climber);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    System.out.println("Driving climber backwards");
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    climber.driveBack(-0.5);
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+    System.out.println("done driving the climber backwards");
+    climber.driveBack(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("climber backwards drive interrupt");
+    climber.driveBack(0);
+  }
+}

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

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+public class DriveClimberForward extends CommandBase {
+  public DriveClimberForward() {
+    super("driveclimberforward");
+    // Use requires() here to declare subsystem dependencies
+    requires(climber);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    System.out.println("Driving climber forward");
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    climber.driveBack(0.5);
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+    System.out.println("done driving the climber forwards");
+    climber.driveBack(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("climber forward drive interrupted");
+    climber.driveBack(0);
+  }
+}

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

@@ -15,6 +15,7 @@
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PWMTalonSRX;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import frc.robot.RobotMap;
 import frc.robot.commands.ClimberDrive;
@@ -86,9 +87,11 @@ public void driveBack(double amnt) {
   public void toggleAll() {
     if (gg) {
       front.set(REVERSE);
+      Timer.delay(0.195);
       back.set(REVERSE);
     } else {
       front.set(FORWARD);
+      Timer.delay(0.195);
       back.set(FORWARD);
     }
     gg = !gg;

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

@@ -35,14 +35,17 @@ public Grabber() {
 
   public void toggle() {
     if (diskExtended) {
+      System.out.println("Grabber is closing.");
       disk.set(REVERSE);
     } else {
+      System.out.println("Grabber is opening.");
       disk.set(FORWARD);
     }
     diskExtended = !diskExtended;
   }
 
   public void setForward() {
+    System.out.println("Grabber has been set forward");
     disk.set(FORWARD);
     diskExtended = true;
   }

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

@@ -63,7 +63,7 @@ public void camMid() {
 
   public void camLow() {
     System.out.println("low");
-    cameraServo.set(0.25);
+    cameraServo.set(0.10);
   }
   // Put methods for controlling this subsystem
   // here. Call these from Commands.