add necessary commands/buttons

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index d51dfe76479c73135d26c9d7c0afcedaaddbf3eb..f0cc6e2a0f0286ed0aafe6ce1b3d845e0e68c7a9 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -14,18 +14,22 @@ public class Constants {
   public static class OI {
     public final static int LEFT_STICK_PORT = 0;
     public final static int RIGHT_STICK_PORT = 1;
+    public static final int GAME_PAD_PORT = 2;
 
-    // Need to fill in the port numbers of the following buttons
-    public final static int TOGGLE_FLYWHEEL_PORT = 4;
-    public final static int RUN_INDEXWHEEL_PORT = 1;
-    public final static int REVERSE_INDEXWHEEL_PORT = 2;
     public final static int TOGGLE_GEAR_PORT = 5;
     public final static int RUN_INTAKE_PORT = 1;
     public final static int REVERSE_INTAKE_PORT = 4;
-    public static final int INCREASE_SHOOTER_SPEED_PORT = 6;
-    public static final int DECREASE_SHOOTER_SPEED_PORT = 2;
+
+    public final static int RUN_INDEXWHEEL_PORT = 1;
+    public final static int REVERSE_INDEXWHEEL_PORT = 2;
     public static final int BRAKE_CANTALONS_PORT = 5;
     public static final int COAST_CANTALONS_PORT = 3;
+
+    public final static int TOGGLE_FLYWHEEL_PORT = 4;
+    public static final int REVERSE_FLYWHEEL_PORT = 1;
+    public static final int INCREASE_SHOOTER_SPEED_PORT = 6;
+    public static final int DECREASE_SHOOTER_SPEED_PORT = 2;
+    public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 3;
   }
 
   public static class Shooter {
@@ -43,12 +47,14 @@ public class Constants {
   }
 
   public static class DriveTrain {
-    // GEARS
-    public static final int PISTON_MODULE = 10, LEFT_GEAR_PISTON_FORWARD = 0,
+    public static final int PISTON_MODULE = 10;
+    public static final int GEAR_MANIPULATOR_PISTON_FORWARD = 4,
+        GEAR_MANIPULATOR_PISTON_REVERSE = 5;
+    public static final int LEFT_GEAR_PISTON_FORWARD = 0,
         LEFT_GEAR_PISTON_REVERSE = 1, RIGHT_GEAR_PISTON_FORWARD = 3,
         RIGHT_GEAR_PISTON_REVERSE = 2;
-    public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
-    public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
+    public static final Value FORWARD_PISTON_VALUE = DoubleSolenoid.Value.kForward;
+    public static final Value REVERSE_PISTON_VALUE = DoubleSolenoid.Value.kReverse;
 
     // MOTOR CONTROLLERS
     public static final int FRONT_LEFT = 1;

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index a1abe0ad63a7b3e3a1fd22c450c5b547402d0a71..9f8ea7ae594b5d33d07fa18bcb79935745041915 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -2,11 +2,13 @@ package org.usfirst.frc.team3501.robot;
 
 import org.usfirst.frc.team3501.robot.commands.driving.BrakeCANTalons;
 import org.usfirst.frc.team3501.robot.commands.driving.CoastCANTalons;
-import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
+import org.usfirst.frc.team3501.robot.commands.driving.ToggleDriveGear;
+import org.usfirst.frc.team3501.robot.commands.driving.ToggleGearManipulatorPiston;
 import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
 import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous;
 import org.usfirst.frc.team3501.robot.commands.shooter.DecreaseShootingSpeed;
 import org.usfirst.frc.team3501.robot.commands.shooter.IncreaseShootingSpeed;
+import org.usfirst.frc.team3501.robot.commands.shooter.ReverseFlyWheelContinuous;
 import org.usfirst.frc.team3501.robot.commands.shooter.ReverseIndexWheelContinuous;
 import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
 import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheelContinuous;
@@ -19,12 +21,15 @@ public class OI {
   private static OI oi;
   public static Joystick leftJoystick;
   public static Joystick rightJoystick;
+  public static Joystick gamePad;
 
   public static Button runIndexWheel;
   public static Button reverseIndexWheel;
   public static Button toggleFlyWheel;
+  public static Button reverseFlyWheel;
 
   public static Button toggleGear;
+  public static Button toggleGearManipulatorPiston;
 
   public static Button runIntake;
   public static Button reverseIntake;
@@ -38,6 +43,7 @@ public class OI {
   public OI() {
     leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
     rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT);
+    gamePad = new Joystick(Constants.OI.GAME_PAD_PORT);
 
     runIndexWheel = new JoystickButton(rightJoystick,
         Constants.OI.RUN_INDEXWHEEL_PORT);
@@ -47,13 +53,21 @@ public class OI {
         Constants.OI.REVERSE_INDEXWHEEL_PORT);
     reverseIndexWheel.whileHeld(new ReverseIndexWheelContinuous());
 
-    toggleFlyWheel = new JoystickButton(rightJoystick,
+    toggleFlyWheel = new JoystickButton(gamePad,
         Constants.OI.TOGGLE_FLYWHEEL_PORT);
     toggleFlyWheel.toggleWhenPressed(new RunFlyWheelContinuous());
 
+    reverseFlyWheel = new JoystickButton(gamePad,
+        Constants.OI.REVERSE_FLYWHEEL_PORT);
+    reverseFlyWheel.whileHeld(new ReverseFlyWheelContinuous());
+
     toggleGear = new JoystickButton(leftJoystick,
         Constants.OI.TOGGLE_GEAR_PORT);
-    toggleGear.whenPressed(new ToggleGear());
+    toggleGear.whenPressed(new ToggleDriveGear());
+
+    toggleGearManipulatorPiston = new JoystickButton(gamePad,
+        Constants.OI.TOGGLE_GEAR_MANIPULATOR_PORT);
+    toggleGearManipulatorPiston.whenPressed(new ToggleGearManipulatorPiston());
 
     runIntake = new JoystickButton(leftJoystick, Constants.OI.RUN_INTAKE_PORT);
     runIntake.whileHeld(new RunIntakeContinuous());
@@ -62,11 +76,11 @@ public class OI {
         Constants.OI.REVERSE_INTAKE_PORT);
     reverseIntake.whileHeld(new ReverseIntakeContinuous());
 
-    increaseShooterSpeed = new JoystickButton(leftJoystick,
+    increaseShooterSpeed = new JoystickButton(gamePad,
         Constants.OI.INCREASE_SHOOTER_SPEED_PORT);
     increaseShooterSpeed.whenPressed(new IncreaseShootingSpeed());
 
-    decreaseShooterSpeed = new JoystickButton(leftJoystick,
+    decreaseShooterSpeed = new JoystickButton(gamePad,
         Constants.OI.DECREASE_SHOOTER_SPEED_PORT);
     decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed());
 

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index a8c48643019bffbf7786a505e4264316be539b42..a9fb3bf859cd9fd27cbdbdcc55ca9c7b87c52aaf 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -67,7 +67,7 @@ public class Robot extends IterativeRobot {
 
   @Override
   public void teleopPeriodic() {
-    // driveTrain.printEncoderOutput();
+    driveTrain.printEncoderOutput();
     Scheduler.getInstance().run();
     updateSmartDashboard();
   }

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java
new file mode 100644 (file)
index 0000000..ee15ec8
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command toggles the gear(low or high).
+ *
+ * post-condition: if the drivetrain is running at high gear, it will be made to
+ * run at low gear, and vice versa
+ */
+public class ToggleDriveGear extends Command {
+  DriveTrain driveTrain = Robot.getDriveTrain();
+
+  public ToggleDriveGear() {
+    requires(driveTrain);
+  }
+
+  @Override
+  protected void initialize() {
+
+  }
+
+  @Override
+  protected void execute() {
+    Value leftPistonValue = driveTrain.getLeftDriveTrainPiston();
+    Value rightPistonValue = driveTrain.getRightDriveTrainPiston();
+    if (leftPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) {
+      driveTrain.setHighGear();
+    } else {
+      driveTrain.setLowGear();
+    }
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+  }
+
+  @Override
+  protected void interrupted() {
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
deleted file mode 100644 (file)
index ce74890..0000000
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGear.java
+++ /dev/null
@@ -1,51 +0,0 @@
-package org.usfirst.frc.team3501.robot.commands.driving;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
-
-import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- * This command toggles the gear(low or high).
- *
- * post-condition: if the drivetrain is running at high gear, it will be made to
- * run at low gear, and vice versa
- */
-public class ToggleGear extends Command {
-  DriveTrain driveTrain = Robot.getDriveTrain();
-
-  public ToggleGear() {
-    requires(driveTrain);
-  }
-
-  @Override
-  protected void initialize() {
-
-  }
-
-  @Override
-  protected void execute() {
-    Value leftGearPistonValue = driveTrain.getLeftGearPistonValue();
-    Value rightGearPistonValue = driveTrain.getRightGearPistonValue();
-    if (leftGearPistonValue == Constants.DriveTrain.LOW_GEAR) {
-      driveTrain.setHighGear();
-    } else {
-      driveTrain.setLowGear();
-    }
-  }
-
-  @Override
-  protected boolean isFinished() {
-    return true;
-  }
-
-  @Override
-  protected void end() {
-  }
-
-  @Override
-  protected void interrupted() {
-  }
-}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGearManipulatorPiston.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGearManipulatorPiston.java
new file mode 100644 (file)
index 0000000..eed7f14
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleGearManipulatorPiston.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ToggleGearManipulatorPiston extends Command {
+  private DriveTrain driveTrain = Robot.getDriveTrain();
+
+  public ToggleGearManipulatorPiston() {
+  }
+
+  @Override
+  protected void initialize() {
+    Value gearPistonValue = driveTrain.getGearManipulatorPistonValue();
+    if (gearPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) {
+      driveTrain.extendGearManipulatorPiston();
+    } else {
+      driveTrain.retractGearManipulatorPiston();
+    }
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseFlyWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseFlyWheelContinuous.java
new file mode 100644 (file)
index 0000000..dd4f149
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseFlyWheelContinuous.java
@@ -0,0 +1,38 @@
+package org.usfirst.frc.team3501.robot.commands.shooter;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ReverseFlyWheelContinuous extends Command {
+  private Shooter shooter = Robot.getShooter();
+
+  public ReverseFlyWheelContinuous() {
+  }
+
+  @Override
+  protected void initialize() {
+  }
+
+  @Override
+  protected void execute() {
+    shooter.reverseFlyWheel();
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  protected void end() {
+    shooter.stopFlyWheel();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
index 95940130f11aac8c50ebc54dc29af5d2d13dcf76..815577e24d63cfde14158f23d4d66400b75a636e 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java
@@ -1,6 +1,5 @@
 package org.usfirst.frc.team3501.robot.commands.shooter;
 
-import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
 
@@ -34,14 +33,6 @@ public class RunIndexWheelContinuous extends Command {
 
   @Override
   protected void execute() {
-    if (timeSinceInitialized() % 0.5 <= 0.02) {
-      if (Shooter.getShooter().getPistonValue() == Constants.Shooter.LOW_GEAR) {
-        Shooter.getShooter().setHighGear();
-      } else {
-        Shooter.getShooter().setLowGear();
-      }
-    }
-
     double shooterSpeed = shooter.getShooterRPM();
     double targetShooterSpeed = shooter.getTargetShootingSpeed();
     double threshold = shooter.getRPMThreshold();

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index bad303aebe622f0db77aa8a41d45e79b92c68325..a6a6f8c9ce7e7c7e434e144dcd316a3e3d35e8d9 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -37,7 +37,8 @@ public class DriveTrain extends Subsystem {
   private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
   private final RobotDrive robotDrive;
   private final Encoder leftEncoder, rightEncoder;
-  private final DoubleSolenoid leftGearPiston, rightGearPiston;
+  private final DoubleSolenoid leftDriveTrainPiston, rightDriveTrainPiston;
+  private final DoubleSolenoid gearManipulatorPiston;
 
   private ADXRS450_Gyro imu;
 
@@ -65,12 +66,19 @@ public class DriveTrain extends Subsystem {
     this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
 
     // TODO: Not sure if MODULE_NUMBER should be the same for both
-    leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.PISTON_MODULE,
+    leftDriveTrainPiston = new DoubleSolenoid(
+        Constants.DriveTrain.PISTON_MODULE,
         Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD,
         Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE);
-    rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.PISTON_MODULE,
+    rightDriveTrainPiston = new DoubleSolenoid(
+        Constants.DriveTrain.PISTON_MODULE,
         Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
         Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
+
+    gearManipulatorPiston = new DoubleSolenoid(
+        Constants.DriveTrain.PISTON_MODULE,
+        Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_FORWARD,
+        Constants.DriveTrain.GEAR_MANIPULATOR_PISTON_REVERSE);
   }
 
   public static DriveTrain getDriveTrain() {
@@ -158,38 +166,50 @@ public class DriveTrain extends Subsystem {
    * @return a value that is the current setpoint for the piston kReverse or
    * KForward
    */
-  public Value getLeftGearPistonValue() {
-    return leftGearPiston.get();
+  public Value getLeftDriveTrainPiston() {
+    return leftDriveTrainPiston.get();
   }
 
   /*
    * @return a value that is the current setpoint for the piston kReverse or
    * KForward
    */
-  public Value getRightGearPistonValue() {
-    return rightGearPiston.get();
+  public Value getRightDriveTrainPiston() {
+    return rightDriveTrainPiston.get();
   }
 
   /*
    * Changes the ball shift gear assembly to high
    */
   public void setHighGear() {
-    changeGear(Constants.DriveTrain.HIGH_GEAR);
+    changeGear(Constants.DriveTrain.FORWARD_PISTON_VALUE);
   }
 
   /*
    * Changes the ball shift gear assembly to low
    */
   public void setLowGear() {
-    changeGear(Constants.DriveTrain.LOW_GEAR);
+    changeGear(Constants.DriveTrain.REVERSE_PISTON_VALUE);
   }
 
   /*
    * Changes the gear to a DoubleSolenoid.Value
    */
   private void changeGear(DoubleSolenoid.Value gear) {
-    leftGearPiston.set(gear);
-    rightGearPiston.set(gear);
+    leftDriveTrainPiston.set(gear);
+    rightDriveTrainPiston.set(gear);
+  }
+
+  public Value getGearManipulatorPistonValue() {
+    return gearManipulatorPiston.get();
+  }
+
+  public void extendGearManipulatorPiston() {
+    gearManipulatorPiston.set(Constants.DriveTrain.FORWARD_PISTON_VALUE);
+  }
+
+  public void retractGearManipulatorPiston() {
+    gearManipulatorPiston.set(Constants.DriveTrain.REVERSE_PISTON_VALUE);
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
index faca5ddac1c1e7b9caabab21298a932c8192e5d0..85ecf468d6a2498a420813de8a41e84105c8c0ce 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java
@@ -20,8 +20,9 @@ public class Shooter extends Subsystem {
   private PIDController wheelController;
 
   private static final double RPM_THRESHOLD = 10;
-  private static final double DEFAULT_INDEXING_MOTOR_VALUE = -0.75;
-  private static final double DEFAULT_SHOOTING_SPEED = 3100; // rpm
+  private static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75;
+  private static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75;
+  private static final double DEFAULT_SHOOTING_SPEED = 2700; // rpm
   private static final double SHOOTING_SPEED_INCREMENT = 50;
 
   private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED;
@@ -163,4 +164,8 @@ public class Shooter extends Subsystem {
   private void changeGear(DoubleSolenoid.Value gear) {
     piston.set(gear);
   }
+
+  public void reverseFlyWheel() {
+    this.setFlyWheelMotorVal(shooter.REVERSE_FLYWHEEL_MOTOR_VALUE);
+  }
 }