From 3402fb88ea519560c67e3f3023e5d49bdf3d72ba Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 20 Mar 2024 19:53:54 -0700 Subject: [PATCH 1/5] Added new light class --- .../mechanisms/ForwardApriltagCamera.java | 11 +- .../team766/robot/reva/mechanisms/Lights.java | 125 +++++++++++------- .../robot/reva/procedures/IntakeUntilIn.java | 8 +- 3 files changed, 86 insertions(+), 58 deletions(-) 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..3d851c356 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; @@ -30,11 +29,11 @@ public ForwardApriltagCamera() throws AprilTagGeneralCheckedException { throw new AprilTagGeneralCheckedException("Couldn't find alliance correctly"); } - if (camera.isConnected()) { - Robot.lights.signalCameraConnected(); - } else { - Robot.lights.signalCameraNotConnected(); - } + // 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 d7d29a209..41fecdae5 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,103 @@ 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(2); + private CANdle candle; + private static final int CANID = 2; + private double brightness; public Lights() { - // Show that robot lights mechanism is ready - Optional alliance = DriverStation.getAlliance(); - - 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); - } + this(0.1); } - // Lime green - public boolean signalCameraConnected() { - ErrorCode e = m_candle.setLEDs(92, 250, 40); - return handleErrorCode(e); + public Lights(double brightness) { + candle = new CANdle(CANID); + setBrightness(brightness); } - // Orange - public boolean signalCameraNotConnected() { - ErrorCode e = m_candle.setLEDs(250, 87, 0); - return handleErrorCode(e); + 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 Animation TOO_HIGH_ANIMATION = + new StrobeAnimation(255, 0, 0, 0, 0.00001, FRONT_NUM_LEDS, FRONT_OFFSET); + + public void signalTooHigh() { + animate(TOO_HIGH_ANIMATION, FRONT_SLOT); } - // Forest green - public boolean signalNoteInIntake() { - ErrorCode e = m_candle.setLEDs(0, 112, 50); - return handleErrorCode(e); + private static final Animation CAN_SHOOT_ANIMATION = + new RainbowAnimation(1, 0.1, FRONT_NUM_LEDS, false, FRONT_OFFSET); + + public void signalCanShoot() { + animate(CAN_SHOOT_ANIMATION, FRONT_SLOT); } - // Off - public boolean turnLightsOff() { - ErrorCode e = m_candle.setLEDs(0, 0, 0); - return handleErrorCode(e); + public void signalNothingFront() { + clearArea(FRONT_NUM_LEDS, FRONT_OFFSET); } - // Blue - public boolean signalNoNoteInIntakeYet() { - ErrorCode e = m_candle.setLEDs(0, 0, 100); - return handleErrorCode(e); + private static final int BACK_NUM_LEDS = 39; + private static final int BACK_OFFSET = 48; + private static final int BACK_SLOT = 2; + + public void signalPlayersGreen() { + candle.clearAnimation(BACK_SLOT); + setAreaColor(0, 255, 0, BACK_NUM_LEDS, BACK_OFFSET); } - public boolean isDoingShootingProcedure() { - ErrorCode e = m_candle.setLEDs(0, 227, 197); - return handleErrorCode(e); + public void signalPlayersRed() { + candle.clearAnimation(BACK_SLOT); + setAreaColor(255, 0, 0, BACK_NUM_LEDS, BACK_OFFSET); } - private boolean handleErrorCode(ErrorCode e) { - if (e.equals(ErrorCode.OK)) { - return true; - } + public void signalPlayersNothing() { + candle.clearAnimation(BACK_SLOT); + setAreaColor(0, 0, 0, BACK_NUM_LEDS, BACK_OFFSET); + } + + private void animate(Animation animation, int slot) { + checkContextOwnership(); + candle.animate(animation, slot); + } + + private void clearArea(int count, int offset) { + setAreaColor(0, 0, 0, count, offset); + } - return false; + private void setAreaColor(int r, int g, int b, int count, int offset) { + checkContextOwnership(); + candle.setLEDs(r, g, b, 0, r, count); + } + + public void setBrightness(double value) { + brightness = com.team766.math.Math.clamp(value, 0, 1); + candle.configBrightnessScalar(brightness); + } + + public double getBrightness() { + return brightness; + } + + public void changeBrightness(double change) { + setBrightness(brightness + change); + } + + public void clear() { + setColor(0, 0, 0); + } + + public void setColor(int r, int g, int b) { + for (int i = 0; i < 10; i++) { + candle.clearAnimation(0); + } + checkContextOwnership(); + 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..518d021a1 100644 --- a/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java +++ b/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java @@ -7,7 +7,7 @@ public class IntakeUntilIn extends Procedure { public void run(Context context) { context.takeOwnership(Robot.intake); - Robot.lights.signalNoNoteInIntakeYet(); + // Robot.lights.signalNoNoteInIntakeYet(); while (!Robot.intake.hasNoteInIntake()) { Robot.intake.setIntakePowerForSensorDistance(); context.yield(); @@ -15,10 +15,10 @@ public void run(Context context) { context.releaseOwnership(Robot.intake); - Robot.lights.signalNoteInIntake(); + // Robot.lights.signalNoteInIntake(); - context.waitForSeconds(2); + // context.waitForSeconds(2); - Robot.lights.turnLightsOff(); + // Robot.lights.turnLightsOff(); } } From 63fb258f0206e9f79458bf338f4548f6dbf6674a Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 20 Mar 2024 20:46:15 -0700 Subject: [PATCH 2/5] Integrated lights into intake and shooter --- .../java/com/team766/robot/reva/DriverOI.java | 6 ++++ .../team766/robot/reva/mechanisms/Lights.java | 33 +++++++++++-------- .../robot/reva/procedures/IntakeUntilIn.java | 8 ++--- 3 files changed, 30 insertions(+), 17 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index cb81a2c1e..f07285709 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/src/main/java/com/team766/robot/reva/DriverOI.java @@ -108,6 +108,8 @@ public void handleOI(Context context) { drive.setCross(); context.releaseOwnership(drive); + + Robot.lights.turnOffFront(); } // TODO: update OI with new optimization OI @@ -169,6 +171,10 @@ public void handleOI(Context context) { context.takeOwnership(drive); drive.stopDrive(); } + + if (isRotatingToSpeaker && drive.isAtRotationTarget() && Robot.shoulder.isFinished()) { + Robot.lights.signalCanShoot(); + } } /** 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 41fecdae5..19603c2c1 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Lights.java @@ -32,6 +32,14 @@ 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); + } + private static final Animation CAN_SHOOT_ANIMATION = new RainbowAnimation(1, 0.1, FRONT_NUM_LEDS, false, FRONT_OFFSET); @@ -39,8 +47,8 @@ public void signalCanShoot() { animate(CAN_SHOOT_ANIMATION, FRONT_SLOT); } - public void signalNothingFront() { - clearArea(FRONT_NUM_LEDS, FRONT_OFFSET); + public void turnOffFront() { + clearArea(FRONT_NUM_LEDS, FRONT_OFFSET, FRONT_SLOT); } private static final int BACK_NUM_LEDS = 39; @@ -49,30 +57,29 @@ public void signalNothingFront() { public void signalPlayersGreen() { candle.clearAnimation(BACK_SLOT); - setAreaColor(0, 255, 0, BACK_NUM_LEDS, BACK_OFFSET); + setAreaColor(0, 255, 0, BACK_NUM_LEDS, BACK_OFFSET, BACK_SLOT); } public void signalPlayersRed() { - candle.clearAnimation(BACK_SLOT); - setAreaColor(255, 0, 0, BACK_NUM_LEDS, BACK_OFFSET); + setAreaColor(255, 0, 0, BACK_NUM_LEDS, BACK_OFFSET, BACK_SLOT); } public void signalPlayersNothing() { - candle.clearAnimation(BACK_SLOT); - setAreaColor(0, 0, 0, BACK_NUM_LEDS, BACK_OFFSET); + setAreaColor(0, 0, 0, BACK_NUM_LEDS, BACK_OFFSET, BACK_SLOT); } private void animate(Animation animation, int slot) { - checkContextOwnership(); + // checkContextOwnership(); candle.animate(animation, slot); } - private void clearArea(int count, int offset) { - setAreaColor(0, 0, 0, count, offset); + private void clearArea(int count, int offset, int slot) { + setAreaColor(0, 0, 0, count, offset, slot); } - private void setAreaColor(int r, int g, int b, int count, int offset) { - checkContextOwnership(); + 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); } @@ -97,7 +104,7 @@ public void setColor(int r, int g, int b) { for (int i = 0; i < 10; i++) { candle.clearAnimation(0); } - checkContextOwnership(); + // checkContextOwnership(); 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 518d021a1..70b5c429e 100644 --- a/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java +++ b/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java @@ -7,7 +7,7 @@ public class IntakeUntilIn extends Procedure { public void run(Context context) { context.takeOwnership(Robot.intake); - // Robot.lights.signalNoNoteInIntakeYet(); + Robot.lights.signalNoNoteInIntake(); while (!Robot.intake.hasNoteInIntake()) { Robot.intake.setIntakePowerForSensorDistance(); context.yield(); @@ -15,10 +15,10 @@ public void run(Context context) { context.releaseOwnership(Robot.intake); - // Robot.lights.signalNoteInIntake(); + Robot.lights.signalNoteInIntake(); - // context.waitForSeconds(2); + context.waitForSeconds(2); - // Robot.lights.turnLightsOff(); + Robot.lights.turnOffFront(); } } From f79ba2a359baf319a93b5b7b83c1d298edfff328 Mon Sep 17 00:00:00 2001 From: Jonathan Gassend Date: Fri, 22 Mar 2024 16:23:00 -0700 Subject: [PATCH 3/5] Added context ownersip and updated a few other things --- .../java/com/team766/robot/reva/DriverOI.java | 4 ++ .../mechanisms/ForwardApriltagCamera.java | 6 --- .../team766/robot/reva/mechanisms/Lights.java | 40 ++++++++----------- .../robot/reva/procedures/IntakeUntilIn.java | 12 ++++-- 4 files changed, 29 insertions(+), 33 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index f07285709..7cf68aa45 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/src/main/java/com/team766/robot/reva/DriverOI.java @@ -109,7 +109,9 @@ public void handleOI(Context context) { context.releaseOwnership(drive); + context.takeOwnership(Robot.lights); Robot.lights.turnOffFront(); + context.releaseOwnership(Robot.lights); } // TODO: update OI with new optimization OI @@ -173,7 +175,9 @@ public void handleOI(Context context) { } 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 3d851c356..340b94782 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -28,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 19603c2c1..7023f9c07 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Lights.java @@ -12,6 +12,19 @@ public class Lights extends Mechanism { 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() { this(0.1); } @@ -21,13 +34,6 @@ public Lights(double brightness) { setBrightness(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 Animation TOO_HIGH_ANIMATION = - new StrobeAnimation(255, 0, 0, 0, 0.00001, FRONT_NUM_LEDS, FRONT_OFFSET); - public void signalTooHigh() { animate(TOO_HIGH_ANIMATION, FRONT_SLOT); } @@ -40,9 +46,6 @@ public void signalNoteInIntake() { setAreaColor(0, 255, 0, FRONT_NUM_LEDS, FRONT_OFFSET, FRONT_SLOT); } - private static final Animation CAN_SHOOT_ANIMATION = - new RainbowAnimation(1, 0.1, FRONT_NUM_LEDS, false, FRONT_OFFSET); - public void signalCanShoot() { animate(CAN_SHOOT_ANIMATION, FRONT_SLOT); } @@ -51,10 +54,6 @@ public void turnOffFront() { clearArea(FRONT_NUM_LEDS, FRONT_OFFSET, FRONT_SLOT); } - private static final int BACK_NUM_LEDS = 39; - private static final int BACK_OFFSET = 48; - private static final int BACK_SLOT = 2; - public void signalPlayersGreen() { candle.clearAnimation(BACK_SLOT); setAreaColor(0, 255, 0, BACK_NUM_LEDS, BACK_OFFSET, BACK_SLOT); @@ -69,7 +68,7 @@ public void signalPlayersNothing() { } private void animate(Animation animation, int slot) { - // checkContextOwnership(); + checkContextOwnership(); candle.animate(animation, slot); } @@ -78,12 +77,13 @@ private void clearArea(int count, int offset, int slot) { } private void setAreaColor(int r, int g, int b, int count, int offset, int slot) { - // checkContextOwnership(); + checkContextOwnership(); candle.clearAnimation(slot); candle.setLEDs(r, g, b, 0, r, count); } public void setBrightness(double value) { + checkContextOwnership(); brightness = com.team766.math.Math.clamp(value, 0, 1); candle.configBrightnessScalar(brightness); } @@ -99,12 +99,4 @@ public void changeBrightness(double change) { public void clear() { setColor(0, 0, 0); } - - public void setColor(int r, int g, int b) { - for (int i = 0; i < 10; i++) { - candle.clearAnimation(0); - } - // checkContextOwnership(); - 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 70b5c429e..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,18 +6,24 @@ public class IntakeUntilIn extends Procedure { public void run(Context context) { - context.takeOwnership(Robot.intake); + context.takeOwnership(Robot.lights); Robot.lights.signalNoNoteInIntake(); + context.releaseOwnership(Robot.lights); + + context.takeOwnership(Robot.intake); 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.turnOffFront(); } From 2ace978d7ba6a51c5aa15a6e4bfc2c77f11b5cda Mon Sep 17 00:00:00 2001 From: Jonathan Gassend Date: Fri, 22 Mar 2024 17:08:52 -0700 Subject: [PATCH 4/5] fixed code --- .../java/com/team766/robot/reva/mechanisms/Lights.java | 8 ++++++++ 1 file changed, 8 insertions(+) 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 7023f9c07..33e0e44fe 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Lights.java @@ -32,6 +32,7 @@ public Lights() { public Lights(double brightness) { candle = new CANdle(CANID); setBrightness(brightness); + clear(); } public void signalTooHigh() { @@ -99,4 +100,11 @@ public void changeBrightness(double change) { public void clear() { setColor(0, 0, 0); } + + private void setColor(int r, int g, int b) { + checkContextOwnership(); + candle.clearAnimation(FRONT_SLOT); + candle.clearAnimation(BACK_SLOT); + candle.setLEDs(r, g, b); + } } From b9152be2fa1b23d7df174db82f9d12a70c3a804b Mon Sep 17 00:00:00 2001 From: Jonathan Gassend Date: Fri, 22 Mar 2024 17:54:51 -0700 Subject: [PATCH 5/5] removed ownership check from constructor (not sure if it is necessary) --- .../java/com/team766/robot/reva/mechanisms/Lights.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) 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 33e0e44fe..8d026bea6 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Lights.java @@ -31,7 +31,7 @@ public Lights() { public Lights(double brightness) { candle = new CANdle(CANID); - setBrightness(brightness); + setBrightnessNoCheck(brightness); clear(); } @@ -83,12 +83,16 @@ private void setAreaColor(int r, int g, int b, int count, int offset, int slot) candle.setLEDs(r, g, b, 0, r, count); } - public void setBrightness(double value) { - checkContextOwnership(); + 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; }