not so pretty, but it works :/

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

@@ -27,14 +27,10 @@ public static void init() {
     auxStick = new Joystick(RobotMap.auxillaryStick);
 
     Button moveArmToPos = new JoystickButton(leftDriveStick, 9);
-    Button driveVacuum = new JoystickButton(auxStick, 8);
     Button movePivotDown = new JoystickButton(auxStick, 4);
     Button movePivotUp = new JoystickButton(auxStick, 5);
 
     moveArmToPos.whileHeld(new MoveTo47());
-    driveVacuum.whileHeld(new RunVacuumForward());
-    movePivotDown.whileHeld(new MovePivotDown());
-    movePivotUp.whileHeld(new MovePivotUp());
   }
 
   public static Joystick getRightDriveStick() {

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

@@ -36,7 +36,7 @@ public class RobotMap {
   public static final int leftBackMotor = 4;
   public static final int rightFrontMotor = 1;
   public static final int rightBackMotor = 2;
-  public static final float speedLimiter = 0.2f; // Out of 1.0, this value represents the max speed that the robot will
+  public static final float speedLimiter = 0.4f; // Out of 1.0, this value represents the max speed that the robot will
                                                   // go. 0.8, for example, means 80% max motor speed
 
   public static class rightStick {

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

@@ -8,7 +8,6 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
-import frc.robot.subsystems.ArmPivot;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.Grabber;
@@ -25,7 +24,6 @@ public class CommandBase extends Command {
   public static Climber climber;
   public static Grabber grabber;
   public static Vacuum vacuum;
-  public static ArmPivot pivot;
 
   // Init all subsystems
   public static void init() {
@@ -36,7 +34,6 @@ public static void init() {
     vision = new Vision();
     grabber = new Grabber();
     vacuum = new Vacuum();
-    pivot = new ArmPivot();
 
     OI.init();
   }

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

@@ -15,6 +15,7 @@
 public class VisionControl extends CommandBase {
 
   Joystick rightStick = OI.getRightDriveStick();
+  Joystick auxJoystick = OI.getAuxStick();
   int visionMode = 0;
 
   public VisionControl() {
@@ -45,6 +46,18 @@ protected void execute() {
     if (rightStick.getRawButtonPressed(RobotMap.rightStick.cycleCamMode)) {
       vision.toggleCamMode();
     }
+
+    if (auxJoystick.getRawButtonPressed(4)) {
+      vision.camLow();
+    }
+
+    if (auxJoystick.getRawButtonPressed(2)) {
+      vision.camMid();
+    }
+
+    if (auxJoystick.getRawButtonPressed(5)) {
+      vision.camHigh();
+    }
   }
 
   // Make this return true when this Command no longer needs to run execute()

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

@@ -51,12 +51,12 @@ public Climber() {
     front.set(REVERSE);
     back.set(REVERSE);
 
-    armPID.setP(0.05);
+    armPID.setP(0.25);
     armPID.setI(0);
-    armPID.setD(1);
+    armPID.setD(0.20);
     armPID.setIZone(0);
     armPID.setFF(0);
-    armPID.setOutputRange(-0.15, 0.5);
+    armPID.setOutputRange(-0.15, 0.6);
   }
 
   public void setArmPos(double revs) {

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

@@ -29,7 +29,8 @@ public class Grabber extends Subsystem {
 
   public Grabber() {
     System.out.println("Grabber subsystem init");
-    disk = new DoubleSolenoid(RobotMap.pcm.auxPcm, 0, 1); // The disk grabber solenoid is plugged into the main pcm (ID=1), 0 being the open forward channel and 1 being the reverse
+    disk = new DoubleSolenoid(RobotMap.pcm.mainPcm, 4, 5); // The disk grabber solenoid is plugged into the main pcm (ID=1), 0 being the open forward channel and 1 being the reverse
+    disk.set(REVERSE);
   }
 
   public void toggle() {

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

@@ -10,6 +10,7 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import frc.robot.commands.VisionControl;
 
@@ -20,9 +21,11 @@ public class Vision extends Subsystem {
 
   NetworkTable nTable;
   NetworkTableEntry tx, ty, ta, tv, ts;
+  Servo cameraServo;
   boolean driverMode;
 
   public Vision() {
+    cameraServo = new Servo(7);
     nTable = NetworkTableInstance.getDefault().getTable("limelight");
     tx = nTable.getEntry("tx");
     ty = nTable.getEntry("ty");
@@ -47,6 +50,21 @@ public void toggleCamMode() {
       nTable.getEntry("camMode").setDouble(0);
     }
   }
+
+  public void camHigh() {
+    System.out.println("high");
+    cameraServo.set(0.7);
+  }
+
+  public void camMid() {
+    System.out.println("mid");
+    cameraServo.set(0.5);
+  }
+
+  public void camLow() {
+    System.out.println("low");
+    cameraServo.set(0.25);
+  }
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   @Override