Shooterlights

src/main/java/com/team766/robot/reva/DriverOI.java

@@ -30,6 +30,7 @@ public class DriverOI extends OIFragment {
     protected boolean isCross = false;
 
     private final OICondition movingJoysticks;
+    private final OICondition isAtGoodSpeed;
 
     private LaunchedContext visionContext;
 
@@ -57,6 +58,9 @@ public DriverOI(
                                                         + Math.abs(leftJoystickY)
                                                         + Math.abs(rightJoystickY)
                                                 > 0);
+        isAtGoodSpeed =
+                new OICondition(
+                        () -> Robot.shooter.isCloseToExpectedSpeed() && Robot.shooter.isNotZero());
     }
 
     public void handleOI(Context context) {
@@ -95,20 +99,23 @@ public void handleOI(Context context) {
         visionSpeakerHelper.update();
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_TARGET_SHOOTER)) {
-
             visionContext = context.startAsync(new DriverShootNow());
 
         } else if (leftJoystick.getButtonReleased(InputConstants.BUTTON_TARGET_SHOOTER)) {
             visionContext.stop();
             context.takeOwnership(drive);
             context.takeOwnership(intake);
 
+            Robot.lights.signalShotComplete();
             Robot.intake.stop();
             drive.stopDrive();
 
             context.releaseOwnership(drive);
             context.releaseOwnership(intake);
         }
+        else if(isAtGoodSpeed.isNewlyTriggering()) {
+                Robot.lights.signalAtSpeed();
+            }
 
         // Moves the robot if there are joystick inputs
         if (movingJoysticks.isTriggering()) {

src/main/java/com/team766/robot/reva/mechanisms/Lights.java

@@ -93,4 +93,19 @@ private boolean handleErrorCode(ErrorCode e) {
 
         return false;
     }
+
+    public boolean signalStartSpinup() {
+        ErrorCode e = m_candle.setLEDs(2, 245, 229);
+        return handleErrorCode(e);
+    }
+
+    public boolean signalAtSpeed() {
+        ErrorCode e = m_candle.setLEDs(172, 245, 2);
+        return handleErrorCode(e);
+    }
+
+    public boolean signalShotComplete() {
+        ErrorCode e = m_candle.setLEDs(0, 0, 0);
+        return handleErrorCode(e);
+    }
 }

src/main/java/com/team766/robot/reva/mechanisms/Shooter.java

@@ -49,6 +49,10 @@ public boolean isCloseToExpectedSpeed() {
                 && (Math.abs(targetSpeed - getShooterSpeedBottom()) < SPEED_TOLERANCE));
     }
 
+    public boolean isNotZero() {
+        return ((getShooterSpeedTop() <= SPEED_TOLERANCE) || (getShooterSpeedBottom() <= SPEED_TOLERANCE));
+    }
+
     private double getShooterSpeedTop() {
         return shooterMotorTop.getSensorVelocity();
     }
@@ -94,7 +98,6 @@ public void run() {
         }
 
         // SmartDashboard.putBoolean("Shooter At Speed", isCloseToExpectedSpeed());
-
         // FIXME: problem with this - does not pay attention to changes in PID values
         // https://github.com/Team766/2024/pull/49 adds support to address this
         // until then, this is equivalent to the earlier approach