add commands to digital buttons in OI and create/edit necessary commands

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / OI.java

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index adb37e4033af821e96a0f50487a48059b436d13a..4cd6450fa6acda96b779658017c526d14a1e663f 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,16 +1,20 @@
 package org.usfirst.frc.team3501.robot;
 
+import org.usfirst.frc.team3501.robot.commands.auton.CompactRobot;
 import org.usfirst.frc.team3501.robot.commands.auton.PassChevalDeFrise;
 import org.usfirst.frc.team3501.robot.commands.auton.PassDrawBridge;
 import org.usfirst.frc.team3501.robot.commands.auton.PassPortcullis;
 import org.usfirst.frc.team3501.robot.commands.auton.PassSallyPort;
+import org.usfirst.frc.team3501.robot.commands.driving.Turn180;
 import org.usfirst.frc.team3501.robot.commands.intakearm.IntakeBall;
+import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArmToAngle;
 import org.usfirst.frc.team3501.robot.commands.scaler.ExtendLift;
 import org.usfirst.frc.team3501.robot.commands.scaler.RetractLift;
 import org.usfirst.frc.team3501.robot.commands.scaler.RunWinchContinuous;
 import org.usfirst.frc.team3501.robot.commands.scaler.StopWinch;
 import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
 import org.usfirst.frc.team3501.robot.commands.shooter.runShooter;
+import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Joystick;
@@ -98,8 +102,14 @@ public class OI {
     passDrawbridge.whenPressed(new PassDrawBridge());
     passSallyPort.whenPressed(new PassSallyPort());
 
-    lowerChevalDeFrise
-    .whenPressed(/* TO DO: define this, and fill in commands */);
+    lowerChevalDeFrise.whenPressed(new MoveIntakeArmToAngle(
+        IntakeArm.potAngles[0], IntakeArm.moveIntakeArmSpeed));
+    moveToIntakeBoulder.whenPressed(new MoveIntakeArmToAngle(
+        IntakeArm.potAngles[1], IntakeArm.moveIntakeArmSpeed));
+    poiseAboveChevalDeFrise.whenPressed(new MoveIntakeArmToAngle(
+        IntakeArm.potAngles[2], IntakeArm.moveIntakeArmSpeed));
+    moveIntakeArmInsideRobot.whenPressed(new MoveIntakeArmToAngle(
+        IntakeArm.potAngles[3], IntakeArm.moveIntakeArmSpeed));
 
     if (toggleScalingMode.get()) {
       if (!isScalingMode) {
@@ -117,6 +127,7 @@ public class OI {
     if (!isScalingMode) {
       toggleShooter.toggleWhenPressed(new runShooter());
       compactRobot_1.whenPressed(new CompactRobot());
+      compactRobot_2.whenPressed(new CompactRobot());
 
       intakeBoulder.whenPressed(new IntakeBall());
       shootBoulder.whenPressed(new Shoot());
@@ -125,13 +136,13 @@ public class OI {
       toggleShooter.whenPressed(new RunWinchContinuous(
           Constants.Scaler.WINCH_IN_SPEED));
       compactRobot_1.whenPressed(new RetractLift());
+      compactRobot_2.whenPressed(new RetractLift());
 
       intakeBoulder.whenReleased(new StopWinch());
       shootBoulder.whenPressed(new ExtendLift());
     }
 
-    SpinRobot180_1
-    .whenPressed(/* rotate robot 180, reorient joystick controls */);
-
+    SpinRobot180_1.whenPressed(new Turn180());
+    SpinRobot180_2.whenPressed(new Turn180());
   }
 }