diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index f9c6527..ebb89ff 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -50,10 +50,12 @@ public class OI { // joystickButtons[0][2].whileHeld(new PIDDrive(400)); //joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect)); joystickButtons[1][0].whileHeld(new AlignVertical()); + joystickButtons[1][1].whileHeld(new SetShooterState(true)); joystickButtons[1][2].whileHeld(new ResetLower(-1)); joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse)); joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot)); - joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot)); + joystickButtons[1][5].whileHeld(new SpinRollers(1, false)); +// joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot)); joystickButtons[1][7].whileHeld(new MoveArm(1)); joystickButtons[1][8].whileHeld(new MoveArm(-1)); } diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 98a2230..a4ecda0 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -9,9 +9,9 @@ public class RobotMap { public static int driveLeftEncoderB = 1; public static int gyro = 1; //Arm - public static double zeroDegrees = 1.469; - public static double ninetyDegrees = 3.274; - public static int armPot = 0; + public static double zeroDegrees = 1.28; + public static double ninetyDegrees = 3.1; + public static int armPot = 3; public static int armLeftMotor = 5; public static int armRightMotor = 6; public static int armBottomLim = 4; diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 04bbed3..f5aaa51 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -19,7 +19,8 @@ public class MainArm extends PIDSubsystem { super("MainArm", 0.06, 0.0, 0.002); getPIDController().setContinuous(false); setSetpoint(70); - enable(); +// enable(); + disable(); } public void initDefaultCommand() { }