diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index d513e81d1..f80aa175f 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/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,7 +99,6 @@ 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)) { @@ -103,12 +106,16 @@ public void handleOI(Context context) { 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()) { diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java b/src/main/java/com/team766/robot/reva/mechanisms/Lights.java index 2f348a726..ce087bf50 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java +++ b/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); + } } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java index 9b91e3f9c..985cf44b0 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java +++ b/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