diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index cb81a2c1e..7cf68aa45 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/src/main/java/com/team766/robot/reva/DriverOI.java @@ -108,6 +108,10 @@ public void handleOI(Context context) { drive.setCross(); context.releaseOwnership(drive); + + context.takeOwnership(Robot.lights); + Robot.lights.turnOffFront(); + context.releaseOwnership(Robot.lights); } // TODO: update OI with new optimization OI @@ -169,6 +173,12 @@ public void handleOI(Context context) { context.takeOwnership(drive); drive.stopDrive(); } + + if (isRotatingToSpeaker && drive.isAtRotationTarget() && Robot.shoulder.isFinished()) { + context.takeOwnership(Robot.lights); + Robot.lights.signalCanShoot(); + context.releaseOwnership(Robot.lights); + } } /** diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index a73ecdbff..340b94782 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -3,7 +3,6 @@ import com.team766.ViSIONbase.AprilTagGeneralCheckedException; import com.team766.ViSIONbase.GrayScaleCamera; import com.team766.framework.Mechanism; -import com.team766.robot.reva.Robot; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -29,12 +28,6 @@ public ForwardApriltagCamera() throws AprilTagGeneralCheckedException { } else { throw new AprilTagGeneralCheckedException("Couldn't find alliance correctly"); } - - if (camera.isConnected()) { - Robot.lights.signalCameraConnected(); - } else { - Robot.lights.signalCameraNotConnected(); - } } public GrayScaleCamera getCamera() { 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 ab9737ad5..8d026bea6 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Lights.java @@ -1,74 +1,114 @@ package com.team766.robot.reva.mechanisms; -import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.StrobeAnimation; import com.team766.framework.Mechanism; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import java.util.Optional; public class Lights extends Mechanism { - CANdle m_candle = new CANdle(58); + private CANdle candle; + private static final int CANID = 2; + private double brightness; + + private static final int FRONT_NUM_LEDS = 40; + private static final int FRONT_OFFSET = 8; + private static final int FRONT_SLOT = 1; + + private static final int BACK_NUM_LEDS = 39; + private static final int BACK_OFFSET = 48; + private static final int BACK_SLOT = 2; + + private static final Animation TOO_HIGH_ANIMATION = + new StrobeAnimation(255, 0, 0, 0, 0.00001, FRONT_NUM_LEDS, FRONT_OFFSET); + private static final Animation CAN_SHOOT_ANIMATION = + new RainbowAnimation(1, 0.1, FRONT_NUM_LEDS, false, FRONT_OFFSET); public Lights() { - // Show that robot lights mechanism is ready - Optional alliance = DriverStation.getAlliance(); + this(0.1); + } + + public Lights(double brightness) { + candle = new CANdle(CANID); + setBrightnessNoCheck(brightness); + clear(); + } + + public void signalTooHigh() { + animate(TOO_HIGH_ANIMATION, FRONT_SLOT); + } + + public void signalNoNoteInIntake() { + setAreaColor(0, 0, 255, FRONT_NUM_LEDS, FRONT_OFFSET, FRONT_SLOT); + } + + public void signalNoteInIntake() { + setAreaColor(0, 255, 0, FRONT_NUM_LEDS, FRONT_OFFSET, FRONT_SLOT); + } - if (alliance.isPresent()) { - if (alliance.get().equals(Alliance.Blue)) { - // Blue - m_candle.setLEDs(0, 0, 100); - } else { - // Red - m_candle.setLEDs(100, 0, 0); - } - } else { - // Purple - m_candle.setLEDs(100, 0, 100); - } + public void signalCanShoot() { + animate(CAN_SHOOT_ANIMATION, FRONT_SLOT); } - // Lime green - public boolean signalCameraConnected() { - ErrorCode e = m_candle.setLEDs(92, 250, 40); - return handleErrorCode(e); + public void turnOffFront() { + clearArea(FRONT_NUM_LEDS, FRONT_OFFSET, FRONT_SLOT); } - // Purple - public boolean signalCameraNotConnected() { - ErrorCode e = m_candle.setLEDs(100, 0, 100); - return handleErrorCode(e); + public void signalPlayersGreen() { + candle.clearAnimation(BACK_SLOT); + setAreaColor(0, 255, 0, BACK_NUM_LEDS, BACK_OFFSET, BACK_SLOT); } - // Coral orange - public boolean signalNoteInIntake() { - ErrorCode e = m_candle.setLEDs(255, 127, 80); - return handleErrorCode(e); + public void signalPlayersRed() { + setAreaColor(255, 0, 0, BACK_NUM_LEDS, BACK_OFFSET, BACK_SLOT); } - // Off - public boolean turnLightsOff() { - ErrorCode e = m_candle.setLEDs(0, 0, 0); - return handleErrorCode(e); + public void signalPlayersNothing() { + setAreaColor(0, 0, 0, BACK_NUM_LEDS, BACK_OFFSET, BACK_SLOT); } - // Blue - public boolean signalNoNoteInIntakeYet() { - ErrorCode e = m_candle.setLEDs(0, 0, 100); - return handleErrorCode(e); + private void animate(Animation animation, int slot) { + checkContextOwnership(); + candle.animate(animation, slot); } - public boolean isDoingShootingProcedure() { - ErrorCode e = m_candle.setLEDs(0, 227, 197); - return handleErrorCode(e); + private void clearArea(int count, int offset, int slot) { + setAreaColor(0, 0, 0, count, offset, slot); } - private boolean handleErrorCode(ErrorCode e) { - if (e.equals(ErrorCode.OK)) { - return true; - } + private void setAreaColor(int r, int g, int b, int count, int offset, int slot) { + checkContextOwnership(); + candle.clearAnimation(slot); + candle.setLEDs(r, g, b, 0, r, count); + } + + private void setBrightnessNoCheck(double value) { + brightness = com.team766.math.Math.clamp(value, 0, 1); + candle.configBrightnessScalar(brightness); + } + + public void setBrightness(double value) { + checkContextOwnership(); + setBrightnessNoCheck(value); + } + + public double getBrightness() { + return brightness; + } + + public void changeBrightness(double change) { + setBrightness(brightness + change); + } + + public void clear() { + setColor(0, 0, 0); + } - return false; + private void setColor(int r, int g, int b) { + checkContextOwnership(); + candle.clearAnimation(FRONT_SLOT); + candle.clearAnimation(BACK_SLOT); + candle.setLEDs(r, g, b); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java b/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java index 8af66e72c..dc5be35c4 100644 --- a/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java +++ b/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java @@ -6,19 +6,25 @@ public class IntakeUntilIn extends Procedure { public void run(Context context) { + context.takeOwnership(Robot.lights); + Robot.lights.signalNoNoteInIntake(); + context.releaseOwnership(Robot.lights); + context.takeOwnership(Robot.intake); - Robot.lights.signalNoNoteInIntakeYet(); while (!Robot.intake.hasNoteInIntake()) { Robot.intake.setIntakePowerForSensorDistance(); context.yield(); } - context.releaseOwnership(Robot.intake); + context.takeOwnership(Robot.lights); Robot.lights.signalNoteInIntake(); + context.releaseOwnership(Robot.lights); - context.waitForSeconds(2); + while (Robot.intake.hasNoteInIntake()) { + context.yield(); + } - Robot.lights.turnLightsOff(); + Robot.lights.turnOffFront(); } }