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

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 66a37d8fec9e9819e53c4401982d1322d386e717..59bbcca3f1baf6561bcd3882aa8a0c4dfc3806b5 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -69,6 +69,8 @@ public class Constants {
     public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
     public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
 
+    public static boolean inverted = false;
+
   }
 
   public static class Scaler {

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());
   }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java b/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java
new file mode 100644 (file)
index 0000000..863d39b
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/Turn180.java
@@ -0,0 +1,16 @@
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Constants;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class Turn180 extends CommandGroup {
+
+  public Turn180() {
+    addSequential(new TurnForAngle(180, 5));
+    Constants.DriveTrain.inverted = !Constants.DriveTrain.inverted;
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index de2626bed86e43a5e5fdddde5eca8ab2374f471b..1282be1ba231ce4216fba8f32307ebd7f80f41ca 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -42,7 +42,7 @@ public class DriveTrain extends PIDSubsystem {
   // Drivetrain specific constants that relate to the PID controllers
   private final static double Kp = 1.0, Ki = 0.0,
       Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-          / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
+      / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
 
   public DriveTrain() {
     super(kp, ki, kd);
@@ -74,6 +74,7 @@ public class DriveTrain extends PIDSubsystem {
         +Constants.DriveTrain.LEFT_REVERSE);
     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
         Constants.DriveTrain.RIGHT_REVERSE);
+    Constants.DriveTrain.inverted = false;
   }
 
   @Override
@@ -220,8 +221,12 @@ public class DriveTrain extends PIDSubsystem {
   }
 
   public void drive(double left, double right) {
-    robotDrive.tankDrive(-left, -right);
     // dunno why but inverted drive (- values is forward)
+    if (!Constants.DriveTrain.inverted)
+      robotDrive.tankDrive(-left,
+          -right);
+    else
+      robotDrive.tankDrive(right, left);
   }
 
   public void driveDistance(double dist, double maxTimeOut) {

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
index a9e9ecd5148c0f76dd045c8e38b4550dc47373cf..5e00d05f0df999e6870ba06e5b32d8bee168002d 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
@@ -23,7 +23,8 @@ public class IntakeArm extends Subsystem {
   private CANTalon intakeRoller;
   private CANTalon intakeArm;
   private AnalogPotentiometer intakePot;
-  private double[] potAngles = { 0, 45, 90 };
+  public static double[] potAngles = { 0, 30, 45, 90 }; // TODO: correct angles
+  public static double moveIntakeArmSpeed = 0;
 
   public IntakeArm() {
     intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);