Changed shooter wheel to go to "idle speed" (1500rpm) after shooting, instead of turning it off completely (to prevent ball jams from a "stuck" ball in the shooter wheel.) Increased revolver speed for "fast" OI button to full speed (previously half speed), and removed a "dead" file (ShootAndDriveForward.java).

coach-streeter · coach-streeter · commit c25fa83d2cb4 · 2020-02-28T22:01:22.000-05:00
diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java
@@ -199,8 +199,7 @@ private void configureDriverPadButtons() {
         DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0));
         DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0));
         DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0));
-        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON
-                .whenPressed(new ShooterWheelSet(RobotContainer.shooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED));
+        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED));
 
         DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1));
 
@@ -217,7 +216,7 @@ private void configureOperatorStickButtons() {
     private void configureOperatorPadButtons() {
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(0.5));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(1.0));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
 
         // new ShooterSetWheel(1000));
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java
@@ -46,7 +46,7 @@ public StartBWOppTrench() {
 
                 // in shooting position, prepare everything for shooting
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
-                                new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
+                                new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED),
                                 new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION + 3000.0),
                                 new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
@@ -60,7 +60,7 @@ public StartBWOppTrench() {
                 addCommands(new ShooterFiringSequence(5.0));
 
                 // turn the shooter wheel and intake off now that the shooting is all done
-                addCommands(new ShooterWheelSet(0.0));
+                addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED));
                 addCommands(new IntakeSetRollers(0.0));
 
                 // turn the wheel off now that the shooting is all done
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java
@@ -27,18 +27,12 @@ public StartBWShoot3() {
                 // is turned towards the target
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
                                 new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
-                                new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
+                                new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED),
                                 new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
                 addCommands(new ShooterFiringSequence(1.5));
 
                 // note that the above turns everything off again when it is done.
-
-                // turn the shooter wheel and intake off now that the shooting is all done
-                // addCommands(new ParallelRaceGroup( // below commands in parallel
-                // new ShooterWheelSet(0.0), //
-                // new IntakeSetRollers(0.0), // turn off the rollers
-                // new HoodSetAbs(Hood.STARTING_POSITION)));
         }
 }
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java
@@ -49,8 +49,7 @@ public StartBWTrench(double extraDistance) {
 
                 // drive diagonally over towards the shooting position, while turning on shooter
                 // wheels, raising the hood, and re-aiming the turret
-                addCommands(new ParallelCommandGroup(
-                                new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
+                addCommands(new ParallelCommandGroup(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED),
                                 new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)),
                                 new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160)));
@@ -65,7 +64,7 @@ public StartBWTrench(double extraDistance) {
                 addCommands(new ShooterFiringSequence(6.0));
 
                 // turn the shooter wheel and intake off now that the shooting is all done
-                addCommands(new ShooterWheelSet(0.0));
+                addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED));
                 addCommands(new IntakeSetRollers(0.0));
 
                 // turn the wheel off now that the shooting is all done
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java
@@ -32,7 +32,7 @@ public StartFWRendezvous() {
                 // is turned towards the target
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
                                 new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
-                                new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
+                                new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED),
                                 new HoodSetAbs(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
@@ -63,15 +63,15 @@ public StartFWRendezvous() {
 
                 // in shooting position, prepare everything for shooting
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
-                                new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_TRENCH_FRONT_SPEED),
+                                new ShooterWheelSet(ShooterWheel.TRENCH_FRONT_SPEED),
                                 new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
                 // turn on the intake gently while shooting to help balls settle
                 addCommands(new IntakeSetRollers(-0.2));
                 addCommands(new ShooterFiringSequence(6.0));
 
                 // turn the shooter wheel and intake off now that the shooting is all done
-                addCommands(new ShooterWheelSet(0.0));
+                addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED));
                 addCommands(new IntakeSetRollers(0.0));
 
                 // turn the wheel off now that the shooting is all done
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java
@@ -27,15 +27,15 @@ public StartFWShoot3() {
                 // is turned towards the target
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
                                 new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
-                                new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
+                                new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED),
                                 new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
                 addCommands(new ShooterReadyAimFire(1.0));
 
                 // turn the shooter wheel and intake off now that the shooting is all done
                 addCommands(new ParallelCommandGroup( // below commands in parallel
-                                new ShooterWheelSet(0.0), //
+                                new ShooterWheelSet(ShooterWheel.IDLE_SPEED), //
                                 new IntakeSetRollers(0.0), // turn off the rollers
                                 new HoodSetAbs(Hood.STARTING_POSITION)));
         }
diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java
@@ -8,6 +8,8 @@
 package org.mayheminc.robot2020.commands;
 
 import org.mayheminc.robot2020.subsystems.Hood;
+import org.mayheminc.robot2020.subsystems.ShooterWheel;
+
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
@@ -27,7 +29,7 @@ public ShooterCeaseFire() {
     // (feed roller, chimney, revolver, and shooter wheels)
 
     addCommands(new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0),
-        new ShooterWheelSetVBus(0.0), new Wait(0.1)));
+        new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new Wait(0.1)));
 
     // Lower the hood now that we're done shooting
     addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION));
diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java
@@ -7,6 +7,9 @@
 
 package org.mayheminc.robot2020.commands;
 
+import org.mayheminc.robot2020.RobotContainer;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
@@ -22,11 +25,11 @@ public ShooterFiringSequence(double waitDuration) {
     // super(new FooCommand(), new BarCommand());
     super();
 
-    // Turn off compressor while actively shooting.
-    addCommands(new AirCompressorPause());
-
-    // one last aim at the target before starting shooting.
-    addCommands(new ShooterAimToTarget());
+    // shooting.
+    addCommands(new ParallelCommandGroup( // prepare for shooting,
+        new AirCompressorPause(), // Turn off compressor while actively shooting,
+        new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), // ensure intake is lowered,
+        new ShooterAimToTarget())); // take one last aim at the target before starting
 
     addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTargetContinuously()));
 
@@ -36,7 +39,7 @@ public ShooterFiringSequence(double waitDuration) {
     addCommands(new ParallelRaceGroup( //
         new TurretAimToTargetContinuously(), // continue aiming while shooting
         new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)),
-        new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration)));
+        new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.5)), new Wait(waitDuration)));
 
     // turn off the feeder, chimney, and revolver, ending after 0.1 seconds
     addCommands(new ShooterCeaseFire());
diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java
@@ -20,9 +20,10 @@ public class ShooterWheel extends SubsystemBase implements PidTunerObject {
     private final double SECONDS_PER_MINUTE = 60.0;
     private final double HUNDRED_MS_PER_SECOND = 10.0;
 
-    public static final double SHOOTER_WHEEL_INITIATION_LINE_SPEED = 3000.0;
-    public static final double SHOOTER_WHEEL_TRENCH_FRONT_SPEED = 3400.0;
-    private static final double MAX_SPEED_RPM = 3500;
+    public static final double IDLE_SPEED = 1500.0;
+    public static final double INITIATION_LINE_SPEED = 3000.0;
+    public static final double TRENCH_FRONT_SPEED = 3400.0;
+    public static final double MAX_SPEED_RPM = 3600;
 
     double m_targetSpeedRPM;
 
diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java
@@ -206,7 +206,13 @@ private double getHoodFromY() {
    * @return
    */
   private double getWheelSpeedFromY() {
-    return 2618 + -1939 * m_bestY + 3583 * m_bestY * m_bestY;
+    double computedWheelSpeed = 2618 + -1939 * m_bestY + 3583 * m_bestY * m_bestY;
+    if (computedWheelSpeed < ShooterWheel.IDLE_SPEED) {
+      computedWheelSpeed = ShooterWheel.IDLE_SPEED;
+    } else if (computedWheelSpeed > ShooterWheel.MAX_SPEED_RPM) {
+      computedWheelSpeed = ShooterWheel.MAX_SPEED_RPM;
+    }
+    return computedWheelSpeed;
   }
 
   /**
