From 3dbe69d3f1e8227ea21d36833ece3581e921ee44 Mon Sep 17 00:00:00 2001 From: jowalkup Date: Fri, 31 Mar 2023 16:15:59 -0400 Subject: [PATCH 01/40] made LED abstract class --- .../stuypulse/robot/subsystems/LED/LED.java | 45 +++++++++ .../robot/subsystems/LED/LEDImpl.java | 91 ++++++++++++++++++ .../robot/subsystems/LED/SimLED.java | 93 +++++++++++++++++++ .../robot/subsystems/LEDController.java | 2 +- 4 files changed, 230 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/LED/LED.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/LED/SimLED.java diff --git a/src/main/java/com/stuypulse/robot/subsystems/LED/LED.java b/src/main/java/com/stuypulse/robot/subsystems/LED/LED.java new file mode 100644 index 00000000..4ad9a6d6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/LED/LED.java @@ -0,0 +1,45 @@ +package com.stuypulse.robot.subsystems.LED; + +import com.stuypulse.stuylib.util.StopWatch; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.Robot.MatchState; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.LEDController; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +// idk? +public abstract class LED extends SubsystemBase { + + //public static final LEDController instance; + + static { + if(Robot.isSimulation()){ + final SimLED instance = new SimLED(); + } + else{ + final LEDImpl instance = new LEDImpl(); + } + } + + private void LEDController(){}; + + public void setColor(LEDColor color, double time){}; + + private void forceSetLEDs(LEDColor color){}; + + private void setLEDConditions(){}; + + public LEDColor getDefaultColor(){ + return null;}; + + public void periodic(){}; + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java b/src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java new file mode 100644 index 00000000..48d3ec6a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java @@ -0,0 +1,91 @@ +package com.stuypulse.robot.subsystems.LED; + +import com.stuypulse.stuylib.util.StopWatch; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.Robot.MatchState; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.LEDController; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +public class LEDImpl extends LED{ + private static LEDImpl instance; + + static { + instance = new LEDImpl(); + } + + public static LEDImpl getInstance() { + return instance; + } + + // Motor that controlls the LEDs + private AddressableLED leds; + private AddressableLEDBuffer ledsBuffer; + + // Stopwatch to check when to start overriding manual updates + private StopWatch lastUpdate; + private double manualTime; + + // The current color to set the LEDs to + private LEDColor manualColor; + + protected LEDImpl() { + leds = new AddressableLED(Ports.LEDController.PORT); + ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); // get length of led strip ? + + // set data + leds.setLength(ledsBuffer.getLength()); + leds.setData(ledsBuffer); + leds.start(); + + this.lastUpdate = new StopWatch(); + } + + public void setColor(LEDColor color, double time) { + manualColor = color; + manualTime = time; + lastUpdate.reset(); + } + + private void forceSetLEDs(LEDColor color) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); + } + leds.setData(ledsBuffer); + } + + private void setLEDConditions() { + } + + public LEDColor getDefaultColor() { + switch (Manager.getInstance().getGamePiece()) { + case CUBE: return LEDColor.PURPLE; + case CONE_TIP_IN: return LEDColor.YELLOW; + case CONE_TIP_UP: return LEDColor.GREEN; + case CONE_TIP_OUT: return LEDColor.ORANGE; + default: return LEDColor.OFF; + } + } + + @Override + public void periodic() { + // If we called .setColor() recently, use that value + if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { + forceSetLEDs(manualColor); + } + + // Otherwise use the default color + else { + forceSetLEDs(getDefaultColor()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/LED/SimLED.java b/src/main/java/com/stuypulse/robot/subsystems/LED/SimLED.java new file mode 100644 index 00000000..decca030 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/LED/SimLED.java @@ -0,0 +1,93 @@ +package com.stuypulse.robot.subsystems.LED; + +import com.stuypulse.stuylib.util.StopWatch; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.Robot.MatchState; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.LEDController; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +public class SimLED extends LED{ + private static SimLED simulation; + + static { + simulation = new SimLED(); + } + + public static SimLED getInstance() { + return simulation; + } + + // Motor that controlls the LEDs + private MechanismRoot2d leds; + private MechanismRoot2d ledsBuffer; + + // Stopwatch to check when to start overriding manual updates + private StopWatch lastUpdate; + private double manualTime; + + // The current color to set the LEDs to + private LEDColor manualColor; + + protected SimLED() { + // leds = new MechanismRoot2d("leds", Ports.LEDController.PORT, Ports.LEDController.PORT); + // ledsBuffer = new MechanismRoot2d("ledsBuffer", Settings.LED.LED_LENGTH, Settings.LED.LED_LENGTH); // get length of led strip ? + + // // set data + // leds.setLength(ledsBuffer.getLength()); + // leds.setData(ledsBuffer); + // leds.start(); + + this.lastUpdate = new StopWatch(); + } + + public void setColor(LEDColor color, double time) { + manualColor = color; + manualTime = time; + lastUpdate.reset(); + } + + private void forceSetLEDs(LEDColor color) { + // for (int i = 0; i < ledsBuffer.getLength(); i++) { + // ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); + // } + // leds.setData(ledsBuffer); + } + + private void setLEDConditions() { + } + + public LEDColor getDefaultColor() { + switch (Manager.getInstance().getGamePiece()) { + case CUBE: return LEDColor.PURPLE; + case CONE_TIP_IN: return LEDColor.YELLOW; + case CONE_TIP_UP: return LEDColor.GREEN; + case CONE_TIP_OUT: return LEDColor.ORANGE; + default: return LEDColor.OFF; + } + } + + @Override + public void periodic() { + // If we called .setColor() recently, use that value + if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { + forceSetLEDs(manualColor); + } + + // Otherwise use the default color + else { + forceSetLEDs(getDefaultColor()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/LEDController.java index f666ff3e..cde557b8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/LEDController.java @@ -51,7 +51,7 @@ public static LEDController getInstance() { // The current color to set the LEDs to private LEDColor manualColor; - protected LEDController() { + public LEDController() { leds = new AddressableLED(Ports.LEDController.PORT); ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); // get length of led strip ? From 7ac1bb7ea75360591984ffe1a9f549652ca98dd0 Mon Sep 17 00:00:00 2001 From: jowalkup Date: Fri, 31 Mar 2023 19:16:54 -0400 Subject: [PATCH 02/40] wrote LED commands and rainbow color Co-authored-by: Richie Xue --- .../com/stuypulse/robot/RobotContainer.java | 1 + .../stuypulse/robot/commands/leds/LEDSet.java | 13 ++- .../balance/SwerveDriveBalanceBlay.java | 2 +- .../stuypulse/robot/subsystems/LED/LED.java | 45 --------- .../robot/subsystems/LED/LEDImpl.java | 91 ------------------- .../subsystems/{ => leds}/LEDController.java | 52 +++-------- .../subsystems/leds/LEDControllerImpl.java | 45 +++++++++ .../robot/subsystems/leds/LEDInstruction.java | 11 +++ .../robot/subsystems/leds/LEDRainbow.java | 28 ++++++ .../subsystems/{LED => leds}/SimLED.java | 5 +- .../com/stuypulse/robot/util/LEDColor.java | 27 ++++-- 11 files changed, 134 insertions(+), 186 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/LED/LED.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java rename src/main/java/com/stuypulse/robot/subsystems/{ => leds}/LEDController.java (56%) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java rename src/main/java/com/stuypulse/robot/subsystems/{LED => leds}/SimLED.java (95%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 07325eeb..a95dd1dc 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -20,6 +20,7 @@ import com.stuypulse.robot.subsystems.*; import com.stuypulse.robot.subsystems.arm.*; import com.stuypulse.robot.subsystems.intake.*; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.odometry.*; import com.stuypulse.robot.subsystems.swerve.*; import com.stuypulse.robot.subsystems.vision.*; diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java index f7c21365..4a242149 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java @@ -3,12 +3,19 @@ /* This work is licensed under the terms of the MIT license. */ /****************************************************************/ +/*- + * Contains: + * - commands for LEDs + * @author Richie Xue + * @author Jo Walkup + */ + package com.stuypulse.robot.commands.leds; -import com.stuypulse.robot.subsystems.LEDController; import com.stuypulse.robot.util.LEDColor; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -16,10 +23,10 @@ public class LEDSet extends InstantCommand { private LEDColor color; private double updateTime; - private LEDController controller; + private LEDControllerImpl controller; public LEDSet(LEDColor color, double updateTime) { - this.controller = LEDController.getInstance(); + this.controller = LEDControllerImpl.getInstance(); this.updateTime = updateTime; this.color = color; } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java b/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java index c1743213..e61c3c87 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java @@ -4,7 +4,7 @@ import com.stuypulse.robot.Robot.MatchState; import com.stuypulse.robot.commands.swerve.SwerveDrivePointWheels; import com.stuypulse.robot.constants.Settings.AutoBalance; -import com.stuypulse.robot.subsystems.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.plant.Plant; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/subsystems/LED/LED.java b/src/main/java/com/stuypulse/robot/subsystems/LED/LED.java deleted file mode 100644 index 4ad9a6d6..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/LED/LED.java +++ /dev/null @@ -1,45 +0,0 @@ -package com.stuypulse.robot.subsystems.LED; - -import com.stuypulse.stuylib.util.StopWatch; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.Robot.MatchState; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.LEDController; -import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -// idk? -public abstract class LED extends SubsystemBase { - - //public static final LEDController instance; - - static { - if(Robot.isSimulation()){ - final SimLED instance = new SimLED(); - } - else{ - final LEDImpl instance = new LEDImpl(); - } - } - - private void LEDController(){}; - - public void setColor(LEDColor color, double time){}; - - private void forceSetLEDs(LEDColor color){}; - - private void setLEDConditions(){}; - - public LEDColor getDefaultColor(){ - return null;}; - - public void periodic(){}; - -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java b/src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java deleted file mode 100644 index 48d3ec6a..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/LED/LEDImpl.java +++ /dev/null @@ -1,91 +0,0 @@ -package com.stuypulse.robot.subsystems.LED; - -import com.stuypulse.stuylib.util.StopWatch; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.Robot.MatchState; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.LEDController; -import com.stuypulse.robot.subsystems.Manager; -import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -public class LEDImpl extends LED{ - private static LEDImpl instance; - - static { - instance = new LEDImpl(); - } - - public static LEDImpl getInstance() { - return instance; - } - - // Motor that controlls the LEDs - private AddressableLED leds; - private AddressableLEDBuffer ledsBuffer; - - // Stopwatch to check when to start overriding manual updates - private StopWatch lastUpdate; - private double manualTime; - - // The current color to set the LEDs to - private LEDColor manualColor; - - protected LEDImpl() { - leds = new AddressableLED(Ports.LEDController.PORT); - ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); // get length of led strip ? - - // set data - leds.setLength(ledsBuffer.getLength()); - leds.setData(ledsBuffer); - leds.start(); - - this.lastUpdate = new StopWatch(); - } - - public void setColor(LEDColor color, double time) { - manualColor = color; - manualTime = time; - lastUpdate.reset(); - } - - private void forceSetLEDs(LEDColor color) { - for (int i = 0; i < ledsBuffer.getLength(); i++) { - ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); - } - leds.setData(ledsBuffer); - } - - private void setLEDConditions() { - } - - public LEDColor getDefaultColor() { - switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PURPLE; - case CONE_TIP_IN: return LEDColor.YELLOW; - case CONE_TIP_UP: return LEDColor.GREEN; - case CONE_TIP_OUT: return LEDColor.ORANGE; - default: return LEDColor.OFF; - } - } - - @Override - public void periodic() { - // If we called .setColor() recently, use that value - if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { - forceSetLEDs(manualColor); - } - - // Otherwise use the default color - else { - forceSetLEDs(getDefaultColor()); - } - } -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java similarity index 56% rename from src/main/java/com/stuypulse/robot/subsystems/LEDController.java rename to src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index cde557b8..c9b562e1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -3,22 +3,15 @@ /* This work is licensed under the terms of the MIT license. */ /****************************************************************/ -package com.stuypulse.robot.subsystems; +package com.stuypulse.robot.subsystems.leds; import com.stuypulse.stuylib.util.StopWatch; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.MatchState; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.Manager; import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - /*- * Contains: * - setColor() : sets color of LEDs for short time @@ -26,24 +19,22 @@ * * @author Sam Belliveau * @author Andrew Liu + * @author Richie Xue + * @author Jo Walkup */ -public class LEDController extends SubsystemBase { +public abstract class LEDController extends SubsystemBase { // singleton - private static LEDController instance; - + private static LEDControllerImpl instance; + static { - instance = new LEDController(); + instance = new LEDControllerImpl(); } - public static LEDController getInstance() { + public static LEDControllerImpl getInstance() { return instance; } - // Motor that controlls the LEDs - private AddressableLED leds; - private AddressableLEDBuffer ledsBuffer; - // Stopwatch to check when to start overriding manual updates private final StopWatch lastUpdate; private double manualTime; @@ -52,34 +43,21 @@ public static LEDController getInstance() { private LEDColor manualColor; public LEDController() { - leds = new AddressableLED(Ports.LEDController.PORT); - ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); // get length of led strip ? - - // set data - leds.setLength(ledsBuffer.getLength()); - leds.setData(ledsBuffer); - leds.start(); - this.lastUpdate = new StopWatch(); } - public void setColor(LEDColor color, double time) { + public final void setColor(LEDColor color, double time) { manualColor = color; manualTime = time; lastUpdate.reset(); } - private void forceSetLEDs(LEDColor color) { - for (int i = 0; i < ledsBuffer.getLength(); i++) { - ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); - } - leds.setData(ledsBuffer); - } + abstract protected void forceSetLED(LEDInstruction instruction); - private void setLEDConditions() { + public final void setLEDConditions() { } - public LEDColor getDefaultColor() { + public final LEDColor getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { case CUBE: return LEDColor.PURPLE; case CONE_TIP_IN: return LEDColor.YELLOW; @@ -93,12 +71,12 @@ public LEDColor getDefaultColor() { public void periodic() { // If we called .setColor() recently, use that value if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { - forceSetLEDs(manualColor); + forceSetLED(manualColor); } // Otherwise use the default color else { - forceSetLEDs(getDefaultColor()); + forceSetLED(getDefaultColor()); } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java new file mode 100644 index 00000000..e55270ce --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java @@ -0,0 +1,45 @@ +/************************ PROJECT DORCAS ************************/ +/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved. */ +/* This work is licensed under the terms of the MIT license. */ +/****************************************************************/ + +package com.stuypulse.robot.subsystems.leds; + +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +/*- + * Contains: + * - setColor() : sets color of LEDs for short time + * - getDefaultColor() : determines LED color if it is not set + * +* @author Sam Belliveau + * @author Andrew Liu + * @author Richie Xue + * @author Jo Walkup + */ +public class LEDControllerImpl extends LEDController { + + // Motor that controlls the LEDs + private AddressableLED leds; + private AddressableLEDBuffer ledsBuffer; + + public LEDControllerImpl() { + leds = new AddressableLED(Ports.LEDController.PORT); + ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); // get length of led strip ? + + // set data + leds.setLength(ledsBuffer.getLength()); + leds.setData(ledsBuffer); + leds.start(); + } + + @Override + protected void forceSetLED(LEDInstruction instruction) { + instruction.setLED(ledsBuffer); + leds.setData(ledsBuffer); + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java new file mode 100644 index 00000000..e7f93687 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +// @author Richie Xue +// @author Jo Walkup + +public interface LEDInstruction { + void setLED(AddressableLEDBuffer ledsBuffer); + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java new file mode 100644 index 00000000..36b5ab99 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -0,0 +1,28 @@ +package com.stuypulse.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +/*- + * Contains: + * - setLED() : sets LEDs to rainbow colors + * @author Richie Xue + * @author Jo Walkup + */ +public class LEDRainbow implements LEDInstruction { + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + int m_rainbowFirstPixelHue = 0; + for (int i = 0; i < ledsBuffer.getLength(); ++i) { + // calculate rainbow color + final var hue = (m_rainbowFirstPixelHue + (i * 180 / ledsBuffer.getLength())) % 180; + + // set the i-th led to the rainbow color + ledsBuffer.setHSV(i, hue, 255, 128); + } + // Increase by to make the rainbow "move" + m_rainbowFirstPixelHue += 3; + // Check bounds + m_rainbowFirstPixelHue %= 180; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/LED/SimLED.java b/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java similarity index 95% rename from src/main/java/com/stuypulse/robot/subsystems/LED/SimLED.java rename to src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java index decca030..ad744b96 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/LED/SimLED.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java @@ -1,11 +1,10 @@ -package com.stuypulse.robot.subsystems.LED; +package com.stuypulse.robot.subsystems.leds; import com.stuypulse.stuylib.util.StopWatch; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.MatchState; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.LEDController; import com.stuypulse.robot.subsystems.Manager; import com.stuypulse.robot.util.LEDColor; @@ -18,7 +17,7 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -public class SimLED extends LED{ +public class SimLED extends LEDController{ private static SimLED simulation; static { diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index 77d0e42e..edfa727a 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -6,7 +6,12 @@ package com.stuypulse.robot.util; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.leds.LEDImpl; +import com.stuypulse.robot.subsystems.leds.LEDInstruction; +import com.stuypulse.robot.subsystems.leds.LEDRainbow; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.Timer; /** @@ -16,13 +21,15 @@ * @author Andrew Liu * @author Reya Miller * @author Colyi Chen + * @author Richie Xue + * @author Jo Walkup */ -public class LEDColor { +public class LEDColor implements LEDInstruction{ private final int red; private final int green; private final int blue; - private LEDColor(int red, int green, int blue) { + public LEDColor(int red, int green, int blue) { this.red = red; this.green = green; this.blue = blue; @@ -39,11 +46,18 @@ public int getGreen() { public int getBlue() { return blue; } - - public LEDColor setColor(int red, int green, int blue) { - return new LEDColor(red, green, blue); + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, getRed(), getGreen(), getBlue()); + } } + // new LEDColor(255, 0, 0).setLED(buffer); + // new LEDRainbow().setLED(buffer); + + /***********************/ /*** COLOR CONSTANTS ***/ /***********************/ @@ -74,7 +88,8 @@ public LEDColor setColor(int red, int green, int blue) { public static final LEDColor YELLOW = new LEDColor(255, 255, 0); public static final LEDColor OFF = new LEDColor(0, 0, 0); - public static final LEDColor RAINBOW = OFF; + public static final LEDInstruction RAINBOW = new LEDRainbow(); + } From 9cdacd95f78f75c72e30a2095c790e64085fd021 Mon Sep 17 00:00:00 2001 From: jowalkup Date: Sat, 1 Apr 2023 13:49:53 -0400 Subject: [PATCH 03/40] fixed auton files and made rainbow command Co-authored-by: Richie Xue --- .../robot/commands/auton/OnePieceDock.java | 7 ++++- .../commands/auton/OnePieceMobilityDock.java | 6 +++- .../commands/auton/OnePiecePickupDock.java | 6 +++- .../auton/OnePiecePickupDockWire.java | 6 +++- .../robot/commands/auton/TwoPiece.java | 6 +++- .../robot/commands/auton/TwoPieceWire.java | 6 +++- .../stuypulse/robot/commands/leds/LEDSet.java | 6 ++-- .../robot/commands/leds/LEDSetRainbow.java | 28 +++++++++++++++++++ .../robot/subsystems/leds/LEDController.java | 12 ++++---- .../subsystems/leds/LEDControllerImpl.java | 2 +- .../robot/subsystems/leds/SimLED.java | 9 +++++- .../com/stuypulse/robot/util/LEDColor.java | 2 +- 12 files changed, 79 insertions(+), 17 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java index 10aaa7bd..e36eaafc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java @@ -5,16 +5,19 @@ import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay; import com.stuypulse.robot.subsystems.Manager.*; +import com.stuypulse.robot.subsystems.leds.LEDRainbow; import com.stuypulse.robot.util.ArmState; import com.stuypulse.robot.util.ArmTrajectory; import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -37,6 +40,8 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final double INTAKE_ACQUIRE_TIME = 0.8; private static final double ENGAGE_TIME = 10.0; + private AddressableLEDBuffer ledsBuffer; + private static final PathConstraints DOCK = new PathConstraints(1.8, 2.5); public OnePieceDock() { @@ -76,7 +81,7 @@ public OnePieceDock() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), + new LEDSetRainbow(), new SwerveDriveBalanceBlay() .withMaxSpeed(0.6) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java index 76986058..45200f33 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; @@ -15,6 +16,7 @@ import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -38,6 +40,8 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final double REF_REACTION_TIME = 0.8; private static final double ENGAGE_TIME = 10.0; + private AddressableLEDBuffer ledsBuffer; + private static final PathConstraints OVER_CHARGE = new PathConstraints(1, 2); private static final PathConstraints DOCK = new PathConstraints(1, 2); @@ -90,7 +94,7 @@ public OnePieceMobilityDock() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), + new LEDSetRainbow(), new SwerveDriveBalanceBlay() .withMaxSpeed(0.6) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java index 2503936d..06842fa1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; @@ -15,6 +16,7 @@ import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -30,6 +32,8 @@ public class OnePiecePickupDock extends DebugSequentialCommandGroup { private static final double ACQUIRE_WAIT_TIME = 0.4; private static final double ENGAGE_TIME = 10.0; + private AddressableLEDBuffer ledsBuffer; + private static final PathConstraints INTAKE_PIECE = new PathConstraints(2, 2); private static final PathConstraints DOCK = new PathConstraints(1, 2); @@ -102,7 +106,7 @@ public OnePiecePickupDock() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), + new LEDSetRainbow(), new SwerveDriveBalanceBlay() .withMaxSpeed(0.7) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java index 40a55506..856edb6b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; @@ -15,6 +16,7 @@ import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -31,6 +33,8 @@ public class OnePiecePickupDockWire extends DebugSequentialCommandGroup { private static final double ENGAGE_TIME = 10.0; private static final double ACQUIRE_WAIT_TIME = 0.4; + private AddressableLEDBuffer ledsBuffer; + private static final PathConstraints INTAKE_PIECE = new PathConstraints(3, 2); private static final PathConstraints DOCK = new PathConstraints(1, 2); @@ -103,7 +107,7 @@ public OnePiecePickupDockWire() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), + new LEDSetRainbow(), new SwerveDriveBalanceBlay() .withMaxSpeed(0.5) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java index 171c9baf..b4693272 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.subsystems.Manager.*; @@ -12,6 +13,7 @@ import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -22,6 +24,8 @@ public class TwoPiece extends DebugSequentialCommandGroup { private static final double INTAKE_WAIT_TIME = 2.0; private static final double ACQUIRE_WAIT_TIME = 0.4; + private AddressableLEDBuffer ledsBuffer; + private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(1.7, 2); private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); @@ -106,7 +110,7 @@ public TwoPiece () { ); addCommands( - new LEDSet(LEDColor.RAINBOW), + new LEDSetRainbow(), new SwerveDriveFollowTrajectory( paths.get("Back Away")) .fieldRelative() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java index 2a66c6e2..d5141d81 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.subsystems.Manager.*; @@ -12,6 +13,7 @@ import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -26,6 +28,8 @@ public class TwoPieceWire extends DebugSequentialCommandGroup { private static final double ACQUIRE_WAIT_TIME = 0.4; private static final double READY_WAIT_TIME = 0.5; + private AddressableLEDBuffer ledsBuffer; + private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); @@ -110,7 +114,7 @@ public TwoPieceWire() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), + new LEDSetRainbow(), new SwerveDriveFollowTrajectory( paths.get("Back Away")) .withStop() diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java index 4a242149..96025193 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java @@ -15,6 +15,8 @@ import com.stuypulse.robot.util.LEDColor; import com.stuypulse.robot.constants.Settings; +//import com.stuypulse.robot.constants.Ports.LEDController; +import com.stuypulse.robot.subsystems.leds.*; import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -23,10 +25,10 @@ public class LEDSet extends InstantCommand { private LEDColor color; private double updateTime; - private LEDControllerImpl controller; + private LEDController controller; public LEDSet(LEDColor color, double updateTime) { - this.controller = LEDControllerImpl.getInstance(); + this.controller = LEDController.getInstance(); this.updateTime = updateTime; this.color = color; } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java new file mode 100644 index 00000000..b7de6ea8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java @@ -0,0 +1,28 @@ +package com.stuypulse.robot.commands.leds; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class LEDSetRainbow extends InstantCommand{ + private double updateTime; + private LEDController controller; + + public LEDSetRainbow(double updateTime) { + this.controller = LEDController.getInstance(); + this.updateTime = updateTime; + } + + public LEDSetRainbow() { + this(Settings.LED.MANUAL_UPDATE_TIME); + } + + @Override + public void initialize() { + controller.forceSetLED(LEDColor.RAINBOW); + + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index c9b562e1..45a9373c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -25,13 +25,13 @@ public abstract class LEDController extends SubsystemBase { // singleton - private static LEDControllerImpl instance; + private static LEDController instance; static { instance = new LEDControllerImpl(); } - public static LEDControllerImpl getInstance() { + public static LEDController getInstance() { return instance; } @@ -46,18 +46,18 @@ public LEDController() { this.lastUpdate = new StopWatch(); } - public final void setColor(LEDColor color, double time) { + public void setColor(LEDColor color, double time) { manualColor = color; manualTime = time; lastUpdate.reset(); } - abstract protected void forceSetLED(LEDInstruction instruction); + public abstract void forceSetLED(LEDInstruction instruction); - public final void setLEDConditions() { + public void setLEDConditions() { } - public final LEDColor getDefaultColor() { + public LEDColor getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { case CUBE: return LEDColor.PURPLE; case CONE_TIP_IN: return LEDColor.YELLOW; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java index e55270ce..73579afa 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java @@ -37,7 +37,7 @@ public LEDControllerImpl() { } @Override - protected void forceSetLED(LEDInstruction instruction) { + public void forceSetLED(LEDInstruction instruction) { instruction.setLED(ledsBuffer); leds.setData(ledsBuffer); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java b/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java index ad744b96..d4314549 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java @@ -24,6 +24,7 @@ public class SimLED extends LEDController{ simulation = new SimLED(); } + //@Override public static SimLED getInstance() { return simulation; } @@ -64,7 +65,7 @@ private void forceSetLEDs(LEDColor color) { // leds.setData(ledsBuffer); } - private void setLEDConditions() { + public void setLEDConditions() { } public LEDColor getDefaultColor() { @@ -89,4 +90,10 @@ public void periodic() { forceSetLEDs(getDefaultColor()); } } + + @Override + public void forceSetLED(LEDInstruction instruction) { + // TODO Auto-generated method stub + + } } diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index edfa727a..c909129e 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.util; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.leds.LEDImpl; +import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; import com.stuypulse.robot.subsystems.leds.LEDInstruction; import com.stuypulse.robot.subsystems.leds.LEDRainbow; From e9cc3ffd99592a9d57efcda1df9034cb6153e355 Mon Sep 17 00:00:00 2001 From: jowalkup Date: Sat, 1 Apr 2023 22:52:14 -0400 Subject: [PATCH 04/40] deleted test folder, worked on rainbow --- src/main/java/com/stuypulse/robot/Main.java | 8 +- src/main/java/com/stuypulse/robot/Robot.java | 4 + .../com/stuypulse/robot/RobotContainer.java | 14 +- .../stuypulse/robot/commands/leds/LEDSet.java | 2 + .../robot/commands/leds/LEDSetRainbow.java | 2 + .../com/stuypulse/robot/constants/Ports.java | 4 +- .../stuypulse/robot/constants/Settings.java | 4 +- .../com/stuypulse/robot/subsystems/Pump.java | 2 + .../robot/subsystems/leds/LEDController.java | 2 +- .../robot/subsystems/leds/LEDRainbow.java | 53 ++++- .../robot/subsystems/leds/RichieMode.java | 11 + .../robot/subsystems/swerve/SwerveDrive.java | 9 +- .../com/stuypulse/robot/test/TestArm.java | 195 ------------------ .../com/stuypulse/robot/test/TestIntake.java | 63 ------ .../com/stuypulse/robot/test/TestPlant.java | 42 ---- .../com/stuypulse/robot/test/TestPump.java | 64 ------ .../stuypulse/robot/test/TestSwerveDrive.java | 149 ------------- .../com/stuypulse/robot/test/TestWing.java | 51 ----- .../com/stuypulse/robot/test/Testbot.java | 79 ------- .../robot/test/TestbotContainer.java | 155 -------------- 20 files changed, 88 insertions(+), 825 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java delete mode 100644 src/main/java/com/stuypulse/robot/test/TestArm.java delete mode 100644 src/main/java/com/stuypulse/robot/test/TestIntake.java delete mode 100644 src/main/java/com/stuypulse/robot/test/TestPlant.java delete mode 100644 src/main/java/com/stuypulse/robot/test/TestPump.java delete mode 100644 src/main/java/com/stuypulse/robot/test/TestSwerveDrive.java delete mode 100644 src/main/java/com/stuypulse/robot/test/TestWing.java delete mode 100644 src/main/java/com/stuypulse/robot/test/Testbot.java delete mode 100644 src/main/java/com/stuypulse/robot/test/TestbotContainer.java diff --git a/src/main/java/com/stuypulse/robot/Main.java b/src/main/java/com/stuypulse/robot/Main.java index 0e9a1ff1..a668edc8 100644 --- a/src/main/java/com/stuypulse/robot/Main.java +++ b/src/main/java/com/stuypulse/robot/Main.java @@ -6,8 +6,6 @@ package com.stuypulse.robot; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.test.Testbot; -import com.stuypulse.robot.test.TestbotContainer; import com.stuypulse.stuylib.util.StopWatch; import com.stuypulse.robot.Robot; @@ -19,10 +17,6 @@ private Main() {} public static void main(String... args) { StopWatch.setDefaultEngine(StopWatch.kFPGAEngine); - if (Settings.ROBOT == Settings.Robot.BLAY_MODE) { - RobotBase.startRobot(Testbot::new); - } else { - RobotBase.startRobot(Robot::new); - } + RobotBase.startRobot(Robot::new); } } diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index f38b1e61..6b2e8dba 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -6,6 +6,9 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.TeleopInit; +import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; +import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -47,6 +50,7 @@ public void robotInit() { @Override public void robotPeriodic() { + //new LEDSetRainbow(); scheduler.run(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a95dd1dc..9bfffd3e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.stuypulse.robot.commands.wing.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.subsystems.*; import com.stuypulse.robot.subsystems.arm.*; import com.stuypulse.robot.subsystems.intake.*; @@ -35,7 +36,6 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.booleans.filters.BFilter; -import com.stuypulse.robot.util.BootlegXbox; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.*; @@ -151,7 +151,9 @@ private void configureDriverBindings() { driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90))); // plant - driver.getRightButton().onTrue(new PlantEngage()); + //driver.getRightButton().onTrue(new PlantEngage()); + + driver.getRightButton().whileTrue(new LEDSetRainbow()); driver.getRightBumper().onTrue(new PlantDisengage()); new Trigger(intake::hasCone) @@ -192,9 +194,11 @@ private void configureOperatorBindings() { .andThen(new ManagerValidateState()) .andThen(new ArmReady())); - operator.getRightButton() - .onTrue(new IntakeScore()) - .onFalse(new IntakeStop()); + // operator.getRightButton() + // .onTrue(new IntakeScore()) + // .onFalse(new IntakeStop()); + + operator.getRightButton().onTrue(new LEDSetRainbow()); // set level to score at operator.getDPadDown().onTrue(new ManagerSetNodeLevel(NodeLevel.LOW)); diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java index 96025193..9b3170ba 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java @@ -35,10 +35,12 @@ public LEDSet(LEDColor color, double updateTime) { public LEDSet(LEDColor color) { this(color, Settings.LED.MANUAL_UPDATE_TIME); + //System.out.println("LEDSet was called!!!"); } @Override public void initialize() { + System.out.println("LEDSet was called!!!"); controller.setColor(color, updateTime); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java index b7de6ea8..e7c934f8 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java @@ -18,10 +18,12 @@ public LEDSetRainbow(double updateTime) { public LEDSetRainbow() { this(Settings.LED.MANUAL_UPDATE_TIME); + //System.out.println("RAINBOW SET WAS CALLED!!!! IT BETTER WORK?"); } @Override public void initialize() { + System.out.println("RAINBOW SET WAS CALLED!!!! IT BETTER WORK?"); controller.forceSetLED(LEDColor.RAINBOW); } diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 9a03ae3d..b3d2a579 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -16,8 +16,8 @@ public interface Gamepad { public interface Intake { int FRONT_MOTOR_PORT = 30; int BACK_MOTOR_PORT = 31; - int FRONT_SENSOR = 0; - int BACK_SENSOR = 1; + // int FRONT_SENSOR = 0; + // int BACK_SENSOR = 1; } public interface Swerve { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a8b916c9..abf28100 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -31,13 +31,13 @@ public interface Settings { public enum Robot { JIM, SACROD, - + LED_TEST_BOARD, // runs voltage control project BLAY_MODE } - Robot ROBOT = Robot.JIM; + Robot ROBOT = Robot.LED_TEST_BOARD; double DT = 0.02; diff --git a/src/main/java/com/stuypulse/robot/subsystems/Pump.java b/src/main/java/com/stuypulse/robot/subsystems/Pump.java index e8733cdd..edd21fb1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Pump.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Pump.java @@ -5,6 +5,8 @@ package com.stuypulse.robot.subsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Robot; import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.wpilibj.Compressor; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 45a9373c..a2c39df7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -63,7 +63,7 @@ public LEDColor getDefaultColor() { case CONE_TIP_IN: return LEDColor.YELLOW; case CONE_TIP_UP: return LEDColor.GREEN; case CONE_TIP_OUT: return LEDColor.ORANGE; - default: return LEDColor.OFF; + default: return LEDColor.RED; } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index 36b5ab99..442460da 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -1,6 +1,9 @@ package com.stuypulse.robot.subsystems.leds; +import com.stuypulse.robot.util.LEDColor; + import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.util.Color; /*- * Contains: @@ -12,17 +15,49 @@ public class LEDRainbow implements LEDInstruction { @Override public void setLED(AddressableLEDBuffer ledsBuffer) { - int m_rainbowFirstPixelHue = 0; - for (int i = 0; i < ledsBuffer.getLength(); ++i) { - // calculate rainbow color - final var hue = (m_rainbowFirstPixelHue + (i * 180 / ledsBuffer.getLength())) % 180; - - // set the i-th led to the rainbow color - ledsBuffer.setHSV(i, hue, 255, 128); + LEDColor[] rainbow = new LEDColor[]{LEDColor.RED, LEDColor.RED_ORANGE, LEDColor.ORANGE, LEDColor.YELLOW, LEDColor.LAWN_GREEN, LEDColor.GREEN, LEDColor.DARK_GREEN, LEDColor.BLUE_GREEN, LEDColor.BLUE, LEDColor.DARK_BLUE, LEDColor.BLUE_VIOLET, LEDColor.VIOLET, LEDColor.PURPLE, LEDColor.PINK}; + System.out.println("RAINBOW SETLED WAS CALLED!"); + int j = 0; + int i = 0; + while(i < rainbow.length) { + System.out.println("j (led) = " + j); + System.out.println("i (rainbow) = " + i); + // for (int rgb = 0; rgb < 255; rgb++) { + // if (rgb < 85) { + // ledsBuffer.setRGB(i, rgb * 3, 255 - rgb * 3, 0); + // } + // else if (rgb < 170) { + // ledsBuffer.setRGB(i, 255 - (rgb - 85) * 3, 0, (rgb - 85) * 3); + // } + // else { + // ledsBuffer.setRGB(i, 0, (rgb-170) * 3, 255 - (rgb - 170) * 3); + // } + // } + ledsBuffer.setRGB(j, rainbow[i].getRed(), rainbow[i].getGreen(), rainbow[i].getBlue()); + j++; + i = j / 4; } + // int m_rainbowFirstPixelHue = 0; + // for (int i = 0; i < ledsBuffer.getLength(); ++i) { + // System.out.println("i = " + i); + // // calculate rainbow color + // final var hue = (m_rainbowFirstPixelHue + (i * 180 / ledsBuffer.getLength())) % 180; + // System.out.println("hue = " + hue); + // // set the i-th led to the rainbow color + // ledsBuffer.setHSV(i, hue, 150, 100); + + // } // Increase by to make the rainbow "move" - m_rainbowFirstPixelHue += 3; + // m_rainbowFirstPixelHue += 3; // Check bounds - m_rainbowFirstPixelHue %= 180; + // m_rainbowFirstPixelHue %= 180; + //int m_rainbowFirstPixelHue = 0; + // for(int i = 0; i < 180; i+=10){ + // final var hue = (m_rainbowFirstPixelHue + (i * 180 / ledsBuffer.getLength())) % 180; + + // for(int j = 0; j < ledsBuffer.getLength(); j++){ + // ledsBuffer.setHSV(j, hue , 255, 50); + // } + // } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java new file mode 100644 index 00000000..e74af070 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java @@ -0,0 +1,11 @@ +package com.stuypulse.robot.subsystems.leds; + +public class RichieMode extends LEDController { + + @Override + public void forceSetLED(LEDInstruction instruction) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'forceSetLED'"); + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index d55d5a50..da076bdb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -51,13 +51,20 @@ public class SwerveDrive extends SubsystemBase { new SL_SwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, Ports.Swerve.BackLeft.TURN, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE), new SL_SwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, Ports.Swerve.BackRight.TURN, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE) ); - } else { + } else if (Settings.ROBOT == Robot.SACROD) { instance = new SwerveDrive( SacrodModule.createFrontRight(), SacrodModule.createFrontLeft(), SacrodModule.createBackLeft(), SacrodModule.createBackRight() ); + } else { + instance = new SwerveDrive( + new SimModule(FrontRight.ID, FrontRight.MODULE_OFFSET), + new SimModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET), + new SimModule(BackLeft.ID, BackLeft.MODULE_OFFSET), + new SimModule(BackRight.ID, BackRight.MODULE_OFFSET) + ); } } else { instance = new SwerveDrive( diff --git a/src/main/java/com/stuypulse/robot/test/TestArm.java b/src/main/java/com/stuypulse/robot/test/TestArm.java deleted file mode 100644 index 2a985bbd..00000000 --- a/src/main/java/com/stuypulse/robot/test/TestArm.java +++ /dev/null @@ -1,195 +0,0 @@ -package com.stuypulse.robot.test; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; - -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.ArmDynamics; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.control.angle.feedforward.AngleArmFeedforward; -import com.stuypulse.stuylib.control.feedforward.ArmFeedforward; -import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; -import com.stuypulse.stuylib.network.SmartBoolean; -import com.stuypulse.stuylib.network.SmartNumber; -import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; - -import static com.stuypulse.robot.constants.Motors.Arm.*; -import static com.stuypulse.robot.constants.Ports.Arm.*; -import static com.stuypulse.robot.constants.Settings.Arm.*; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class TestArm extends SubsystemBase { - - private static final SmartBoolean SETPOINT_CONTROL = new SmartBoolean("Arm/Setpoint Control", false); - - private final CANSparkMax shoulderLeft; - private final CANSparkMax shoulderRight; - private final CANSparkMax wrist; - - private final SmartNumber targetShoulderAngle; - private final SmartNumber targetWristAngle; - - private final AngleController shoulderController; - private final AngleController wristController; - - private final AbsoluteEncoder shoulderEncoder; - private final AbsoluteEncoder wristEncoder; - - private final ArmDynamics dynamics; - - public TestArm() { - shoulderLeft = new CANSparkMax(SHOULDER_LEFT, MotorType.kBrushless); - shoulderRight = new CANSparkMax(SHOULDER_RIGHT, MotorType.kBrushless); - wrist = new CANSparkMax(WRIST, MotorType.kBrushless); - - // shoulderController = new AngleArmFeedforward(Shoulder.Feedforward.kG) - // .add(new AnglePIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD)); - - shoulderController = new MotorFeedforward(Shoulder.Feedforward.kS, Shoulder.Feedforward.kV, Shoulder.Feedforward.kA).angle() - .add(new AngleArmFeedforward(Shoulder.Feedforward.kG)) - .add(new AnglePIDController(Shoulder.PID.kP, Shoulder.PID.kI, Shoulder.PID.kD)); - // .setSetpointFilter(new AMotionProfile(Shoulder.VEL_LIMIT, Shoulder.ACCEL_LIMIT)); - - // wristController = new AngleArmFeedforward(Wrist.Feedforward.kG) - // .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)); - - wristController = new MotorFeedforward(Wrist.Feedforward.kS, Wrist.Feedforward.kV, Wrist.Feedforward.kA).angle() - .add(new AngleArmFeedforward(Wrist.Feedforward.kG)) - .add(new AnglePIDController(Wrist.PID.kP, Wrist.PID.kI, Wrist.PID.kD)); - // .setSetpointFilter(new AMotionProfile(Wrist.VEL_LIMIT, Wrist.ACCEL_LIMIT)); - - shoulderEncoder = shoulderRight.getAbsoluteEncoder(Type.kDutyCycle); - wristEncoder = wrist.getAbsoluteEncoder(Type.kDutyCycle); - - targetShoulderAngle = new SmartNumber("Arm/Target Shoulder Angle (deg)", getShoulderAngle().getDegrees()); - targetWristAngle = new SmartNumber("Arm/Target Wrist Angle (deg)", getWristAngle().getDegrees()); - - dynamics = new ArmDynamics(Shoulder.JOINT, Wrist.JOINT); - - configureMotors(); - } - - public void moveShoulder(Rotation2d delta) { - targetShoulderAngle.set(targetShoulderAngle.get() + delta.getDegrees()); - } - - public void moveWrist(Rotation2d delta) { - targetWristAngle.set(targetWristAngle.get() + delta.getDegrees()); - } - - public Rotation2d getShoulderAngle() { - return Rotation2d.fromRotations(shoulderEncoder.getPosition()).minus(Shoulder.ZERO_ANGLE); - } - - public Rotation2d getWristAngle() { - return Rotation2d.fromRotations(wristEncoder.getPosition()).minus(Wrist.ZERO_ANGLE).plus(getShoulderAngle()); - } - - public Rotation2d getShoulderTargetAngle() { - return Rotation2d.fromDegrees(targetShoulderAngle.get()); - } - - public Rotation2d getWristTargetAngle() { - return Rotation2d.fromDegrees(targetWristAngle.get()); - } - - private Rotation2d getRelativeWristTargetAngle() { - return getWristAngle().minus(getShoulderAngle()); - } - - public void runShoulder(double voltage) { - shoulderLeft.setVoltage(voltage); - shoulderRight.setVoltage(voltage); - - SmartDashboard.putNumber("Arm/Shoulder Voltage", voltage); - } - - public void runWrist(double voltage) { - wrist.setVoltage(voltage); - - SmartDashboard.putNumber("Arm/Wrist Voltage", voltage); - } - - public void configureMotors() { - shoulderEncoder.setZeroOffset(0); - wristEncoder.setZeroOffset(0); - - shoulderEncoder.setInverted(true); - shoulderRight.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20); - - wristEncoder.setInverted(true); - wrist.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20); - - SHOULDER_LEFT_CONFIG.configure(shoulderLeft); - SHOULDER_RIGHT_CONFIG.configure(shoulderRight); - WRIST_CONFIG.configure(wrist); - } - - private Rotation2d lastShoulderAngle; - private Rotation2d lastWristAngle; - - private double lastShoulderVelocity = Double.NaN; - private double lastWristVelocity = Double.NaN; - - @Override - public void periodic() { - if (SETPOINT_CONTROL.get()) { - var u_ff = VecBuilder.fill(0, 0); - - if (lastShoulderAngle != null && lastWristAngle != null) { - lastShoulderVelocity = getShoulderTargetAngle().minus(lastShoulderAngle).getRadians() / Settings.DT; - lastWristVelocity = getWristTargetAngle().minus(lastWristAngle).getRadians() / Settings.DT; - } - - if (!Double.isNaN(lastShoulderVelocity) && !Double.isNaN(lastWristVelocity)) { - double currentShoulderVelocity = getShoulderTargetAngle().minus(lastShoulderAngle).getRadians() / Settings.DT; - double currentWristVelocity = getWristTargetAngle().minus(lastWristAngle).getRadians() / Settings.DT; - - u_ff = dynamics.feedforward( - VecBuilder.fill(getShoulderTargetAngle().getRadians(), getRelativeWristTargetAngle().getRadians()), - VecBuilder.fill(currentShoulderVelocity, currentWristVelocity), - VecBuilder.fill( - (currentShoulderVelocity - lastShoulderVelocity)/ Settings.DT, - (currentWristVelocity - lastWristVelocity) / Settings.DT)); - - lastShoulderVelocity = currentShoulderVelocity; - lastWristVelocity = currentWristVelocity; - } - - lastWristAngle = getWristTargetAngle(); - lastShoulderAngle = getShoulderTargetAngle(); - - u_ff = VecBuilder.fill( - MathUtil.clamp(u_ff.get(0, 0), -12, 12), - MathUtil.clamp(u_ff.get(1, 0), -12, 12)); - - double shoulderVolts = - // u_ff.get(0, 0) + - shoulderController.update(Angle.fromDegrees(targetShoulderAngle.get()), Angle.fromRotation2d(getShoulderAngle())); - - double wristVolts = - // u_ff.get(1, 0) + - wristController.update(Angle.fromDegrees(targetWristAngle.get()), Angle.fromRotation2d(getWristAngle())); - - - runShoulder(shoulderVolts); - runWrist(wristVolts); - } - - SmartDashboard.putNumber("Arm/Shoulder Encoder", shoulderEncoder.getPosition()); - SmartDashboard.putNumber("Arm/Wrist Encoder", wristEncoder.getPosition()); - - SmartDashboard.putNumber("Arm/Shoulder Angle", getShoulderAngle().getDegrees()); - SmartDashboard.putNumber("Arm/Wrist Angle", getWristAngle().getDegrees()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/test/TestIntake.java b/src/main/java/com/stuypulse/robot/test/TestIntake.java deleted file mode 100644 index c7db884f..00000000 --- a/src/main/java/com/stuypulse/robot/test/TestIntake.java +++ /dev/null @@ -1,63 +0,0 @@ -package com.stuypulse.robot.test; -import static com.stuypulse.robot.constants.Motors.Intake.*; -import static com.stuypulse.robot.constants.Ports.Intake.*; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.network.SmartNumber; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class TestIntake extends SubsystemBase{ - - private SmartNumber frontDutyCycle; - private SmartNumber backDutyCycle; - - private CANSparkMax frontMotor; - private CANSparkMax backMotor; - - private DigitalInput frontSensor; - private DigitalInput backSensor; - - public TestIntake() { - frontDutyCycle = new SmartNumber("Intake/Front Duty Cycle", 0.0); - backDutyCycle = new SmartNumber("Intake/Back Duty Cycle", 0.0); - - frontMotor = new CANSparkMax(FRONT_MOTOR_PORT, MotorType.kBrushless); - backMotor = new CANSparkMax(BACK_MOTOR_PORT, MotorType.kBrushless); - - FRONT_MOTOR.configure(frontMotor); - BACK_MOTOR.configure(backMotor); - - frontSensor = new DigitalInput(FRONT_SENSOR); - backSensor = new DigitalInput(BACK_SENSOR); - } - - public void runFront() { - frontMotor.set(frontDutyCycle.get()); - } - - public void runBack() { - backMotor.set(backDutyCycle.get()); - } - - public void stop() { - frontMotor.set(0); - backMotor.set(0); - } - - @Override - public void periodic(){ - SmartDashboard.putNumber("Intake/Front Roller Current", frontMotor.getOutputCurrent()); - SmartDashboard.putNumber("Intake/Back Roller Current", backMotor.getOutputCurrent()); - - SmartDashboard.putNumber("Intake/Front Motor", frontMotor.get()); - SmartDashboard.putNumber("Intake/Back Motor", backMotor.get()); - - SmartDashboard.putBoolean("Intake/Front Sensor", frontSensor.get()); - SmartDashboard.putBoolean("Intake/Back Sensor", backSensor.get()); - } - -} diff --git a/src/main/java/com/stuypulse/robot/test/TestPlant.java b/src/main/java/com/stuypulse/robot/test/TestPlant.java deleted file mode 100644 index b9f6cd04..00000000 --- a/src/main/java/com/stuypulse/robot/test/TestPlant.java +++ /dev/null @@ -1,42 +0,0 @@ -package com.stuypulse.robot.test; - -import static com.stuypulse.robot.constants.Ports.Plant.*; - -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/* -* @author Samuel Chen -* @author Carmin Vuong -* @author Jiayu Yan -* @author Tracey Lin -* -*/ -public class TestPlant extends SubsystemBase { - - private final DoubleSolenoid solenoid; - - public TestPlant() { - this.solenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, FORWARD, REVERSE); - disengage(); - } - - public void engage() { - solenoid.set(Value.kReverse); - } - - public void disengage() { - solenoid.set(Value.kForward); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Is Engaged", solenoid.get()==Value.kReverse); - } -} - diff --git a/src/main/java/com/stuypulse/robot/test/TestPump.java b/src/main/java/com/stuypulse/robot/test/TestPump.java deleted file mode 100644 index 7c6b6e62..00000000 --- a/src/main/java/com/stuypulse/robot/test/TestPump.java +++ /dev/null @@ -1,64 +0,0 @@ -/************************ PROJECT DORCAS ************************/ -/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved. */ -/* This work is licensed under the terms of the MIT license. */ -/****************************************************************/ - -package com.stuypulse.robot.test; - -import com.stuypulse.stuylib.network.SmartBoolean; - -import edu.wpi.first.wpilibj.Compressor; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/*- - * Pumps the robot full of air - * - * Contains: - * - network boolean for controlling state of compressor - * - compressor pneumatics module - * - * @author Myles Pasetsky - * @author SE - */ -public class TestPump extends SubsystemBase { - - // Pump control & hardware - private final SmartBoolean enabled; - private final Compressor compressor; - - public TestPump() { - enabled = new SmartBoolean("Pump/Compressor Enabled", true); - compressor = new Compressor(PneumaticsModuleType.CTREPCM); - - // stop(); - } - - public boolean getCompressing() { - return compressor.isEnabled(); - } - - // Start Compressing the Robot - public void compress() { - this.set(true); - } - - // Stop Compressing - public void stop() { - this.set(false); - } - - // Set the compressor to on or off - public void set(boolean compressing) { - enabled.set(compressing); - } - - @Override - public void periodic() { - if (enabled.get()) { - compressor.enableDigital(); - } else { - compressor.disable(); - } - } -} diff --git a/src/main/java/com/stuypulse/robot/test/TestSwerveDrive.java b/src/main/java/com/stuypulse/robot/test/TestSwerveDrive.java deleted file mode 100644 index c0b7c5f8..00000000 --- a/src/main/java/com/stuypulse/robot/test/TestSwerveDrive.java +++ /dev/null @@ -1,149 +0,0 @@ -package com.stuypulse.robot.test; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import static com.stuypulse.robot.constants.Motors.Swerve.*; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAbsoluteEncoder; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Ports.Swerve.*; -import com.stuypulse.robot.constants.Settings.Swerve.Encoder; -import com.stuypulse.stuylib.network.SmartNumber; - -public class TestSwerveDrive extends SubsystemBase { - - private SmartNumber turningVoltage; - private SmartNumber drivingVoltage; - - private CANSparkMax turnMotorBL; - private CANSparkMax turnMotorFR; - private CANSparkMax turnMotorBR; - private CANSparkMax turnMotorFL; - private SparkMaxAbsoluteEncoder absoluteEncoderBL; - private SparkMaxAbsoluteEncoder absoluteEncoderBR; - private SparkMaxAbsoluteEncoder absoluteEncoderFL; - private SparkMaxAbsoluteEncoder absoluteEncoderFR; - - private CANSparkMax driveMotorBL; - private CANSparkMax driveMotorBR; - private CANSparkMax driveMotorFL; - private CANSparkMax driveMotorFR; - private RelativeEncoder driveEncoderBL; - private RelativeEncoder driveEncoderBR; - private RelativeEncoder driveEncoderFL; - private RelativeEncoder driveEncoderFR; - - public TestSwerveDrive() { - - turningVoltage = new SmartNumber("Swerve/Turning Voltage", 0.0); - drivingVoltage = new SmartNumber("Swerve/Drive Voltage", 0.0); - - /** turn motors */ - - turnMotorBL = new CANSparkMax(BackLeft.TURN, MotorType.kBrushless); - turnMotorBR = new CANSparkMax(BackRight.TURN, MotorType.kBrushless); - turnMotorFL = new CANSparkMax(FrontLeft.TURN, MotorType.kBrushless); - turnMotorFR = new CANSparkMax(FrontRight.TURN, MotorType.kBrushless); - - /** drive motors */ - driveMotorBL = new CANSparkMax(BackLeft.DRIVE, MotorType.kBrushless); - driveMotorBR = new CANSparkMax(BackRight.DRIVE, MotorType.kBrushless); - driveMotorFL = new CANSparkMax(FrontLeft.DRIVE, MotorType.kBrushless); - driveMotorFR = new CANSparkMax(FrontRight.DRIVE, MotorType.kBrushless); - - /** turn encoders */ - absoluteEncoderBL = turnMotorBL.getAbsoluteEncoder(Type.kDutyCycle); - absoluteEncoderBR = turnMotorBR.getAbsoluteEncoder(Type.kDutyCycle); - absoluteEncoderFL = turnMotorFL.getAbsoluteEncoder(Type.kDutyCycle); - absoluteEncoderFR = turnMotorFR.getAbsoluteEncoder(Type.kDutyCycle); - - // drive encoders - driveEncoderBL = driveMotorBL.getEncoder(); - driveEncoderBL.setPositionConversionFactor(Encoder.Drive.POSITION_CONVERSION); - driveEncoderBL.setVelocityConversionFactor(Encoder.Drive.VELOCITY_CONVERSION); - - driveEncoderBR = driveMotorBR.getEncoder(); - driveEncoderBR.setPositionConversionFactor(Encoder.Drive.POSITION_CONVERSION); - driveEncoderBR.setVelocityConversionFactor(Encoder.Drive.VELOCITY_CONVERSION); - - driveEncoderFL = driveMotorFL.getEncoder(); - driveEncoderFL.setPositionConversionFactor(Encoder.Drive.POSITION_CONVERSION); - driveEncoderFL.setVelocityConversionFactor(Encoder.Drive.VELOCITY_CONVERSION); - - driveEncoderFR = driveMotorFR.getEncoder(); - driveEncoderFR.setPositionConversionFactor(Encoder.Drive.POSITION_CONVERSION); - driveEncoderFR.setVelocityConversionFactor(Encoder.Drive.VELOCITY_CONVERSION); - - // configure motors - TURN.configure(turnMotorBL); - TURN.configure(turnMotorBR); - TURN.configure(turnMotorFL); - TURN.configure(turnMotorFR); - - DRIVE.configure(driveMotorBL); - DRIVE.configure(driveMotorBR); - DRIVE.configure(driveMotorFL); - DRIVE.configure(driveMotorFR); - } - - public void turnMotorBL() { - turnMotorBL.setVoltage(turningVoltage.get()); - } - - public void turnMotorBR() { - turnMotorBR.setVoltage(turningVoltage.get()); - } - public void turnMotorFL() { - turnMotorFL.setVoltage(turningVoltage.get()); - } - - public void turnMotorFR() { - turnMotorFR.setVoltage(turningVoltage.get()); - } - - public void driveMotorBL() { - driveMotorBL.setVoltage(drivingVoltage.get()); - } - - public void driveMotorBR() { - driveMotorBR.setVoltage(drivingVoltage.get()); - } - - public void driveMotorFL() { - driveMotorFL.setVoltage(drivingVoltage.get()); - } - - public void driveMotorFR() { - driveMotorFR.setVoltage(drivingVoltage.get()); - } - - public void stop() { - turnMotorBL.stopMotor(); - turnMotorBR.stopMotor(); - turnMotorFL.stopMotor(); - turnMotorFR.stopMotor(); - driveMotorBL.stopMotor(); - driveMotorBR.stopMotor(); - driveMotorFL.stopMotor(); - driveMotorFR.stopMotor(); - } - - @Override - public void periodic() { - // log encoder stuff - SmartDashboard.putNumber("Swerve/Turn/Back Left", absoluteEncoderBL.getPosition()); - SmartDashboard.putNumber("Swerve/Turn/Back Right", absoluteEncoderBR.getPosition()); - SmartDashboard.putNumber("Swerve/Turn/Front Left", absoluteEncoderFL.getPosition()); - SmartDashboard.putNumber("Swerve/Turn/Front Right", absoluteEncoderFR.getPosition()); - - SmartDashboard.putNumber("Swerve/Drive/Back Left Rotations", driveEncoderBL.getPosition()); - SmartDashboard.putNumber("Swerve/Drive/Back Right Rotations", driveEncoderBR.getPosition()); - SmartDashboard.putNumber("Swerve/Drive/Front Left Rotations", driveEncoderFL.getPosition()); - SmartDashboard.putNumber("Swerve/Drive/Front Right Rotations", driveEncoderFR.getPosition()); - - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/test/TestWing.java b/src/main/java/com/stuypulse/robot/test/TestWing.java deleted file mode 100644 index 20fd11d0..00000000 --- a/src/main/java/com/stuypulse/robot/test/TestWing.java +++ /dev/null @@ -1,51 +0,0 @@ -package com.stuypulse.robot.test; - - -import static com.stuypulse.robot.constants.Ports.Wings.*; - -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class TestWing extends SubsystemBase { - - // left solenoids - private final DoubleSolenoid deploy; - private final Solenoid latch; - - public TestWing() { - deploy = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, DEPLOY_FORWARD, DEPLOY_REVERSE); - latch = new Solenoid(PneumaticsModuleType.CTREPCM, LATCH); - - latch.set(true); - - deploy.set(DoubleSolenoid.Value.kForward); - } - - public void extendLatch() { - latch.set(true); - } - - public void retractLatch() { - latch.set(false); - } - - public void extendDeploy() { - deploy.set(DoubleSolenoid.Value.kReverse); - } - - public void retractDeploy() { - deploy.set(DoubleSolenoid.Value.kForward); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Wings/Latch Engaged", latch.get()); - SmartDashboard.putBoolean("Wings/Deployed", deploy.get() == DoubleSolenoid.Value.kReverse); - - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/test/Testbot.java b/src/main/java/com/stuypulse/robot/test/Testbot.java deleted file mode 100644 index 6b975d6c..00000000 --- a/src/main/java/com/stuypulse/robot/test/Testbot.java +++ /dev/null @@ -1,79 +0,0 @@ -/************************ PROJECT PHIL ************************/ -/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ -/* This work is licensed under the terms of the MIT license. */ -/**************************************************************/ - -package com.stuypulse.robot.test; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -public class Testbot extends TimedRobot { - - private TestbotContainer robot; - - /*************************/ - /*** ROBOT SCHEDULEING ***/ - /*************************/ - - @Override - public void robotInit() { - robot = new TestbotContainer(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - /*********************/ - /*** DISABLED MODE ***/ - /*********************/ - - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - /***********************/ - /*** AUTONOMOUS MODE ***/ - /***********************/ - - @Override - public void autonomousInit() {} - - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - - /*******************/ - /*** TELEOP MODE ***/ - /*******************/ - - @Override - public void teleopInit() {} - - @Override - public void teleopPeriodic() {} - - @Override - public void teleopExit() {} - - /*****************/ - /*** TEST MODE ***/ - /*****************/ - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() {} - - @Override - public void testExit() {} -} diff --git a/src/main/java/com/stuypulse/robot/test/TestbotContainer.java b/src/main/java/com/stuypulse/robot/test/TestbotContainer.java deleted file mode 100644 index e5016727..00000000 --- a/src/main/java/com/stuypulse/robot/test/TestbotContainer.java +++ /dev/null @@ -1,155 +0,0 @@ -package com.stuypulse.robot.test; - -import com.stuypulse.robot.commands.odometry.OdometryRealign; -import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.Pump; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.util.BootlegXbox; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.math.SLMath; -import com.stuypulse.stuylib.network.SmartBoolean; -import com.stuypulse.stuylib.network.SmartNumber; -import com.stuypulse.stuylib.streams.IStream; -import com.stuypulse.stuylib.streams.filters.LowPassFilter; - -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class TestbotContainer { - - // Gamepads - public final Gamepad driver = new BootlegXbox(Ports.Gamepad.DRIVER); - public final Gamepad operator = new BootlegXbox(Ports.Gamepad.OPERATOR); - - // // Subsystem - public final TestIntake intake = new TestIntake(); - public final TestSwerveDrive swerve = new TestSwerveDrive(); - public final TestArm arm = new TestArm(); - public final TestPlant plant = new TestPlant(); - public final TestWing wings = new TestWing(); - - public final TestPump pump = new TestPump(); - - public TestbotContainer() { - configureDefaultCommands(); - configureButtonBindings(); - - DriverStation.silenceJoystickConnectionWarning(true); - - CameraServer.startAutomaticCapture(); - } - - private void configureButtonBindings() { - driver.getDPadDown().onTrue(new OdometryRealign()); - // swerve - // driver.getDPadLeft() - // .onTrue(swerve.runOnce(swerve::turnMotorFL)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // driver.getDPadRight() - // .onTrue(swerve.runOnce(swerve::turnMotorFR)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // driver.getDPadDown() - // .onTrue(swerve.runOnce(swerve::turnMotorBR)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // driver.getDPadUp() - // .onTrue(swerve.runOnce(swerve::turnMotorBL)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // driver.getLeftButton() - // .onTrue(swerve.runOnce(swerve::driveMotorFL)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // driver.getTopButton() - // .onTrue(swerve.runOnce(swerve::driveMotorFR)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // driver.getRightButton() - // .onTrue(swerve.runOnce(swerve::driveMotorBR)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // driver.getBottomButton() - // .onTrue(swerve.runOnce(swerve::driveMotorBL)) - // .onFalse(swerve.runOnce(swerve::stop)); - - // wings - // operator.getDPadLeft().onTrue(wings.runOnce(wings::extendLeftDeploy)); - // operator.getDPadUp().onTrue(wings.runOnce(wings::retractLeftDeploy)); - // operator.getDPadRight().onTrue(wings.runOnce(wings::extendRightDeploy)); - // operator.getDPadDown().onTrue(wings.runOnce(wings::retractRightDeploy)); - - operator.getLeftButton().onTrue(wings.runOnce(wings::extendLatch)); - operator.getTopButton().onTrue(wings.runOnce(wings::retractLatch)); - - - // plant - operator.getLeftBumper().onTrue(plant.runOnce(plant::disengage)); - operator.getRightBumper().onTrue(plant.runOnce(plant::engage)); - - // intake - // driver.getLeftTriggerButton() - // .onTrue(intake.runOnce(intake::runFront)) - // .onFalse(intake.runOnce(intake::stop)); - - // driver.getRightTriggerButton() - // .onTrue(intake.runOnce(intake::runBack)) - // .onFalse(intake.runOnce(intake::stop)); - } - - private final SmartNumber SHOULDER_VOLTS = new SmartNumber("Arm/Shoulder Input Volts", 0); - private final SmartNumber WRIST_VOLTS = new SmartNumber("Arm/Wrist Input Volts", 0); - private final SmartBoolean ARM_DRIVE = new SmartBoolean("Arm/Arm Drive", false); - - private final IStream shoulder = IStream.create(operator::getLeftY).filtered( - x -> SLMath.deadband(x, Settings.Operator.DEADBAND.get()), - x -> SLMath.spow(x, 2), - x -> x * Settings.Operator.SHOULDER_TELEOP_SPEED.get()); - - private final IStream wrist = IStream.create(operator::getRightY).filtered( - x -> SLMath.deadband(x, Settings.Operator.DEADBAND.get()), - x -> SLMath.spow(x, 2), - x -> x * Settings.Operator.WRIST_TELEOP_SPEED.get()); - - private void configureDefaultCommands() { - - swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - // arm - arm.setDefaultCommand(arm.run(() -> { - if (!SmartDashboard.getBoolean("Arm/Setpoint Control", false)) { - if (ARM_DRIVE.get()) { - double shoulderVolts = MathUtil.applyDeadband(operator.getLeftY(), 0.05) * 3; - double wristVolts = MathUtil.applyDeadband(operator.getRightY(), 0.05) * 12; - - - if (MathUtil.inputModulus(arm.getShoulderAngle().getDegrees(), -180, 180) > 0) { - shoulderVolts = 0; - } - - SmartDashboard.putNumber("Arm/Shoulder Voltage", shoulderVolts); - SmartDashboard.putNumber("Arm/Wrist Voltage", wristVolts); - - arm.runShoulder(shoulderVolts); - arm.runWrist(wristVolts); - } else { - arm.runShoulder(MathUtil.clamp(SHOULDER_VOLTS.get(), -3, 3)); - arm.runWrist(MathUtil.clamp(WRIST_VOLTS.get(), -5, 5)); - } - } else { - if (ARM_DRIVE.get()) { - arm.moveShoulder(Rotation2d.fromDegrees(shoulder.get() * Settings.DT)); - arm.moveWrist(Rotation2d.fromDegrees(wrist.get() * Settings.DT)); - } - } - })); - } - -} From 2ca6b4e8d4a645103914bf9f6ba832063d639085 Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Mon, 3 Apr 2023 16:52:44 -0400 Subject: [PATCH 05/40] LEDRainbow shows LEDS! --- .../java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java | 1 - src/main/java/com/stuypulse/robot/util/LEDColor.java | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java index e7c934f8..a16a4ea0 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java @@ -18,7 +18,6 @@ public LEDSetRainbow(double updateTime) { public LEDSetRainbow() { this(Settings.LED.MANUAL_UPDATE_TIME); - //System.out.println("RAINBOW SET WAS CALLED!!!! IT BETTER WORK?"); } @Override diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index c909129e..ccf3dc22 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -56,6 +56,7 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { // new LEDColor(255, 0, 0).setLED(buffer); // new LEDRainbow().setLED(buffer); + // new PulseRed().setLED(buffer); /***********************/ From 175537f15187b39ea060c8ddcde03455466779a5 Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Mon, 3 Apr 2023 16:52:48 -0400 Subject: [PATCH 06/40] . --- .../robot/subsystems/leds/LEDRainbow.java | 64 ++++++------------- 1 file changed, 20 insertions(+), 44 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index 442460da..46ab8c01 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -3,8 +3,6 @@ import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.util.Color; - /*- * Contains: * - setLED() : sets LEDs to rainbow colors @@ -17,47 +15,25 @@ public class LEDRainbow implements LEDInstruction { public void setLED(AddressableLEDBuffer ledsBuffer) { LEDColor[] rainbow = new LEDColor[]{LEDColor.RED, LEDColor.RED_ORANGE, LEDColor.ORANGE, LEDColor.YELLOW, LEDColor.LAWN_GREEN, LEDColor.GREEN, LEDColor.DARK_GREEN, LEDColor.BLUE_GREEN, LEDColor.BLUE, LEDColor.DARK_BLUE, LEDColor.BLUE_VIOLET, LEDColor.VIOLET, LEDColor.PURPLE, LEDColor.PINK}; System.out.println("RAINBOW SETLED WAS CALLED!"); - int j = 0; - int i = 0; - while(i < rainbow.length) { - System.out.println("j (led) = " + j); - System.out.println("i (rainbow) = " + i); - // for (int rgb = 0; rgb < 255; rgb++) { - // if (rgb < 85) { - // ledsBuffer.setRGB(i, rgb * 3, 255 - rgb * 3, 0); - // } - // else if (rgb < 170) { - // ledsBuffer.setRGB(i, 255 - (rgb - 85) * 3, 0, (rgb - 85) * 3); - // } - // else { - // ledsBuffer.setRGB(i, 0, (rgb-170) * 3, 255 - (rgb - 170) * 3); - // } - // } - ledsBuffer.setRGB(j, rainbow[i].getRed(), rainbow[i].getGreen(), rainbow[i].getBlue()); - j++; - i = j / 4; - } - // int m_rainbowFirstPixelHue = 0; - // for (int i = 0; i < ledsBuffer.getLength(); ++i) { - // System.out.println("i = " + i); - // // calculate rainbow color - // final var hue = (m_rainbowFirstPixelHue + (i * 180 / ledsBuffer.getLength())) % 180; - // System.out.println("hue = " + hue); - // // set the i-th led to the rainbow color - // ledsBuffer.setHSV(i, hue, 150, 100); - - // } - // Increase by to make the rainbow "move" - // m_rainbowFirstPixelHue += 3; - // Check bounds - // m_rainbowFirstPixelHue %= 180; - //int m_rainbowFirstPixelHue = 0; - // for(int i = 0; i < 180; i+=10){ - // final var hue = (m_rainbowFirstPixelHue + (i * 180 / ledsBuffer.getLength())) % 180; - - // for(int j = 0; j < ledsBuffer.getLength(); j++){ - // ledsBuffer.setHSV(j, hue , 255, 50); - // } - // } + LEDController.getInstance().setColor(LEDColor.RED, 0.75); + // int j = 0; + // int i = 0; + // while(i < rainbow.length) { + // System.out.println("j (led) = " + j); + // System.out.println("i (rainbow) = " + i); + // // for (int rgb = 0; rgb < 255; rgb++) { + // // if (rgb < 85) { + // // ledsBuffer.setRGB(i, rgb * 3, 255 - rgb * 3, 0); + // // } + // // else if (rgb < 170) { + // // ledsBuffer.setRGB(i, 255 - (rgb - 85) * 3, 0, (rgb - 85) * 3); + // // } + // // else { + // // ledsBuffer.setRGB(i, 0, (rgb-170) * 3, 255 - (rgb - 170) * 3); + // // } + // // } + // ledsBuffer.setRGB(j, rainbow[i].getRed(), rainbow[i].getGreen(), rainbow[i].getBlue()); + // j++; + // i = j / 4; } } \ No newline at end of file From 9ede47f5360e9a87b6cd2c65800a5f55c038a492 Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Mon, 3 Apr 2023 18:04:01 -0400 Subject: [PATCH 07/40] Add LEDSetPulseRed! (WIP to be used in autons) --- .../robot/commands/leds/LEDSetPulseRed.java | 26 +++++++++++++++++ .../robot/commands/leds/LEDSetRainbow.java | 1 - .../robot/subsystems/leds/LEDPulseRed.java | 29 +++++++++++++++++++ .../com/stuypulse/robot/util/LEDColor.java | 2 ++ 4 files changed, 57 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java new file mode 100644 index 00000000..35bd7d95 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java @@ -0,0 +1,26 @@ +package com.stuypulse.robot.commands.leds; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.leds.LEDController; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class LEDSetPulseRed extends InstantCommand { + private double updateTime; + private LEDController controller; + + public LEDSetPulseRed(double updateTime) { + this.controller = LEDController.getInstance(); + this.updateTime = updateTime; + } + + public LEDSetPulseRed() { + this(Settings.LED.MANUAL_UPDATE_TIME); + } + + @Override + public void initialize() { + controller.forceSetLED(LEDColor.PULSE_RED); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java index a16a4ea0..f1d283d2 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java @@ -2,7 +2,6 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java new file mode 100644 index 00000000..1bba7e88 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.subsystems.leds; + +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +public class LEDPulseRed implements LEDInstruction { + public StopWatch stopwatch; + + public LEDPulseRed() { + stopwatch = new StopWatch(); + } + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + if (stopwatch.getTime() < 0.5) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, 255, 0, 0); + } + } + else { + stopwatch.reset(); + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, 0, 0, 0); + } + } + } +} + diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index ccf3dc22..ed0ea1ed 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; import com.stuypulse.robot.subsystems.leds.LEDInstruction; +import com.stuypulse.robot.subsystems.leds.LEDPulseRed; import com.stuypulse.robot.subsystems.leds.LEDRainbow; import edu.wpi.first.wpilibj.AddressableLED; @@ -91,6 +92,7 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { public static final LEDColor OFF = new LEDColor(0, 0, 0); public static final LEDInstruction RAINBOW = new LEDRainbow(); + public static final LEDInstruction PULSE_RED = new LEDPulseRed(); } From 00a592c7e906249e8dce46f30e5044cc56ba3d76 Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Mon, 3 Apr 2023 19:11:19 -0400 Subject: [PATCH 08/40] Add LEDPulseColor() with any colour, SLColor util --- .../robot/commands/leds/LEDSetPulseRed.java | 2 - .../robot/subsystems/leds/LEDPulseColor.java | 61 +++++++++++++++++++ .../robot/subsystems/leds/LEDPulseRed.java | 29 --------- .../com/stuypulse/robot/util/LEDColor.java | 6 +- .../com/stuypulse/robot/util/SLColor.java | 52 ++++++++++++++++ 5 files changed, 117 insertions(+), 33 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java create mode 100644 src/main/java/com/stuypulse/robot/util/SLColor.java diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java index 35bd7d95..4967d3ae 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java @@ -7,12 +7,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; public class LEDSetPulseRed extends InstantCommand { - private double updateTime; private LEDController controller; public LEDSetPulseRed(double updateTime) { this.controller = LEDController.getInstance(); - this.updateTime = updateTime; } public LEDSetPulseRed() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java new file mode 100644 index 00000000..7d782393 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java @@ -0,0 +1,61 @@ +package com.stuypulse.robot.subsystems.leds; + +import com.stuypulse.robot.util.SLColor; +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +/** + * Class that sets colour of pulsing LEDS on LEDController + * + * @author Richie Xue + * @author Andrew Liu + * @author Reya Miller + * @author Ian Shi + * @author Colyi Chen + * @author Jo Walkup + */ + +public class LEDPulseColor implements LEDInstruction { + public SLColor color; + public SLColor altcolor; + public StopWatch stopwatch; + + public LEDPulseColor(SLColor color) { + this.color = color; + stopwatch = new StopWatch(); + } + + public LEDPulseColor(SLColor color1, SLColor color2) { + this.color = color1; + this.altcolor = color2; + stopwatch = new StopWatch(); + + } + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + if (stopwatch.getTime() < 0.5) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); + } + } + else if (stopwatch.getTime() < 1){ + if (altcolor != null) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, altcolor.getRed(), altcolor.getGreen(), altcolor.getBlue()); + } + } + else { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, 0, 0, 0); + } + } + } + else { + stopwatch.reset(); + } + } + +} + diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java deleted file mode 100644 index 1bba7e88..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseRed.java +++ /dev/null @@ -1,29 +0,0 @@ -package com.stuypulse.robot.subsystems.leds; - -import com.stuypulse.stuylib.util.StopWatch; - -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -public class LEDPulseRed implements LEDInstruction { - public StopWatch stopwatch; - - public LEDPulseRed() { - stopwatch = new StopWatch(); - } - - @Override - public void setLED(AddressableLEDBuffer ledsBuffer) { - if (stopwatch.getTime() < 0.5) { - for (int i = 0; i < ledsBuffer.getLength(); i++) { - ledsBuffer.setRGB(i, 255, 0, 0); - } - } - else { - stopwatch.reset(); - for (int i = 0; i < ledsBuffer.getLength(); i++) { - ledsBuffer.setRGB(i, 0, 0, 0); - } - } - } -} - diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index ed0ea1ed..aa33cddd 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -5,10 +5,11 @@ package com.stuypulse.robot.util; +import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; import com.stuypulse.robot.subsystems.leds.LEDInstruction; -import com.stuypulse.robot.subsystems.leds.LEDPulseRed; +import com.stuypulse.robot.subsystems.leds.LEDPulseColor; import com.stuypulse.robot.subsystems.leds.LEDRainbow; import edu.wpi.first.wpilibj.AddressableLED; @@ -92,7 +93,8 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { public static final LEDColor OFF = new LEDColor(0, 0, 0); public static final LEDInstruction RAINBOW = new LEDRainbow(); - public static final LEDInstruction PULSE_RED = new LEDPulseRed(); + public static final LEDInstruction PULSE_RED = new LEDPulseColor(SLColor.RED); + public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(SLColor.RED, SLColor.BLUE); } diff --git a/src/main/java/com/stuypulse/robot/util/SLColor.java b/src/main/java/com/stuypulse/robot/util/SLColor.java new file mode 100644 index 00000000..6419277f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SLColor.java @@ -0,0 +1,52 @@ +package com.stuypulse.robot.util; + +public class SLColor { + private final int red; + private final int green; + private final int blue; + + public SLColor(int red, int green, int blue) { + this.red = red; + this.green = green; + this.blue = blue; + } + + public int getRed() { + return red; + } + + public int getGreen() { + return green; + } + + public int getBlue() { + return blue; + } + + public static final SLColor AQUA = new SLColor(0, 255, 255); + public static final SLColor BLACK = new SLColor(0, 0, 0); + public static final SLColor BLUE = new SLColor(0, 128, 255); + public static final SLColor BLUE_GREEN = new SLColor(0, 255, 128); + public static final SLColor BLUE_VIOLET = new SLColor(51, 51, 255); + public static final SLColor DARK_BLUE = new SLColor(0, 0, 204); + public static final SLColor DARK_GRAY = new SLColor(64, 64, 64); + public static final SLColor DARK_GREEN = new SLColor(0, 153, 0); + public static final SLColor DARK_RED = new SLColor(204, 0, 0); + public static final SLColor GOLD = new SLColor(218,165,32); + public static final SLColor GRAY = new SLColor(128, 128, 128); + public static final SLColor GREEN = new SLColor(0, 255, 0); + public static final SLColor HOT_PINK = new SLColor(255, 105, 180); + public static final SLColor LAWN_GREEN = new SLColor(102, 204, 0); + public static final SLColor LIME = new SLColor(191, 255, 0); + public static final SLColor ORANGE = new SLColor(255, 128, 0); + public static final SLColor PINK = new SLColor(255, 192, 203); + public static final SLColor PURPLE = new SLColor(160, 32, 240); + public static final SLColor RED = new SLColor(255, 0 , 0); + public static final SLColor RED_ORANGE = new SLColor(255, 83, 73); + public static final SLColor VIOLET = new SLColor(127, 0, 255); + public static final SLColor WHITE = new SLColor(255, 255, 255); + public static final SLColor YELLOW = new SLColor(255, 255, 0); + + public static final SLColor OFF = new SLColor(0, 0, 0); + public static final SLColor RAINBOW = new SLColor(0, 0, 0); +} From 288f6cf595f3c59274157e98f0c2be46b3127c18 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Mon, 3 Apr 2023 19:31:05 -0400 Subject: [PATCH 09/40] stuff --- .../robot/subsystems/leds/LEDController.java | 17 +++---- .../robot/subsystems/leds/LEDInstruction.java | 6 ++- .../robot/subsystems/leds/LEDRainbow.java | 46 ++++++++++--------- .../com/stuypulse/robot/util/LEDColor.java | 2 +- 4 files changed, 39 insertions(+), 32 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index a2c39df7..8898809e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -57,14 +57,15 @@ public void setColor(LEDColor color, double time) { public void setLEDConditions() { } - public LEDColor getDefaultColor() { - switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PURPLE; - case CONE_TIP_IN: return LEDColor.YELLOW; - case CONE_TIP_UP: return LEDColor.GREEN; - case CONE_TIP_OUT: return LEDColor.ORANGE; - default: return LEDColor.RED; - } + public LEDInstruction getDefaultColor() { + // switch (Manager.getInstance().getGamePiece()) { + // case CUBE: return LEDColor.PURPLE; + // case CONE_TIP_IN: return LEDColor.YELLOW; + // case CONE_TIP_UP: return LEDColor.GREEN; + // case CONE_TIP_OUT: return LEDColor.ORANGE; + // default: return LEDColor.RED; + // } + return LEDColor.RAINBOW; } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java index e7f93687..8dc8276e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java @@ -2,8 +2,10 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; -// @author Richie Xue -// @author Jo Walkup +/** + @author Richie Xue + @author Jo Walkup + */ public interface LEDInstruction { void setLED(AddressableLEDBuffer ledsBuffer); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index 46ab8c01..74fe017f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.subsystems.leds; import com.stuypulse.robot.util.LEDColor; +import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; /*- @@ -10,30 +11,33 @@ * @author Jo Walkup */ public class LEDRainbow implements LEDInstruction { + StopWatch timer; + int colorCounter; + boolean firstRun; + + public LEDRainbow() { + timer = new StopWatch(); + colorCounter = 0; + firstRun = true; + } @Override public void setLED(AddressableLEDBuffer ledsBuffer) { - LEDColor[] rainbow = new LEDColor[]{LEDColor.RED, LEDColor.RED_ORANGE, LEDColor.ORANGE, LEDColor.YELLOW, LEDColor.LAWN_GREEN, LEDColor.GREEN, LEDColor.DARK_GREEN, LEDColor.BLUE_GREEN, LEDColor.BLUE, LEDColor.DARK_BLUE, LEDColor.BLUE_VIOLET, LEDColor.VIOLET, LEDColor.PURPLE, LEDColor.PINK}; System.out.println("RAINBOW SETLED WAS CALLED!"); - LEDController.getInstance().setColor(LEDColor.RED, 0.75); - // int j = 0; - // int i = 0; - // while(i < rainbow.length) { - // System.out.println("j (led) = " + j); - // System.out.println("i (rainbow) = " + i); - // // for (int rgb = 0; rgb < 255; rgb++) { - // // if (rgb < 85) { - // // ledsBuffer.setRGB(i, rgb * 3, 255 - rgb * 3, 0); - // // } - // // else if (rgb < 170) { - // // ledsBuffer.setRGB(i, 255 - (rgb - 85) * 3, 0, (rgb - 85) * 3); - // // } - // // else { - // // ledsBuffer.setRGB(i, 0, (rgb-170) * 3, 255 - (rgb - 170) * 3); - // // } - // // } - // ledsBuffer.setRGB(j, rainbow[i].getRed(), rainbow[i].getGreen(), rainbow[i].getBlue()); - // j++; - // i = j / 4; + LEDColor[] rainbow = new LEDColor[]{LEDColor.RED, LEDColor.RED_ORANGE, LEDColor.ORANGE, LEDColor.YELLOW, LEDColor.LAWN_GREEN, LEDColor.GREEN, LEDColor.DARK_GREEN, LEDColor.BLUE_GREEN, LEDColor.BLUE, LEDColor.DARK_BLUE, LEDColor.BLUE_VIOLET, LEDColor.VIOLET, LEDColor.PURPLE, LEDColor.PINK}; + ledsBuffer.setRGB(0, 255, 255, 255); + LEDColor color; + if(firstRun || timer.getTime() > 3 * Math.sin(colorCounter / 2.0)) { + timer.reset(); + System.out.println("changed something"); + firstRun = false; + for(int i = 0; i < ledsBuffer.getLength(); i++) { + //color = rainbow[(i + colorCounter) % rainbow.length]; + //ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); + ledsBuffer.setHSV(i, colorCounter, 255, 128); + colorCounter += 5; + if(colorCounter == 180) colorCounter = 0; + } + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index ccf3dc22..addfca00 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -24,7 +24,7 @@ * @author Richie Xue * @author Jo Walkup */ -public class LEDColor implements LEDInstruction{ +public class LEDColor implements LEDInstruction { private final int red; private final int green; private final int blue; From 0824c6433c5cd24090884830b8f9f376a1d3f271 Mon Sep 17 00:00:00 2001 From: jowalkup Date: Wed, 5 Apr 2023 15:08:56 -0400 Subject: [PATCH 10/40] Made LEDSet take LEDInstruction- pls check this !! --- .../com/stuypulse/robot/commands/leds/LEDSet.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java index 9b3170ba..b68ffa44 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java @@ -26,6 +26,7 @@ public class LEDSet extends InstantCommand { private LEDColor color; private double updateTime; private LEDController controller; + private LEDInstruction instruction; public LEDSet(LEDColor color, double updateTime) { this.controller = LEDController.getInstance(); @@ -33,11 +34,22 @@ public LEDSet(LEDColor color, double updateTime) { this.color = color; } + public LEDSet(LEDInstruction instruction, double updateTime) { + this.controller = LEDController.getInstance(); + this.updateTime = updateTime; + this.instruction = instruction; + } + public LEDSet(LEDColor color) { this(color, Settings.LED.MANUAL_UPDATE_TIME); //System.out.println("LEDSet was called!!!"); } + public LEDSet(LEDInstruction instruction) { + this(instruction, Settings.LED.MANUAL_UPDATE_TIME); + //System.out.println("LEDSet was called!!!"); + } + @Override public void initialize() { System.out.println("LEDSet was called!!!"); From 6f1392b028acdfd909cfbf77c03ec0171a7d0e72 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Wed, 5 Apr 2023 17:47:31 -0400 Subject: [PATCH 11/40] Cleaned up LED + auton code, made Richie mode Co-authored-by: colyic Co-authored-by: Souvik Basak Co-authored-by: Ian Shi Co-authored-by: Kalimul Co-authored-by: Mustafa --- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9bfffd3e..8a39a53e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,4 +1,4 @@ -/************************ PROJECT PHIL ************************/ + /************************ PROJECT PHIL ************************/ /* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ /* This work is licensed under the terms of the MIT license. */ /**************************************************************/ @@ -153,7 +153,7 @@ private void configureDriverBindings() { // plant //driver.getRightButton().onTrue(new PlantEngage()); - driver.getRightButton().whileTrue(new LEDSetRainbow()); + driver.getRightButton().whileTrue(new LEDSet(LEDColor.RAINBOW)); driver.getRightBumper().onTrue(new PlantDisengage()); new Trigger(intake::hasCone) From ceaf554757fb1a23d210eb0368cf439362153151 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Wed, 5 Apr 2023 17:47:55 -0400 Subject: [PATCH 12/40] Cleaned up LED + auton code, made Richie mode Co-authored-by: colyic Co-authored-by: Souvik Basak Co-authored-by: Ian Shi Co-authored-by: Kalimul Co-authored-by: Mustafa --- .../com/stuypulse/robot/RobotContainer.java | 4 +- .../robot/commands/auton/OnePieceDock.java | 11 --- .../commands/auton/OnePieceMobilityDock.java | 13 +--- .../commands/auton/OnePiecePickupDock.java | 12 ---- .../auton/OnePiecePickupDockWire.java | 11 --- .../robot/commands/auton/TwoPiece.java | 12 ---- .../robot/commands/auton/TwoPieceDock.java | 11 --- .../robot/commands/auton/TwoPieceWire.java | 11 --- .../stuypulse/robot/commands/leds/LEDSet.java | 20 +----- .../robot/commands/leds/LEDSetPulseRed.java | 24 ------- .../robot/commands/leds/LEDSetRainbow.java | 28 -------- .../robot/subsystems/leds/LEDController.java | 15 ++--- .../robot/subsystems/leds/LEDRainbow.java | 22 +++--- .../robot/subsystems/leds/RichieMode.java | 43 ++++++++++-- .../com/stuypulse/robot/util/LEDColor.java | 67 ++++++++++--------- .../com/stuypulse/robot/util/SLColor.java | 27 -------- 16 files changed, 94 insertions(+), 237 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9bfffd3e..8a39a53e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,4 +1,4 @@ -/************************ PROJECT PHIL ************************/ + /************************ PROJECT PHIL ************************/ /* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ /* This work is licensed under the terms of the MIT license. */ /**************************************************************/ @@ -153,7 +153,7 @@ private void configureDriverBindings() { // plant //driver.getRightButton().onTrue(new PlantEngage()); - driver.getRightButton().whileTrue(new LEDSetRainbow()); + driver.getRightButton().whileTrue(new LEDSet(LEDColor.RAINBOW)); driver.getRightBumper().onTrue(new PlantDisengage()); new Trigger(intake::hasCone) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java index e36eaafc..613547aa 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java @@ -4,20 +4,15 @@ import com.pathplanner.lib.PathPlanner; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay; import com.stuypulse.robot.subsystems.Manager.*; -import com.stuypulse.robot.subsystems.leds.LEDRainbow; import com.stuypulse.robot.util.ArmState; import com.stuypulse.robot.util.ArmTrajectory; import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -40,8 +35,6 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final double INTAKE_ACQUIRE_TIME = 0.8; private static final double ENGAGE_TIME = 10.0; - private AddressableLEDBuffer ledsBuffer; - private static final PathConstraints DOCK = new PathConstraints(1.8, 2.5); public OnePieceDock() { @@ -55,7 +48,6 @@ public OnePieceDock() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -64,14 +56,12 @@ public OnePieceDock() { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(PathPlanner.loadPath("1 Piece + Dock", DOCK)) .robotRelative().withStop(), @@ -81,7 +71,6 @@ public OnePieceDock() { ); addCommands( - new LEDSetRainbow(), new SwerveDriveBalanceBlay() .withMaxSpeed(0.6) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java index 45200f33..b5c8e5d7 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -4,8 +4,6 @@ import com.pathplanner.lib.PathPlanner; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; @@ -14,9 +12,7 @@ import com.stuypulse.robot.util.ArmState; import com.stuypulse.robot.util.ArmTrajectory; import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -40,8 +36,6 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final double REF_REACTION_TIME = 0.8; private static final double ENGAGE_TIME = 10.0; - private AddressableLEDBuffer ledsBuffer; - private static final PathConstraints OVER_CHARGE = new PathConstraints(1, 2); private static final PathConstraints DOCK = new PathConstraints(1, 2); @@ -60,7 +54,7 @@ public OnePieceMobilityDock() { // score first piece addCommands( - new LEDSet(LEDColor.RED), + new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -69,14 +63,12 @@ public OnePieceMobilityDock() { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); // over charge station addCommands( - new LEDSet(LEDColor.GREEN), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Over Charge")) .robotRelative().withStop(), @@ -87,15 +79,12 @@ public OnePieceMobilityDock() { // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new WaitCommand(REF_REACTION_TIME), new SwerveDriveFollowTrajectory(paths.get("Dock")) .fieldRelative().withStop() ); addCommands( - new LEDSetRainbow(), - new SwerveDriveBalanceBlay() .withMaxSpeed(0.6) .withTimeout(ENGAGE_TIME), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java index 06842fa1..b4300f85 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java @@ -4,8 +4,6 @@ import com.pathplanner.lib.PathPlanner; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; @@ -14,9 +12,7 @@ import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -32,8 +28,6 @@ public class OnePiecePickupDock extends DebugSequentialCommandGroup { private static final double ACQUIRE_WAIT_TIME = 0.4; private static final double ENGAGE_TIME = 10.0; - private AddressableLEDBuffer ledsBuffer; - private static final PathConstraints INTAKE_PIECE = new PathConstraints(2, 2); private static final PathConstraints DOCK = new PathConstraints(1, 2); @@ -54,7 +48,6 @@ public OnePiecePickupDock() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -63,7 +56,6 @@ public OnePiecePickupDock() { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -72,8 +64,6 @@ public OnePiecePickupDock() { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative().withStop(), @@ -96,7 +86,6 @@ public OnePiecePickupDock() { // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Dock")) .fieldRelative().withStop(), @@ -106,7 +95,6 @@ public OnePiecePickupDock() { ); addCommands( - new LEDSetRainbow(), new SwerveDriveBalanceBlay() .withMaxSpeed(0.7) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java index 856edb6b..752f79b8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java @@ -4,8 +4,6 @@ import com.pathplanner.lib.PathPlanner; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; @@ -14,7 +12,6 @@ import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -33,8 +30,6 @@ public class OnePiecePickupDockWire extends DebugSequentialCommandGroup { private static final double ENGAGE_TIME = 10.0; private static final double ACQUIRE_WAIT_TIME = 0.4; - private AddressableLEDBuffer ledsBuffer; - private static final PathConstraints INTAKE_PIECE = new PathConstraints(3, 2); private static final PathConstraints DOCK = new PathConstraints(1, 2); @@ -55,7 +50,6 @@ public OnePiecePickupDockWire() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -64,7 +58,6 @@ public OnePiecePickupDockWire() { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -73,8 +66,6 @@ public OnePiecePickupDockWire() { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative().withStop(), @@ -97,7 +88,6 @@ public OnePiecePickupDockWire() { // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Dock")) .fieldRelative().withStop(), @@ -107,7 +97,6 @@ public OnePiecePickupDockWire() { ); addCommands( - new LEDSetRainbow(), new SwerveDriveBalanceBlay() .withMaxSpeed(0.5) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java index b4693272..6c775f5f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java @@ -4,14 +4,11 @@ import com.pathplanner.lib.PathPlanner; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -24,8 +21,6 @@ public class TwoPiece extends DebugSequentialCommandGroup { private static final double INTAKE_WAIT_TIME = 2.0; private static final double ACQUIRE_WAIT_TIME = 0.4; - private AddressableLEDBuffer ledsBuffer; - private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(1.7, 2); private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); @@ -47,7 +42,6 @@ public TwoPiece () { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -56,7 +50,6 @@ public TwoPiece () { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -65,8 +58,6 @@ public TwoPiece () { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelCommandGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative() @@ -93,8 +84,6 @@ public TwoPiece () { new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetScoreSide(ScoreSide.BACK), - new LEDSet(LEDColor.RED), - new SwerveDriveFollowTrajectory( paths.get("Score Piece")) .fieldRelative() @@ -110,7 +99,6 @@ public TwoPiece () { ); addCommands( - new LEDSetRainbow(), new SwerveDriveFollowTrajectory( paths.get("Back Away")) .fieldRelative() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java index 1b95bc9f..53776e5c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java @@ -4,7 +4,6 @@ import com.pathplanner.lib.PathPlanner; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; @@ -17,7 +16,6 @@ import com.stuypulse.robot.util.ArmState; import com.stuypulse.robot.util.ArmTrajectory; import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; @@ -88,7 +86,6 @@ public TwoPieceDock() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .withTolerance(7, 9) .setShoulderVelocityTolerance(25) @@ -97,7 +94,6 @@ public TwoPieceDock() { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -106,8 +102,6 @@ public TwoPieceDock() { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative().withStop(), @@ -133,8 +127,6 @@ public TwoPieceDock() { new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetScoreSide(ScoreSide.BACK), - new LEDSet(LEDColor.RED), - new ParallelCommandGroup( new SwerveDriveFollowTrajectory( paths.get("Score Piece")) @@ -157,7 +149,6 @@ public TwoPieceDock() { // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Dock")) .fieldRelative().withStop(), @@ -167,8 +158,6 @@ public TwoPieceDock() { ); addCommands( - new LEDSet(LEDColor.GREEN), - new SwerveDriveBalanceBlay() .withMaxSpeed(0.6) .withTimeout(ENGAGE_TIME) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java index d5141d81..63b95263 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java @@ -4,16 +4,12 @@ import com.pathplanner.lib.PathPlanner; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -28,8 +24,6 @@ public class TwoPieceWire extends DebugSequentialCommandGroup { private static final double ACQUIRE_WAIT_TIME = 0.4; private static final double READY_WAIT_TIME = 0.5; - private AddressableLEDBuffer ledsBuffer; - private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); @@ -52,7 +46,6 @@ public TwoPieceWire() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -61,7 +54,6 @@ public TwoPieceWire() { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -70,7 +62,6 @@ public TwoPieceWire() { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) @@ -97,7 +88,6 @@ public TwoPieceWire() { new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetScoreSide(ScoreSide.BACK), - new LEDSet(LEDColor.RED), new SwerveDriveFollowTrajectory( paths.get("Score Piece")) @@ -114,7 +104,6 @@ public TwoPieceWire() { ); addCommands( - new LEDSetRainbow(), new SwerveDriveFollowTrajectory( paths.get("Back Away")) .withStop() diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java index b68ffa44..eb7f3a57 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java @@ -12,47 +12,29 @@ package com.stuypulse.robot.commands.leds; -import com.stuypulse.robot.util.LEDColor; - import com.stuypulse.robot.constants.Settings; //import com.stuypulse.robot.constants.Ports.LEDController; import com.stuypulse.robot.subsystems.leds.*; -import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; - import edu.wpi.first.wpilibj2.command.InstantCommand; public class LEDSet extends InstantCommand { - private LEDColor color; private double updateTime; private LEDController controller; private LEDInstruction instruction; - public LEDSet(LEDColor color, double updateTime) { - this.controller = LEDController.getInstance(); - this.updateTime = updateTime; - this.color = color; - } - public LEDSet(LEDInstruction instruction, double updateTime) { this.controller = LEDController.getInstance(); this.updateTime = updateTime; this.instruction = instruction; } - public LEDSet(LEDColor color) { - this(color, Settings.LED.MANUAL_UPDATE_TIME); - //System.out.println("LEDSet was called!!!"); - } - public LEDSet(LEDInstruction instruction) { this(instruction, Settings.LED.MANUAL_UPDATE_TIME); - //System.out.println("LEDSet was called!!!"); } @Override public void initialize() { - System.out.println("LEDSet was called!!!"); - controller.setColor(color, updateTime); + controller.forceSetLED(instruction); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java deleted file mode 100644 index 4967d3ae..00000000 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java +++ /dev/null @@ -1,24 +0,0 @@ -package com.stuypulse.robot.commands.leds; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class LEDSetPulseRed extends InstantCommand { - private LEDController controller; - - public LEDSetPulseRed(double updateTime) { - this.controller = LEDController.getInstance(); - } - - public LEDSetPulseRed() { - this(Settings.LED.MANUAL_UPDATE_TIME); - } - - @Override - public void initialize() { - controller.forceSetLED(LEDColor.PULSE_RED); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java deleted file mode 100644 index f1d283d2..00000000 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSetRainbow.java +++ /dev/null @@ -1,28 +0,0 @@ -package com.stuypulse.robot.commands.leds; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class LEDSetRainbow extends InstantCommand{ - private double updateTime; - private LEDController controller; - - public LEDSetRainbow(double updateTime) { - this.controller = LEDController.getInstance(); - this.updateTime = updateTime; - } - - public LEDSetRainbow() { - this(Settings.LED.MANUAL_UPDATE_TIME); - } - - @Override - public void initialize() { - System.out.println("RAINBOW SET WAS CALLED!!!! IT BETTER WORK?"); - controller.forceSetLED(LEDColor.RAINBOW); - - } -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 8898809e..da543f96 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -58,14 +58,13 @@ public void setLEDConditions() { } public LEDInstruction getDefaultColor() { - // switch (Manager.getInstance().getGamePiece()) { - // case CUBE: return LEDColor.PURPLE; - // case CONE_TIP_IN: return LEDColor.YELLOW; - // case CONE_TIP_UP: return LEDColor.GREEN; - // case CONE_TIP_OUT: return LEDColor.ORANGE; - // default: return LEDColor.RED; - // } - return LEDColor.RAINBOW; + switch (Manager.getInstance().getGamePiece()) { + case CUBE: return LEDColor.PURPLE; + case CONE_TIP_IN: return LEDColor.YELLOW; + case CONE_TIP_UP: return LEDColor.GREEN; + case CONE_TIP_OUT: return LEDColor.ORANGE; + default: return LEDColor.RED; + } } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index 74fe017f..b26c2181 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -12,31 +12,25 @@ */ public class LEDRainbow implements LEDInstruction { StopWatch timer; - int colorCounter; - boolean firstRun; + int counter; public LEDRainbow() { timer = new StopWatch(); - colorCounter = 0; - firstRun = true; + counter = 0; } @Override public void setLED(AddressableLEDBuffer ledsBuffer) { - System.out.println("RAINBOW SETLED WAS CALLED!"); LEDColor[] rainbow = new LEDColor[]{LEDColor.RED, LEDColor.RED_ORANGE, LEDColor.ORANGE, LEDColor.YELLOW, LEDColor.LAWN_GREEN, LEDColor.GREEN, LEDColor.DARK_GREEN, LEDColor.BLUE_GREEN, LEDColor.BLUE, LEDColor.DARK_BLUE, LEDColor.BLUE_VIOLET, LEDColor.VIOLET, LEDColor.PURPLE, LEDColor.PINK}; - ledsBuffer.setRGB(0, 255, 255, 255); LEDColor color; - if(firstRun || timer.getTime() > 3 * Math.sin(colorCounter / 2.0)) { + // if(timer.getTime() > 3 * Math.sin(colorCounter * 2.0) + 1) { + if(timer.getTime() > 1.0) { timer.reset(); - System.out.println("changed something"); - firstRun = false; for(int i = 0; i < ledsBuffer.getLength(); i++) { - //color = rainbow[(i + colorCounter) % rainbow.length]; - //ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); - ledsBuffer.setHSV(i, colorCounter, 255, 128); - colorCounter += 5; - if(colorCounter == 180) colorCounter = 0; + color = rainbow[(i + counter) % rainbow.length]; + ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); + ++counter; + if(counter == rainbow.length) counter = 0; } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java index e74af070..764885c1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java @@ -1,11 +1,46 @@ package com.stuypulse.robot.subsystems.leds; -public class RichieMode extends LEDController { +import com.stuypulse.robot.util.SLColor; +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +/** + * LED Color that travels through the strip + * + * @author Colyi Chen + * @author Naowal Rahman + * @author Mustafa Abdullah + * @author Kalimul Kaif + * @author Rahel Arka + */ + +public class RichieMode implements LEDInstruction { + public SLColor color; + private StopWatch stopwatch; + private int index; + private int prevIndex; + + public RichieMode(SLColor color) { + this.color = color; + stopwatch = new StopWatch(); + } @Override - public void forceSetLED(LEDInstruction instruction) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'forceSetLED'"); + public void setLED(AddressableLEDBuffer ledsBuffer) { + if(stopwatch.getTime() > 0.25) { + ledsBuffer.setRGB(index, color.getRed(), color.getGreen(), color.getBlue()); + ledsBuffer.setRGB(prevIndex, 0, 0, 0); + prevIndex = index; + ++index; + + if(index == ledsBuffer.getLength()) { + index = 0; + } + + stopwatch.reset(); + } + } } diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index becdc514..efe3a56a 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -25,6 +25,8 @@ * @author Colyi Chen * @author Richie Xue * @author Jo Walkup + * @author Naowal Rahman + * @author Souvik Basak */ public class LEDColor implements LEDInstruction { private final int red; @@ -37,6 +39,10 @@ public LEDColor(int red, int green, int blue) { this.blue = blue; } + public LEDColor(SLColor color) { + this(color.getRed(), color.getGreen(), color.getBlue()); + } + public int getRed() { return red; } @@ -49,6 +55,10 @@ public int getBlue() { return blue; } + public SLColor getSLColor() { + return new SLColor(red, green, blue); + } + @Override public void setLED(AddressableLEDBuffer ledsBuffer) { for (int i = 0; i < ledsBuffer.getLength(); i++) { @@ -56,45 +66,40 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { } } - // new LEDColor(255, 0, 0).setLED(buffer); - // new LEDRainbow().setLED(buffer); - // new PulseRed().setLED(buffer); - /***********************/ /*** COLOR CONSTANTS ***/ /***********************/ + public static final LEDColor AQUA = new LEDColor(new SLColor(0, 255, 255)); + public static final LEDColor BLACK = new LEDColor(new SLColor(0, 0, 0)); + public static final LEDColor BLUE = new LEDColor(new SLColor(0, 128, 255)); + public static final LEDColor BLUE_GREEN = new LEDColor(new SLColor(0, 255, 128)); + public static final LEDColor BLUE_VIOLET = new LEDColor(new SLColor(51, 51, 255)); + public static final LEDColor DARK_BLUE = new LEDColor(new SLColor(0, 0, 204)); + public static final LEDColor DARK_GRAY = new LEDColor(new SLColor(64, 64, 64)); + public static final LEDColor DARK_GREEN = new LEDColor(new SLColor(0, 153, 0)); + public static final LEDColor DARK_RED = new LEDColor(new SLColor(204, 0, 0)); + public static final LEDColor GOLD = new LEDColor(new SLColor(218,165,32)); + public static final LEDColor GRAY = new LEDColor(new SLColor(128, 128, 128)); + public static final LEDColor GREEN = new LEDColor(new SLColor(0, 255, 0)); + public static final LEDColor HOT_PINK = new LEDColor(new SLColor(255, 105, 180)); + public static final LEDColor LAWN_GREEN = new LEDColor(new SLColor(102, 204, 0)); + public static final LEDColor LIME = new LEDColor(new SLColor(191, 255, 0)); + public static final LEDColor ORANGE = new LEDColor(new SLColor(255, 128, 0)); + public static final LEDColor PINK = new LEDColor(new SLColor(255, 192, 203)); + public static final LEDColor PURPLE = new LEDColor(new SLColor(160, 32, 240)); + public static final LEDColor RED = new LEDColor(new SLColor(255, 0 , 0)); + public static final LEDColor RED_ORANGE = new LEDColor(new SLColor(255, 83, 73)); + public static final LEDColor VIOLET = new LEDColor(new SLColor(127, 0, 255)); + public static final LEDColor WHITE = new LEDColor(new SLColor(255, 255, 255)); + public static final LEDColor YELLOW = new LEDColor(new SLColor(255, 255, 0)); - public static final LEDColor AQUA = new LEDColor(0, 255, 255); - public static final LEDColor BLACK = new LEDColor(0, 0, 0); - public static final LEDColor BLUE = new LEDColor(0, 128, 255); - public static final LEDColor BLUE_GREEN = new LEDColor(0, 255, 128); - public static final LEDColor BLUE_VIOLET = new LEDColor(51, 51, 255); - public static final LEDColor DARK_BLUE = new LEDColor(0, 0, 204); - public static final LEDColor DARK_GRAY = new LEDColor(64, 64, 64); - public static final LEDColor DARK_GREEN = new LEDColor(0, 153, 0); - public static final LEDColor DARK_RED = new LEDColor(204, 0, 0); - public static final LEDColor GOLD = new LEDColor(218,165,32); - public static final LEDColor GRAY = new LEDColor(128, 128, 128); - public static final LEDColor GREEN = new LEDColor(0, 255, 0); - public static final LEDColor HOT_PINK = new LEDColor(255, 105, 180); - public static final LEDColor LAWN_GREEN = new LEDColor(102, 204, 0); - public static final LEDColor LIME = new LEDColor(191, 255, 0); - public static final LEDColor ORANGE = new LEDColor(255, 128, 0); - public static final LEDColor PINK = new LEDColor(255, 192, 203); - public static final LEDColor PURPLE = new LEDColor(160, 32, 240); - public static final LEDColor RED = new LEDColor(255, 0 , 0); - public static final LEDColor RED_ORANGE = new LEDColor(255, 83, 73); - public static final LEDColor VIOLET = new LEDColor(127, 0, 255); - public static final LEDColor WHITE = new LEDColor(255, 255, 255); - public static final LEDColor YELLOW = new LEDColor(255, 255, 0); - - public static final LEDColor OFF = new LEDColor(0, 0, 0); + public static final LEDColor OFF = new LEDColor(new SLColor(0, 0, 0)); public static final LEDInstruction RAINBOW = new LEDRainbow(); - public static final LEDInstruction PULSE_RED = new LEDPulseColor(SLColor.RED); - public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(SLColor.RED, SLColor.BLUE); + public static final LEDInstruction PULSE_RED = new LEDPulseColor(RED.getSLColor()); + public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(RED.getSLColor(), BLUE.getSLColor()); } diff --git a/src/main/java/com/stuypulse/robot/util/SLColor.java b/src/main/java/com/stuypulse/robot/util/SLColor.java index 6419277f..1a9242e7 100644 --- a/src/main/java/com/stuypulse/robot/util/SLColor.java +++ b/src/main/java/com/stuypulse/robot/util/SLColor.java @@ -22,31 +22,4 @@ public int getGreen() { public int getBlue() { return blue; } - - public static final SLColor AQUA = new SLColor(0, 255, 255); - public static final SLColor BLACK = new SLColor(0, 0, 0); - public static final SLColor BLUE = new SLColor(0, 128, 255); - public static final SLColor BLUE_GREEN = new SLColor(0, 255, 128); - public static final SLColor BLUE_VIOLET = new SLColor(51, 51, 255); - public static final SLColor DARK_BLUE = new SLColor(0, 0, 204); - public static final SLColor DARK_GRAY = new SLColor(64, 64, 64); - public static final SLColor DARK_GREEN = new SLColor(0, 153, 0); - public static final SLColor DARK_RED = new SLColor(204, 0, 0); - public static final SLColor GOLD = new SLColor(218,165,32); - public static final SLColor GRAY = new SLColor(128, 128, 128); - public static final SLColor GREEN = new SLColor(0, 255, 0); - public static final SLColor HOT_PINK = new SLColor(255, 105, 180); - public static final SLColor LAWN_GREEN = new SLColor(102, 204, 0); - public static final SLColor LIME = new SLColor(191, 255, 0); - public static final SLColor ORANGE = new SLColor(255, 128, 0); - public static final SLColor PINK = new SLColor(255, 192, 203); - public static final SLColor PURPLE = new SLColor(160, 32, 240); - public static final SLColor RED = new SLColor(255, 0 , 0); - public static final SLColor RED_ORANGE = new SLColor(255, 83, 73); - public static final SLColor VIOLET = new SLColor(127, 0, 255); - public static final SLColor WHITE = new SLColor(255, 255, 255); - public static final SLColor YELLOW = new SLColor(255, 255, 0); - - public static final SLColor OFF = new SLColor(0, 0, 0); - public static final SLColor RAINBOW = new SLColor(0, 0, 0); } From 85c028d901f056161e3d6a380e101fe40ba7e85e Mon Sep 17 00:00:00 2001 From: vincentw921 Date: Thu, 6 Apr 2023 10:57:57 -0400 Subject: [PATCH 13/40] - remove unused imports - changed LEDRainbow to use SLColor --- src/main/java/com/stuypulse/robot/Robot.java | 4 ---- .../com/stuypulse/robot/RobotContainer.java | 13 ++++--------- .../robot/subsystems/leds/LEDRainbow.java | 19 ++++++++++++++++--- .../com/stuypulse/robot/util/LEDColor.java | 8 +------- 4 files changed, 21 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 6b2e8dba..af6e0ddb 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -6,10 +6,6 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.TeleopInit; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; -import com.stuypulse.robot.util.LEDColor; - import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8a39a53e..384a9ba4 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -17,7 +17,6 @@ import com.stuypulse.robot.commands.wing.*; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.leds.LEDSetRainbow; import com.stuypulse.robot.subsystems.*; import com.stuypulse.robot.subsystems.arm.*; import com.stuypulse.robot.subsystems.intake.*; @@ -151,9 +150,7 @@ private void configureDriverBindings() { driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90))); // plant - //driver.getRightButton().onTrue(new PlantEngage()); - - driver.getRightButton().whileTrue(new LEDSet(LEDColor.RAINBOW)); + driver.getRightButton().onTrue(new PlantEngage()); driver.getRightBumper().onTrue(new PlantDisengage()); new Trigger(intake::hasCone) @@ -194,11 +191,9 @@ private void configureOperatorBindings() { .andThen(new ManagerValidateState()) .andThen(new ArmReady())); - // operator.getRightButton() - // .onTrue(new IntakeScore()) - // .onFalse(new IntakeStop()); - - operator.getRightButton().onTrue(new LEDSetRainbow()); + operator.getRightButton() + .onTrue(new IntakeScore()) + .onFalse(new IntakeStop()); // set level to score at operator.getDPadDown().onTrue(new ManagerSetNodeLevel(NodeLevel.LOW)); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index b26c2181..6431c07a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -1,6 +1,6 @@ package com.stuypulse.robot.subsystems.leds; -import com.stuypulse.robot.util.LEDColor; +import com.stuypulse.robot.util.SLColor; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -21,8 +21,21 @@ public LEDRainbow() { @Override public void setLED(AddressableLEDBuffer ledsBuffer) { - LEDColor[] rainbow = new LEDColor[]{LEDColor.RED, LEDColor.RED_ORANGE, LEDColor.ORANGE, LEDColor.YELLOW, LEDColor.LAWN_GREEN, LEDColor.GREEN, LEDColor.DARK_GREEN, LEDColor.BLUE_GREEN, LEDColor.BLUE, LEDColor.DARK_BLUE, LEDColor.BLUE_VIOLET, LEDColor.VIOLET, LEDColor.PURPLE, LEDColor.PINK}; - LEDColor color; + SLColor[] rainbow = new SLColor[]{new SLColor (218,165,32), + new SLColor(255, 83, 73), + new SLColor(255, 128, 0), + new SLColor(255, 255, 0), + new SLColor(102, 204, 0), + new SLColor(0, 255, 0), + new SLColor(0, 153, 0), + new SLColor(0, 255, 128), + new SLColor(0, 128, 255), + new SLColor(0, 0, 204), + new SLColor(51, 51, 255), + new SLColor(127, 0, 255), + new SLColor(160, 32, 240), + new SLColor(255, 192, 203)}; + SLColor color; // if(timer.getTime() > 3 * Math.sin(colorCounter * 2.0) + 1) { if(timer.getTime() > 1.0) { timer.reset(); diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index efe3a56a..9913db1d 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -4,18 +4,12 @@ /****************************************************************/ package com.stuypulse.robot.util; - -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.leds.LEDControllerImpl; import com.stuypulse.robot.subsystems.leds.LEDInstruction; import com.stuypulse.robot.subsystems.leds.LEDPulseColor; import com.stuypulse.robot.subsystems.leds.LEDRainbow; -import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.Timer; - + /** * Class that stores all of the different RGB values for the LED Controller. * From 1f6306b0db4d95679ba2c6c60e8f2c52eee8de58 Mon Sep 17 00:00:00 2001 From: vincentw921 Date: Thu, 6 Apr 2023 11:20:32 -0400 Subject: [PATCH 14/40] simplify pulse color logic --- .../robot/subsystems/leds/LEDPulseColor.java | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java index 7d782393..511483ca 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java @@ -22,8 +22,7 @@ public class LEDPulseColor implements LEDInstruction { public StopWatch stopwatch; public LEDPulseColor(SLColor color) { - this.color = color; - stopwatch = new StopWatch(); + this(color, new SLColor(0,0,0)); } public LEDPulseColor(SLColor color1, SLColor color2) { @@ -35,23 +34,20 @@ public LEDPulseColor(SLColor color1, SLColor color2) { @Override public void setLED(AddressableLEDBuffer ledsBuffer) { - if (stopwatch.getTime() < 0.5) { + double time = stopwatch.getTime(); + + if (time < 0.5) { for (int i = 0; i < ledsBuffer.getLength(); i++) { ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); } } - else if (stopwatch.getTime() < 1){ - if (altcolor != null) { - for (int i = 0; i < ledsBuffer.getLength(); i++) { - ledsBuffer.setRGB(i, altcolor.getRed(), altcolor.getGreen(), altcolor.getBlue()); - } - } - else { - for (int i = 0; i < ledsBuffer.getLength(); i++) { - ledsBuffer.setRGB(i, 0, 0, 0); - } + + else if (time < 1){ + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, altcolor.getRed(), altcolor.getGreen(), altcolor.getBlue()); } - } + } + else { stopwatch.reset(); } From 6cd9fd3e14797569bfbb962cf8662f6d6e24a504 Mon Sep 17 00:00:00 2001 From: vincentw921 Date: Thu, 6 Apr 2023 11:20:42 -0400 Subject: [PATCH 15/40] clean rainbow led --- .../java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index 6431c07a..0193d644 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -35,12 +35,12 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { new SLColor(127, 0, 255), new SLColor(160, 32, 240), new SLColor(255, 192, 203)}; - SLColor color; + // if(timer.getTime() > 3 * Math.sin(colorCounter * 2.0) + 1) { if(timer.getTime() > 1.0) { timer.reset(); for(int i = 0; i < ledsBuffer.getLength(); i++) { - color = rainbow[(i + counter) % rainbow.length]; + SLColor color = rainbow[(i + counter) % rainbow.length]; ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); ++counter; if(counter == rainbow.length) counter = 0; From 9aa79ead47c401e261603c118cc8a73faf237edd Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 09:32:51 -0400 Subject: [PATCH 16/40] Add 3 piece with low first piece --- .../deploy/pathplanner/3 Piece W Low.path | 199 +++++++++++ .../com/stuypulse/robot/RobotContainer.java | 1 + .../robot/commands/auton/ThreePieceWLow.java | 321 ++++++++++++++++++ .../robot/constants/ArmTrajectories.java | 4 +- 4 files changed, 523 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/3 Piece W Low.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java diff --git a/src/main/deploy/pathplanner/3 Piece W Low.path b/src/main/deploy/pathplanner/3 Piece W Low.path new file mode 100644 index 00000000..45e1f455 --- /dev/null +++ b/src/main/deploy/pathplanner/3 Piece W Low.path @@ -0,0 +1,199 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.19, + "y": 4.46 + }, + "prevControl": null, + "nextControl": { + "x": 3.5735287054512384, + "y": 4.46 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.187741628152297, + "y": 4.440796639600396 + }, + "prevControl": { + "x": 5.435011671065698, + "y": 4.440796639600396 + }, + "nextControl": { + "x": 6.396944039439681, + "y": 4.440796639600396 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.62, + "y": 4.44 + }, + "prevControl": { + "x": 6.497482928507895, + "y": 4.44 + }, + "nextControl": { + "x": 6.497482928507895, + "y": 4.44 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.26, + "y": 4.6 + }, + "prevControl": { + "x": 3.5298747164225914, + "y": 4.6 + }, + "nextControl": { + "x": 3.5298747164225914, + "y": 4.6 + }, + "holonomicAngle": 6.538707501667168, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.58, + "y": 3.53 + }, + "prevControl": { + "x": 5.084308630155721, + "y": 5.127206493729227 + }, + "nextControl": { + "x": 6.708196571169469, + "y": 3.393102508930692 + }, + "holonomicAngle": -30.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.9, + "y": 3.25 + }, + "prevControl": { + "x": 6.842860523327884, + "y": 3.3115587598696563 + }, + "nextControl": { + "x": 6.842860523327884, + "y": 3.3115587598696563 + }, + "holonomicAngle": -30.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.75, + "y": 4.5 + }, + "prevControl": { + "x": 5.893518753704175, + "y": 5.298494015791451 + }, + "nextControl": { + "x": 5.893518753704175, + "y": 5.298494015791451 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.317578789003732, + "y": 5.355589856478859 + }, + "prevControl": { + "x": 5.392626169867724, + "y": 5.21855983882908 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 95534570..5de4ec6c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -237,6 +237,7 @@ public void configureAutons() { autonChooser.addOption("One Piece Dock", new OnePieceDock()); autonChooser.addOption("One Piece Mobility Dock", new OnePieceMobilityDock()); autonChooser.setDefaultOption("Three Piece", new ThreePiece()); + autonChooser.setDefaultOption("Three Piece W Low", new ThreePieceWLow()); autonChooser.addOption("Three Piece Wire", new ThreePieceWire()); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java new file mode 100644 index 00000000..757d9789 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -0,0 +1,321 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.auton; + +import com.stuypulse.robot.commands.arm.routines.*; +import com.stuypulse.robot.commands.intake.*; +import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.manager.*; +import com.stuypulse.robot.commands.swerve.*; +import com.stuypulse.robot.constants.ArmTrajectories; +import com.stuypulse.robot.constants.ArmTrajectories.Ready; +import com.stuypulse.robot.constants.Settings.Arm.Wrist; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.subsystems.Manager.*; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.util.ArmState; +import com.stuypulse.robot.util.ArmTrajectory; +import com.stuypulse.robot.util.DebugSequentialCommandGroup; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; + +public class ThreePieceWLow extends DebugSequentialCommandGroup { + + static class ConeAutonReady extends ArmRoutine { + public ConeAutonReady() { + super(Manager.getInstance()::getReadyTrajectory); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + + return new ArmTrajectory() + .addState(new ArmState(dest.getShoulderState(), src.getWristState()) + .setShoulderTolerance(20).setWristLimp(true).setWristTolerance(360)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()).setWristTolerance(7) + .setShoulderTolerance(15)); + } + } + + static class AutonMidCubeReady extends ArmRoutine { + public AutonMidCubeReady() { + super(() -> Ready.Mid.kAutonCubeBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(7).setShoulderTolerance(20)); + } + } + + static class AutonHighCubeReady extends ArmRoutine { + public AutonHighCubeReady() { + super(() -> Ready.High.kCubeAutonBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(30).setShoulderTolerance(20)); + } + } + + static class ArmIntakeFirst extends ArmRoutine { + public ArmIntakeFirst() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 11); + // 8.37); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + // .addState( + // new ArmState(intermediateShoulderDegrees, wristSafeAngle) + // .setShoulderTolerance(15) + // .setWristTolerance(360)) + + // .addState( + // new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + // .setWristTolerance(360)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + static class ArmIntakeSecond extends ArmRoutine { + public ArmIntakeSecond() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 10); + // 8.37); + double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + .addState( + new ArmState(intermediateShoulderDegrees, wristSafeAngle) + .setShoulderTolerance(15) + .setWristTolerance(15)) + + .addState( + new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + .setWristTolerance(20)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + + private static final double INTAKE_DEACQUIRE_TIME = 0.3; + private static final double INTAKE_STOP_WAIT_TIME = 1; + private static final double INTAKE_WAIT_TIME = 1.0; + private static final double ACQUIRE_WAIT_TIME = 0.02; + private static final double WIGGLE_PERIOD = 0.6; + private static final double WIGGLE_VEL_AMPLITUDE = 0.6; + + private static final PathConstraints INTAKE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.5, 2); + private static final PathConstraints INTAKE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(2.2, 1.2); + + private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.5, 3.5); + private static final PathConstraints THIRD_SCORE_PIECE_CONSTRAINTS = new PathConstraints(3, 2); + + private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); + + public ThreePieceWLow() { + + var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( + PathPlanner.loadPathGroup("3 Piece W Low", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" + ); + + var arm = Arm.getInstance(); + + // initial setup + addCommands( + new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), + new ManagerSetNodeLevel(NodeLevel.LOW), + new ManagerSetGamePiece(GamePiece.CONE_TIP_IN), + new ManagerSetScoreSide(ScoreSide.BACK) + ); + + // score first piece + addCommands( + new LEDSet(LEDColor.BLUE), + // new ConeAutonReady() + // .withTimeout(1.5) + new IntakeScore(), + new WaitCommand(INTAKE_DEACQUIRE_TIME) + ); + + // intake second piece + addCommands( + + new LEDSet(LEDColor.GREEN), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) + .robotRelative() + .withStop(), + // .until(Intake.getInstance()::hasGamePiece), // interrupting IntakeScore? idk one time the intake just stopped early + + new IntakeScore() + .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) + .andThen(new IntakeStop()) + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) + .andThen(new IntakeAcquire()), + + new ArmIntakeFirst() + .withTolerance(4, 10) + ), + + new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) + .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))), + + // new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) + // .until(Intake.getInstance()::hasGamePiece) + // .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) + // .withTimeout(ACQUIRE_WAIT_TIME), + + arm.runOnce(() -> arm.setWristVoltage(0)) + ); + + // drive to grid and score second piece :: TODO: make custom arm setpoint for this + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + new ManagerSetScoreSide(ScoreSide.BACK), + + new LEDSet(LEDColor.RED), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new AutonMidCubeReady(), + new WaitCommand(0.5).andThen(new IntakeStop()) + ), + + new ManagerSetGridNode(1), + // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), + new IntakeDeacquire(), + new WaitCommand(INTAKE_DEACQUIRE_TIME), + new IntakeStop() + ); + + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + + // intake third piece + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + + new LEDSet(LEDColor.GREEN), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory(paths.get("Intake Third Piece")) + .fieldRelative() + .withStop() + .until(Intake.getInstance()::hasGamePiece), + + new WaitCommand(INTAKE_STOP_WAIT_TIME) + .andThen(new IntakeStop()) + .andThen(new WaitCommand(INTAKE_WAIT_TIME)) + .andThen(new IntakeAcquire()), + + new ArmIntakeSecond() + // .setShoulderVelocityTolerance(10) + // .withTolerance(4, 10) + ), + + new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) + .until(Intake.getInstance()::hasGamePiece) + .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) + .withTimeout(ACQUIRE_WAIT_TIME), + + arm.runOnce(() -> arm.setWristVoltage(0)) + ); + + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(5); + arm.setShoulderVelocityFeedbackDebounce(0.2); + }) + ); + + // drive to grid and score third piece + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + new ManagerSetNodeLevel(NodeLevel.HIGH), + new ManagerSetScoreSide(ScoreSide.BACK), + + new LEDSet(LEDColor.RED), + + new SwerveDriveFollowTrajectory( + paths.get("Score Third Piece")) + .fieldRelative() + .withStop() + .alongWith(new AutonHighCubeReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), + + new ManagerSetGridNode(1), + // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), + new IntakeDeacquire(), + new WaitCommand(INTAKE_DEACQUIRE_TIME), + new IntakeStop() + ); + + // back away + addCommands( + new LEDSet(LEDColor.RAINBOW), + + new SwerveDriveFollowTrajectory( + paths.get("Back Away")) + .fieldRelative() + .withStop() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 051f1664..6d194932 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -77,7 +77,7 @@ public interface Mid { ArmState kAutonCubeBack = new ArmState( new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Shoulder", -165), - new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133)); + new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133 + 10)); ArmState kCubeFront = new ArmState( new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -26), @@ -99,7 +99,7 @@ public interface High { new SmartNumber("Arm Trajectories/Ready High Tip Up Back Wrist", 136)); ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", 3), + new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", -2), new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 50)); ArmState kCubeFront = new ArmState( From 760d05cfcc3b6c57f1cfc0bdfd31a700e6558e3c Mon Sep 17 00:00:00 2001 From: BenG49 Date: Mon, 10 Apr 2023 09:57:02 -0400 Subject: [PATCH 17/40] Remove NYC function, add theoretical y values --- .../com/stuypulse/robot/constants/Field.java | 64 ++++++++++++------- 1 file changed, 42 insertions(+), 22 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index c86e20e0..5e7bbdd5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -84,10 +84,6 @@ public static Number[] getYPoseArray(Alliance alliance, ScoreSide side) { return alliance == Alliance.Red ? Back.RED_Y_POSES : Back.BLUE_Y_POSES; } - public static Number redToBlueNYC(Number yPose) { - return IStream.create(() -> yPose.doubleValue() - Units.inchesToMeters(3.0)).number(); - } - public interface Back { SmartNumber ONE = new SmartNumber("Alignment/Y Poses/Red 1", 7.4376); SmartNumber TWO = new SmartNumber("Alignment/Y Poses/Red 2", 6.905); @@ -99,6 +95,18 @@ public interface Back { SmartNumber EIGHT = new SmartNumber("Alignment/Y Poses/Red 8", 3.557); SmartNumber NINE = new SmartNumber("Alignment/Y Poses/Red 9", 2.89); + /* + * Theoretical values: + * 295.07in | 7.494778m + * 273.07in | 6.935978m + * 251.07in | 6.377178m + * 230.07in | 5.843778m + * 207.07in | 5.259578m + * 185.07in | 4.700778m + * 163.07in | 4.141978m + * 141.07in | 3.583178m + * 119.07in | 3.024378m + */ Number RED_Y_POSES[] = { Back.ONE, Back.TWO, @@ -111,16 +119,28 @@ public interface Back { Back.NINE }; + /* + * Theoretical values: + * 196in | 4.9784m + * 174in | 4.4196m + * 152in | 3.8608m + * 130in | 3.3020m + * 108in | 2.7432m + * 86in | 2.1844m + * 64in | 1.6256m + * 42in | 1.0668m + * 20in | 0.5080m + */ Number BLUE_Y_POSES[] = { - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.NINE)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.EIGHT)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.SEVEN)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.SIX)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.FIVE)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.FOUR)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.THREE)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.TWO)), - redToBlueNYC(AllianceUtil.getMirroredYPose(Back.ONE)) + AllianceUtil.getMirroredYPose(Back.NINE), + AllianceUtil.getMirroredYPose(Back.EIGHT), + AllianceUtil.getMirroredYPose(Back.SEVEN), + AllianceUtil.getMirroredYPose(Back.SIX), + AllianceUtil.getMirroredYPose(Back.FIVE), + AllianceUtil.getMirroredYPose(Back.FOUR), + AllianceUtil.getMirroredYPose(Back.THREE), + AllianceUtil.getMirroredYPose(Back.TWO), + AllianceUtil.getMirroredYPose(Back.ONE) }; } @@ -142,15 +162,15 @@ private static Number backToFront(Number backYPose) { }; Number BLUE_Y_POSES[] = { - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.NINE))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.EIGHT))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.SEVEN))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.SIX))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.FIVE))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.FOUR))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.THREE))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.TWO))), - redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.ONE))) + AllianceUtil.getMirroredYPose(backToFront(Back.NINE)), + AllianceUtil.getMirroredYPose(backToFront(Back.EIGHT)), + AllianceUtil.getMirroredYPose(backToFront(Back.SEVEN)), + AllianceUtil.getMirroredYPose(backToFront(Back.SIX)), + AllianceUtil.getMirroredYPose(backToFront(Back.FIVE)), + AllianceUtil.getMirroredYPose(backToFront(Back.FOUR)), + AllianceUtil.getMirroredYPose(backToFront(Back.THREE)), + AllianceUtil.getMirroredYPose(backToFront(Back.TWO)), + AllianceUtil.getMirroredYPose(backToFront(Back.ONE)) }; } } From aa0ca1c9ca88c25249a2486f45bd507c8afafed4 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Mon, 10 Apr 2023 10:02:30 -0400 Subject: [PATCH 18/40] Revert X alignment poses to pre-NYC values --- src/main/java/com/stuypulse/robot/constants/Field.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 5e7bbdd5..b58e13d6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -55,16 +55,16 @@ public static Pose2d getAprilTagFromId(int id) { public interface ScoreXPoses { public interface High { SmartNumber CUBE_BACK = new SmartNumber("Alignment/X Poses/High/Cube Back", 1.98); - SmartNumber CUBE_FRONT = new SmartNumber("Alignment/X Poses/High/Cube Front", 1.830060); - SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/High/Cone Tip In", 1.894 - 0.075); - SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/High/Cone Tip Out", 1.82 - 0.05); + SmartNumber CUBE_FR`ONT = new SmartNumber("Alignment/X Poses/High/Cube Front", 1.830060); + SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/High/Cone Tip In", 1.894); + SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/High/Cone Tip Out", 1.82); } public interface Mid { SmartNumber CUBE_BACK = new SmartNumber("Alignment/X Poses/Mid/Cube Back", 1.868); SmartNumber CUBE_FRONT = new SmartNumber("Alignment/X Poses/Mid/Cube Front", 2.083577); - SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/Mid/Cone Tip In", 2.275 - 0.075); - SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/Mid/Cone Tip Out", 2.1433 - 0.1); + SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/Mid/Cone Tip In", 2.275); + SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/Mid/Cone Tip Out", 2.1433); } public interface Low { From 18e62bffb162d0e2fea208f8fb88ebcf4941143d Mon Sep 17 00:00:00 2001 From: BenG49 Date: Mon, 10 Apr 2023 10:03:26 -0400 Subject: [PATCH 19/40] Fix typo --- src/main/java/com/stuypulse/robot/constants/Field.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index b58e13d6..3add2da9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -55,7 +55,7 @@ public static Pose2d getAprilTagFromId(int id) { public interface ScoreXPoses { public interface High { SmartNumber CUBE_BACK = new SmartNumber("Alignment/X Poses/High/Cube Back", 1.98); - SmartNumber CUBE_FR`ONT = new SmartNumber("Alignment/X Poses/High/Cube Front", 1.830060); + SmartNumber CUBE_FRONT = new SmartNumber("Alignment/X Poses/High/Cube Front", 1.830060); SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/High/Cone Tip In", 1.894); SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/High/Cone Tip Out", 1.82); } From 9bc4e6cf3397ba57bb6b5bb4e75d1958d3aaaed4 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 10:48:38 -0400 Subject: [PATCH 20/40] Three piece W Low changes --- .../robot/commands/auton/ThreePieceWLow.java | 35 ++++++++----------- 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 757d9789..1822b068 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -23,6 +23,7 @@ import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -178,18 +179,8 @@ public ThreePieceWLow() { new ManagerSetScoreSide(ScoreSide.BACK) ); - // score first piece + // score first piece + intake second piece addCommands( - new LEDSet(LEDColor.BLUE), - // new ConeAutonReady() - // .withTimeout(1.5) - new IntakeScore(), - new WaitCommand(INTAKE_DEACQUIRE_TIME) - ); - - // intake second piece - addCommands( - new LEDSet(LEDColor.GREEN), new ParallelDeadlineGroup( @@ -201,11 +192,11 @@ public ThreePieceWLow() { new IntakeScore() .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) + .andThen(new ArmIntakeFirst() + .withTolerance(4, 10)) .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) - .andThen(new IntakeAcquire()), + .andThen(new IntakeAcquire()) - new ArmIntakeFirst() - .withTolerance(4, 10) ), new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) @@ -226,14 +217,16 @@ public ThreePieceWLow() { new LEDSet(LEDColor.RED), - new ParallelDeadlineGroup( - new SwerveDriveFollowTrajectory( - paths.get("Score Piece")) - .fieldRelative() - .withStop(), + new ParallelCommandGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new WaitCommand(0.5).andThen(new IntakeStop()), - new AutonMidCubeReady(), - new WaitCommand(0.5).andThen(new IntakeStop()) + new AutonMidCubeReady() + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) ), new ManagerSetGridNode(1), From e1f0f9f191c8cc47664f52f8352b4e7e82f898c7 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 10:54:37 -0400 Subject: [PATCH 21/40] Increase robot score wrist voltage --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 181a19ed..19868713 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -384,7 +384,7 @@ public interface Score { // Tip in scoring SmartNumber kForwardSpeed = new SmartNumber("Robot Score/Forward Speed (in per s)", 4); - SmartNumber kWristVoltage = new SmartNumber("Robot Score/Wrist Voltage", 2); + SmartNumber kWristVoltage = new SmartNumber("Robot Score/Wrist Voltage", 3); // Tip in release SmartNumber kBackwardsTipInSpeed = new SmartNumber("Robot Score/Tip In Backwards Speed (in per s)", 16); From 4d344534a4d5ceadaf33c3dd7751c12c5a9b8df9 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 10:59:30 -0400 Subject: [PATCH 22/40] Update pathplannerlib Co-authored-by: anivanchen --- vendordeps/PathplannerLib.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 69fddcd1..8e61586b 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.3", + "version": "2023.4.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" @@ -11,7 +11,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.3" + "version": "2023.4.4" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.3", + "version": "2023.4.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 57c3b12dd4caef4a6cc2ce02dec76b68b699f42c Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 12:08:37 -0400 Subject: [PATCH 23/40] Adjust 3 piece w low path and prep for arm jig --- .../deploy/pathplanner/3 Piece W Low.path | 45 ++++++++++--------- .../robot/commands/auton/ThreePieceWLow.java | 2 +- .../stuypulse/robot/constants/Settings.java | 4 +- 3 files changed, 27 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/3 Piece W Low.path b/src/main/deploy/pathplanner/3 Piece W Low.path index 45e1f455..f4920acc 100644 --- a/src/main/deploy/pathplanner/3 Piece W Low.path +++ b/src/main/deploy/pathplanner/3 Piece W Low.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 2.19, - "y": 4.46 + "x": 1.8, + "y": 4.6 }, "prevControl": null, "nextControl": { - "x": 3.5735287054512384, - "y": 4.46 + "x": 3.1835287054512387, + "y": 4.6 }, "holonomicAngle": 0, "isReversal": false, @@ -25,15 +25,15 @@ { "anchorPoint": { "x": 6.187741628152297, - "y": 4.440796639600396 + "y": 4.6 }, "prevControl": { "x": 5.435011671065698, - "y": 4.440796639600396 + "y": 4.6 }, "nextControl": { "x": 6.396944039439681, - "y": 4.440796639600396 + "y": 4.6 }, "holonomicAngle": 0, "isReversal": false, @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 6.62, - "y": 4.44 + "y": 4.6 }, "prevControl": { "x": 6.497482928507895, - "y": 4.44 + "y": 4.6 }, "nextControl": { "x": 6.497482928507895, - "y": 4.44 + "y": 4.6 }, "holonomicAngle": 0, "isReversal": true, @@ -85,7 +85,7 @@ "x": 3.5298747164225914, "y": 4.6 }, - "holonomicAngle": 6.538707501667168, + "holonomicAngle": 0.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -99,16 +99,16 @@ }, { "anchorPoint": { - "x": 6.58, - "y": 3.53 + "x": 6.614477160578253, + "y": 3.6198762995816596 }, "prevControl": { - "x": 5.084308630155721, - "y": 5.127206493729227 + "x": 5.107146966430685, + "y": 5.035853148629374 }, "nextControl": { - "x": 6.708196571169469, - "y": 3.393102508930692 + "x": 6.751173163320354, + "y": 3.4914649030663525 }, "holonomicAngle": -30.0, "isReversal": false, @@ -153,12 +153,12 @@ "y": 4.5 }, "prevControl": { - "x": 5.893518753704175, - "y": 5.298494015791451 + "x": 5.929327072329358, + "y": 5.28707484765397 }, "nextControl": { - "x": 5.893518753704175, - "y": 5.298494015791451 + "x": 5.929327072329358, + "y": 5.28707484765397 }, "holonomicAngle": 0.0, "isReversal": true, @@ -195,5 +195,8 @@ } } ], + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "isReversed": null, "markers": [] } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 1822b068..3018e6d4 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -226,7 +226,7 @@ public ThreePieceWLow() { new WaitCommand(0.5).andThen(new IntakeStop()), new AutonMidCubeReady() - .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 1) ), new ManagerSetGridNode(1), diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 19868713..b1683f91 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -188,7 +188,7 @@ public interface Shoulder { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.695582 + 180.0/360.0).plus(Rotation2d.fromDegrees(+90)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(AAAA).plus(Rotation2d.fromDegrees()).plus(Rotation2d.fromDegrees(+90)); SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Shoulder/Teleop Max Velocity (deg)", 315); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Shoulder/Teleop Max Acceleration (deg)", 420); @@ -237,7 +237,7 @@ public interface Wrist { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(60)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(0)); SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0); From 47f89700b2a86192358091cfd24211b4dedd1028 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 12:42:24 -0400 Subject: [PATCH 24/40] Tune wrist and shoulder zero angle --- src/main/java/com/stuypulse/robot/constants/Settings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b1683f91..3dab8403 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -188,7 +188,7 @@ public interface Shoulder { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(AAAA).plus(Rotation2d.fromDegrees()).plus(Rotation2d.fromDegrees(+90)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.025333).plus(Rotation2d.fromDegrees(+90)); SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Shoulder/Teleop Max Velocity (deg)", 315); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Shoulder/Teleop Max Acceleration (deg)", 420); @@ -237,7 +237,7 @@ public interface Wrist { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(0)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.662482).plus(Rotation2d.fromDegrees(180)); SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0); From e0cbe159d7af7abed26e95f6e44d8a302164722b Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 14:53:19 -0400 Subject: [PATCH 25/40] Tune stuff --- .../robot/constants/ArmTrajectories.java | 24 +++++++++---------- .../com/stuypulse/robot/constants/Field.java | 18 +++++++------- .../stuypulse/robot/constants/Settings.java | 4 ++-- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 6d194932..8893f924 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -63,12 +63,12 @@ public interface Stow { public interface Ready { public interface Mid { ArmState kConeTipUpBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Shoulder", -161.7), - new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Wrist",133.9)); + new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Shoulder", -172), + new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Wrist", 136)); ArmState kConeTipInBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -177), - new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Wrist", -168)); + new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -171), + new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Wrist", -175)); // -4, 0 ArmState kConeTipOutFront = new ArmState( @@ -77,10 +77,10 @@ public interface Mid { ArmState kAutonCubeBack = new ArmState( new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Shoulder", -165), - new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133 + 10)); + new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133)); ArmState kCubeFront = new ArmState( - new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -26), + new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -21), new SmartNumber("Arm Trajectories/Mid Cube Front/Wrist", 49)); ArmState kCubeBack = new ArmState( @@ -90,7 +90,7 @@ public interface Mid { public interface High { ArmState kConeTipInBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -193), + new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -185), new SmartNumber("Arm Trajectories/Ready High Tip In Back Wrist", -180)); // -175, 128 @@ -99,11 +99,11 @@ public interface High { new SmartNumber("Arm Trajectories/Ready High Tip Up Back Wrist", 136)); ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", -2), - new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 50)); + new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", 3), + new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 37)); ArmState kCubeFront = new ArmState( - new SmartNumber("Arm Trajectories/High Cube Front/Shoulder", -10), + new SmartNumber("Arm Trajectories/High Cube Front/Shoulder", -5), new SmartNumber("Arm Trajectories/High Cube Front/Wrist", 46)); ArmState kCubeAutonBack = new ArmState( @@ -119,8 +119,8 @@ public interface High { public interface Score { public interface High { ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -11 - 5), - new SmartNumber("Arm Trajectories/Score High Tip Out Front Wrist", 44)); + new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -5), + new SmartNumber("Arm Trajectories/Score High Tip Out Front Wrist", 37)); } public interface Mid { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 3add2da9..ce06861c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -85,15 +85,15 @@ public static Number[] getYPoseArray(Alliance alliance, ScoreSide side) { } public interface Back { - SmartNumber ONE = new SmartNumber("Alignment/Y Poses/Red 1", 7.4376); - SmartNumber TWO = new SmartNumber("Alignment/Y Poses/Red 2", 6.905); - SmartNumber THREE = new SmartNumber("Alignment/Y Poses/Red 3", 6.3238); - SmartNumber FOUR = new SmartNumber("Alignment/Y Poses/Red 4", 5.822); - SmartNumber FIVE = new SmartNumber("Alignment/Y Poses/Red 5", 5.2947); - SmartNumber SIX = new SmartNumber("Alignment/Y Poses/Red 6", 4.6); - SmartNumber SEVEN = new SmartNumber("Alignment/Y Poses/Red 7", 4.1028); - SmartNumber EIGHT = new SmartNumber("Alignment/Y Poses/Red 8", 3.557); - SmartNumber NINE = new SmartNumber("Alignment/Y Poses/Red 9", 2.89); + SmartNumber ONE = new SmartNumber("Alignment/Y Poses/Red 1", 7.494778); + SmartNumber TWO = new SmartNumber("Alignment/Y Poses/Red 2", 6.935978); + SmartNumber THREE = new SmartNumber("Alignment/Y Poses/Red 3", 6.377178); + SmartNumber FOUR = new SmartNumber("Alignment/Y Poses/Red 4", 5.843778); + SmartNumber FIVE = new SmartNumber("Alignment/Y Poses/Red 5", 5.259578); + SmartNumber SIX = new SmartNumber("Alignment/Y Poses/Red 6", 4.700778); + SmartNumber SEVEN = new SmartNumber("Alignment/Y Poses/Red 7", 4.14197); + SmartNumber EIGHT = new SmartNumber("Alignment/Y Poses/Red 8", 3.583178); + SmartNumber NINE = new SmartNumber("Alignment/Y Poses/Red 9", 3.024378); /* * Theoretical values: diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3dab8403..48b4bff1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -245,7 +245,7 @@ public interface Wrist { SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Auton Shoulder Velocity Feedback Cutoff (deg per s)", 5.0); SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Auton Feedback Enabled Debounce", 0.15); - SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Teleop Shoulder Velocity Feedback Cutoff (deg per s)", 15.0); + SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Teleop Shoulder Velocity Feedback Cutoff (deg per s)", 20.0); SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Teleop Feedback Enabled Debounce", 0.0); SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 80); @@ -358,7 +358,7 @@ public static Vector2D vpow(Vector2D vec, double power) { public interface Alignment { - SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.3); + SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); SmartNumber ALIGNED_CUBE_THRESHOLD_X = new SmartNumber("Alignment/X Cube Threshold", 0.08); SmartNumber ALIGNED_CUBE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cube Threshold", 0.1); From 5a3f0a7f9c9fd87e4efec364cd9ca26011778e26 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 14:53:29 -0400 Subject: [PATCH 26/40] Tune one piece mobility dock --- .../stuypulse/robot/commands/auton/OnePieceMobilityDock.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java index e234fe5b..8c8fc521 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -67,7 +67,7 @@ public OnePieceMobilityDock() { .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) .withTolerance(7, 9) - .withTimeout(6) + .withTimeout(3) ); addCommands( From 4b96190ec06def3b0a51c92812288e15e8d85057 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 14:53:43 -0400 Subject: [PATCH 27/40] Tune 2 piece wire --- src/main/deploy/pathplanner/2 Piece Wire.path | 40 +++++++++---------- .../robot/commands/auton/TwoPieceWire.java | 7 ++++ 2 files changed, 27 insertions(+), 20 deletions(-) diff --git a/src/main/deploy/pathplanner/2 Piece Wire.path b/src/main/deploy/pathplanner/2 Piece Wire.path index c38f8ffc..eecab3a2 100644 --- a/src/main/deploy/pathplanner/2 Piece Wire.path +++ b/src/main/deploy/pathplanner/2 Piece Wire.path @@ -25,15 +25,15 @@ { "anchorPoint": { "x": 4.025, - "y": 0.7036808379666587 + "y": 0.6 }, "prevControl": { - "x": 3.939417195382434, - "y": 0.7036808379666587 + "x": 3.9394171953824344, + "y": 0.6 }, "nextControl": { "x": 4.091564403591444, - "y": 0.7036808379666587 + "y": 0.6 }, "holonomicAngle": 0, "isReversal": false, @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 7.5, - "y": 1.4200000000000002 + "y": 0.97 }, "prevControl": { - "x": 5.986422913016767, - "y": 0.7879226014862292 + "x": 6.083140422508604, + "y": 0.6941716374535951 }, "nextControl": { - "x": 7.519005814510022, - "y": 1.4279369236594843 + "x": 7.520216962619652, + "y": 0.9739357546673141 }, "holonomicAngle": 0, "isReversal": false, @@ -75,15 +75,15 @@ { "anchorPoint": { "x": 7.5, - "y": 1.3 + "y": 0.8500000000000001 }, "prevControl": { - "x": 7.524864538684214, - "y": 1.306041827491513 + "x": 7.525347858738666, + "y": 0.8534978594569578 }, "nextControl": { - "x": 6.542296228896253, - "y": 1.0672878211629957 + "x": 6.301852034309053, + "y": 0.6846624369405326 }, "holonomicAngle": 0, "isReversal": false, @@ -124,16 +124,16 @@ }, { "anchorPoint": { - "x": 2.0, - "y": 1.2 + "x": 2.15, + "y": 1.35 }, "prevControl": { - "x": 2.1225494577571427, - "y": 0.6936088327872868 + "x": 3.2377422608937754, + "y": 0.9963188456626031 }, "nextControl": { - "x": 2.1225494577571427, - "y": 0.6936088327872868 + "x": 3.2377422608937754, + "y": 0.9963188456626031 }, "holonomicAngle": 0, "isReversal": true, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java index d9226529..bd2619f0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java @@ -100,6 +100,13 @@ public TwoPieceWire() { arm.runOnce(() -> arm.setWristVoltage(0)) ); + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + // drive to grid and score second piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), From bc671ba67aa5edf6c3f3130e48dc6324f41e7b7d Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 15:09:55 -0400 Subject: [PATCH 28/40] Comment out old autos --- src/main/java/com/stuypulse/robot/RobotContainer.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5de4ec6c..df8836c7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -229,16 +229,16 @@ private void configureOperatorBindings() { public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Mobility", new MobilityAuton()); - autonChooser.addOption("1.5 Piece Dock", new OnePiecePickupDock()); - autonChooser.addOption("1.5 Piece Dock + Wire", new OnePiecePickupDockWire()); + // autonChooser.addOption("1.5 Piece Dock", new OnePiecePickupDock()); + // autonChooser.addOption("1.5 Piece Dock + Wire", new OnePiecePickupDockWire()); autonChooser.addOption("Two Piece", new TwoPiece()); autonChooser.addOption("Two Piece Wire", new TwoPieceWire()); autonChooser.addOption("Two Piece Dock", new TwoPieceDock()); - autonChooser.addOption("One Piece Dock", new OnePieceDock()); + // autonChooser.addOption("One Piece Dock", new OnePieceDock()); autonChooser.addOption("One Piece Mobility Dock", new OnePieceMobilityDock()); - autonChooser.setDefaultOption("Three Piece", new ThreePiece()); + // autonChooser.addOption("Three Piece", new ThreePiece()); autonChooser.setDefaultOption("Three Piece W Low", new ThreePieceWLow()); - autonChooser.addOption("Three Piece Wire", new ThreePieceWire()); + // autonChooser.addOption("Three Piece Wire", new ThreePieceWire()); SmartDashboard.putData("Autonomous", autonChooser); } From 05d62b21714b14ce719cc049646e46a30ab72c9b Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 15:10:08 -0400 Subject: [PATCH 29/40] BOOM THREE PIECE --- .../deploy/pathplanner/3 Piece W Low.path | 56 +++++++++---------- .../robot/commands/auton/ThreePieceWLow.java | 29 +++++----- 2 files changed, 44 insertions(+), 41 deletions(-) diff --git a/src/main/deploy/pathplanner/3 Piece W Low.path b/src/main/deploy/pathplanner/3 Piece W Low.path index f4920acc..2d09f50b 100644 --- a/src/main/deploy/pathplanner/3 Piece W Low.path +++ b/src/main/deploy/pathplanner/3 Piece W Low.path @@ -25,15 +25,15 @@ { "anchorPoint": { "x": 6.187741628152297, - "y": 4.6 + "y": 4.44 }, "prevControl": { "x": 5.435011671065698, - "y": 4.6 + "y": 4.44 }, "nextControl": { "x": 6.396944039439681, - "y": 4.6 + "y": 4.44 }, "holonomicAngle": 0, "isReversal": false, @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 6.62, - "y": 4.6 + "y": 4.4 }, "prevControl": { - "x": 6.497482928507895, - "y": 4.6 + "x": 6.501547629778108, + "y": 4.431296466187375 }, "nextControl": { - "x": 6.497482928507895, - "y": 4.6 + "x": 6.501547629778108, + "y": 4.431296466187375 }, "holonomicAngle": 0, "isReversal": true, @@ -74,18 +74,18 @@ }, { "anchorPoint": { - "x": 2.26, + "x": 2.1599999999999997, "y": 4.6 }, "prevControl": { - "x": 3.5298747164225914, + "x": 3.4298747164225913, "y": 4.6 }, "nextControl": { - "x": 3.5298747164225914, + "x": 3.4298747164225913, "y": 4.6 }, - "holonomicAngle": 0.0, + "holonomicAngle": 6.54, "isReversal": true, "velOverride": null, "isLocked": false, @@ -99,16 +99,16 @@ }, { "anchorPoint": { - "x": 6.614477160578253, - "y": 3.6198762995816596 + "x": 6.58, + "y": 3.53 }, "prevControl": { - "x": 5.107146966430685, - "y": 5.035853148629374 + "x": 4.903826385233523, + "y": 4.751390831153475 }, "nextControl": { - "x": 6.751173163320354, - "y": 3.4914649030663525 + "x": 6.7315776729384345, + "y": 3.419548684990858 }, "holonomicAngle": -30.0, "isReversal": false, @@ -149,18 +149,18 @@ }, { "anchorPoint": { - "x": 1.75, + "x": 1.7999999999999998, "y": 4.5 }, "prevControl": { - "x": 5.929327072329358, - "y": 5.28707484765397 + "x": 5.794373267068212, + "y": 4.905051768145495 }, "nextControl": { - "x": 5.929327072329358, - "y": 5.28707484765397 + "x": 5.794373267068212, + "y": 4.905051768145495 }, - "holonomicAngle": 0.0, + "holonomicAngle": 10.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -174,12 +174,12 @@ }, { "anchorPoint": { - "x": 6.317578789003732, - "y": 5.355589856478859 + "x": 7.494791213358649, + "y": 5.540372759067196 }, "prevControl": { - "x": 5.392626169867724, - "y": 5.21855983882908 + "x": 6.738011797701916, + "y": 5.2881129538482865 }, "nextControl": null, "holonomicAngle": 0, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 3018e6d4..3e2fd420 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -124,11 +124,14 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { -70.82, 10); // 8.37); - double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + // double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double intermediateShoulderDegrees = -60; double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); return new ArmTrajectory() - // .addState(src.getShoulderDegrees(), wristSafeAngle) + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setShoulderTolerance(694) + .setWristTolerance(15)) .addState( new ArmState(intermediateShoulderDegrees, wristSafeAngle) @@ -175,7 +178,7 @@ public ThreePieceWLow() { addCommands( new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), new ManagerSetNodeLevel(NodeLevel.LOW), - new ManagerSetGamePiece(GamePiece.CONE_TIP_IN), + new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) ); @@ -192,10 +195,10 @@ public ThreePieceWLow() { new IntakeScore() .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) - .andThen(new ArmIntakeFirst() - .withTolerance(4, 10)) .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) .andThen(new IntakeAcquire()) + .andThen(new ArmIntakeFirst() + .withTolerance(4, 10)) ), @@ -210,6 +213,13 @@ public ThreePieceWLow() { arm.runOnce(() -> arm.setWristVoltage(0)) ); + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + // drive to grid and score second piece :: TODO: make custom arm setpoint for this addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -226,7 +236,7 @@ public ThreePieceWLow() { new WaitCommand(0.5).andThen(new IntakeStop()), new AutonMidCubeReady() - .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 1) + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) ), new ManagerSetGridNode(1), @@ -236,13 +246,6 @@ public ThreePieceWLow() { new IntakeStop() ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - // intake third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), From e3a0924aedce67fd2dc43e3e9de46b7c6254e7b4 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 16:21:34 -0400 Subject: [PATCH 30/40] Three piece wire (sad) --- src/main/deploy/pathplanner/3 Piece Wire.path | 186 ++++++++++----- .../robot/commands/auton/ThreePieceWLow.java | 16 -- .../robot/commands/auton/ThreePieceWire.java | 219 ++++++++++++++---- 3 files changed, 297 insertions(+), 124 deletions(-) diff --git a/src/main/deploy/pathplanner/3 Piece Wire.path b/src/main/deploy/pathplanner/3 Piece Wire.path index 4cc58162..c7948301 100644 --- a/src/main/deploy/pathplanner/3 Piece Wire.path +++ b/src/main/deploy/pathplanner/3 Piece Wire.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.9941842018915368, - "y": 0.5 + "x": 1.8, + "y": 0.74 }, "prevControl": null, "nextControl": { - "x": 2.994184201891537, - "y": 0.5 + "x": 2.8000000000000003, + "y": 0.74 }, "holonomicAngle": 0, "isReversal": false, @@ -24,16 +24,16 @@ }, { "anchorPoint": { - "x": 3.943570307069441, - "y": 0.7036808379666587 + "x": 4.025, + "y": 0.74 }, "prevControl": { - "x": 3.8579875024518744, - "y": 0.7036808379666587 + "x": 3.9394171953824344, + "y": 0.74 }, "nextControl": { - "x": 4.010134710660885, - "y": 0.7036808379666587 + "x": 4.091564403591444, + "y": 0.74 }, "holonomicAngle": 0, "isReversal": false, @@ -49,16 +49,16 @@ }, { "anchorPoint": { - "x": 7.2, - "y": 0.9 + "x": 7.5, + "y": 0.8500000000000001 }, "prevControl": { - "x": 5.60314484579627, - "y": 0.9469663280648167 + "x": 7.525347858738666, + "y": 0.8534978594569578 }, "nextControl": { - "x": 5.60314484579627, - "y": 0.9469663280648167 + "x": 7.525347858738666, + "y": 0.8534978594569578 }, "holonomicAngle": 0, "isReversal": true, @@ -68,22 +68,22 @@ "stopEvent": { "names": [], "executionBehavior": "parallel", - "waitBehavior": "before", + "waitBehavior": "none", "waitTime": 0 } }, { "anchorPoint": { - "x": 3.945273556840507, - "y": 0.9472995877682691 + "x": 4.6000000000000005, + "y": 0.74 }, "prevControl": { - "x": 4.121236461957803, - "y": 0.9472995877682691 + "x": 4.707626879752849, + "y": 0.74 }, "nextControl": { - "x": 3.759125629434196, - "y": 0.9472995877682691 + "x": 3.249347993927497, + "y": 0.7399999999999999 }, "holonomicAngle": 0, "isReversal": false, @@ -99,16 +99,16 @@ }, { "anchorPoint": { - "x": 2.36, - "y": 1.05 + "x": 2.45, + "y": 1.2000000000000002 }, "prevControl": { - "x": 2.9550460021986837, - "y": 0.8334209671805778 + "x": 3.1424514319266095, + "y": 0.8002129793764304 }, "nextControl": { - "x": 2.9550460021986837, - "y": 0.8334209671805778 + "x": 3.1424514319266095, + "y": 0.8002129793764304 }, "holonomicAngle": 0, "isReversal": true, @@ -124,20 +124,45 @@ }, { "anchorPoint": { - "x": 3.9364500016977466, - "y": 0.9280457054566447 + "x": 4.33, + "y": 0.9 }, "prevControl": { - "x": 3.7975022038197768, - "y": 0.9280457054566447 + "x": 3.0442054627112523, + "y": 0.9 }, "nextControl": { - "x": 4.075397799575718, - "y": 0.9280457054566447 + "x": 4.776276695913049, + "y": 0.9 }, "holonomicAngle": 0, "isReversal": false, - "velOverride": 0.75, + "velOverride": 0.6, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.46706608098531, + "y": 0.9436385306337061 + }, + "prevControl": { + "x": 5.99141720254821, + "y": 0.779621676000223 + }, + "nextControl": { + "x": 7.008957514418527, + "y": 1.1304976456106774 + }, + "holonomicAngle": 15.0, + "isReversal": false, + "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": { @@ -149,18 +174,18 @@ }, { "anchorPoint": { - "x": 6.982883890789484, - "y": 2.0618508255788215 + "x": 8.27, + "y": 2.2 }, "prevControl": { - "x": 5.70395524896903, - "y": 0.7829221837583678 + "x": 7.8756722931627685, + "y": 1.6405622825231674 }, "nextControl": { - "x": 5.70395524896903, - "y": 0.7829221837583678 + "x": 7.8756722931627685, + "y": 1.6405622825231674 }, - "holonomicAngle": 45.0, + "holonomicAngle": 30.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -174,20 +199,45 @@ }, { "anchorPoint": { - "x": 3.9254150735135243, - "y": 0.9488987206598537 + "x": 6.46706608098531, + "y": 0.9342955748848557 }, "prevControl": { - "x": 4.676603404598951, - "y": 0.9488987206598537 + "x": 7.205159585144346, + "y": 1.2706419818434043 }, "nextControl": { - "x": 3.2358275315495755, - "y": 0.9488987206598537 + "x": 5.901838611156348, + "y": 0.6767235633172274 }, "holonomicAngle": 0, "isReversal": false, - "velOverride": 0.6, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.73, + "y": 0.85 + }, + "prevControl": { + "x": 4.834901218567718, + "y": 0.8674835364279528 + }, + "nextControl": { + "x": 4.5618267965207275, + "y": 0.8219711327534546 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 0.5, "isLocked": false, "isStopPoint": false, "stopEvent": { @@ -199,12 +249,37 @@ }, { "anchorPoint": { - "x": 2.04, - "y": 1.05 + "x": 2.65, + "y": 1.2052412916014645 + }, + "prevControl": { + "x": 3.7898406013595243, + "y": 0.8221801058986732 + }, + "nextControl": { + "x": 3.7898406013595243, + "y": 0.8221801058986732 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9631539402938953, + "y": 0.803494194400977 }, "prevControl": { - "x": 2.894697148904932, - "y": 0.8719604723009351 + "x": 3.983786695704312, + "y": 0.7983367444252163 }, "nextControl": null, "holonomicAngle": 0, @@ -220,8 +295,5 @@ } } ], - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "isReversed": null, "markers": [] } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 3e2fd420..2a0b0e1e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -32,22 +32,6 @@ public class ThreePieceWLow extends DebugSequentialCommandGroup { - static class ConeAutonReady extends ArmRoutine { - public ConeAutonReady() { - super(Manager.getInstance()::getReadyTrajectory); - } - - @Override - protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - - return new ArmTrajectory() - .addState(new ArmState(dest.getShoulderState(), src.getWristState()) - .setShoulderTolerance(20).setWristLimp(true).setWristTolerance(360)) - .addState(new ArmState(dest.getShoulderState(), dest.getWristState()).setWristTolerance(7) - .setShoulderTolerance(15)); - } - } - static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { super(() -> Ready.Mid.kAutonCubeBack); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java index 7c15feb3..23689c47 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java @@ -10,100 +10,204 @@ import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.swerve.*; +import com.stuypulse.robot.constants.ArmTrajectories; +import com.stuypulse.robot.constants.ArmTrajectories.Ready; +import com.stuypulse.robot.constants.Settings.Arm.Wrist; import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.util.ArmState; +import com.stuypulse.robot.util.ArmTrajectory; import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; public class ThreePieceWire extends DebugSequentialCommandGroup { + static class AutonMidCubeReady extends ArmRoutine { + public AutonMidCubeReady() { + super(() -> Ready.Mid.kAutonCubeBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(7).setShoulderTolerance(20)); + } + } + + static class AutonHighCubeReady extends ArmRoutine { + public AutonHighCubeReady() { + super(() -> Ready.High.kCubeAutonBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(30).setShoulderTolerance(20)); + } + } + + static class ArmIntakeFirst extends ArmRoutine { + public ArmIntakeFirst() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 11); + // 8.37); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + // .addState( + // new ArmState(intermediateShoulderDegrees, wristSafeAngle) + // .setShoulderTolerance(15) + // .setWristTolerance(360)) + + // .addState( + // new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + // .setWristTolerance(360)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + static class ArmIntakeSecond extends ArmRoutine { + public ArmIntakeSecond() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 10); + // 8.37); + // double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double intermediateShoulderDegrees = -60; + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setShoulderTolerance(694) + .setWristTolerance(15)) + + .addState( + new ArmState(intermediateShoulderDegrees, wristSafeAngle) + .setShoulderTolerance(15) + .setWristTolerance(15)) + + .addState( + new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + .setWristTolerance(20)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } private static final double INTAKE_DEACQUIRE_TIME = 0.5; private static final double INTAKE_STOP_WAIT_TIME = 0.5; private static final double INTAKE_WAIT_TIME = 2.0; private static final double ACQUIRE_WAIT_TIME = 0.4; - private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(4, 3.2); - private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.8, 4); + private static final PathConstraints INTAKE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.5, 2); + private static final PathConstraints SCORE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.2, 1.2); + private static final PathConstraints INTAKE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(4, 3.5); + private static final PathConstraints SCORE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(3, 2); public ThreePieceWire() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece Wire", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS), - "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece" + PathPlanner.loadPathGroup("3 Piece Wire", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_SECOND_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, SCORE_THIRD_PIECE_CONSTRAINTS), + "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); var arm = Arm.getInstance(); // initial setup addCommands( - new ManagerSetNodeLevel(NodeLevel.MID), + new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), + new ManagerSetNodeLevel(NodeLevel.LOW), new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) ); - // score first piece - addCommands( - new LEDSet(LEDColor.RED), - new ArmReady() - .setWristVelocityTolerance(25) - .setShoulderVelocityTolerance(45) - .withTolerance(7, 9) - .withTimeout(4) - ); - + // score first piece + intake second piece addCommands( - new LEDSet(LEDColor.BLUE), - new IntakeScore(), - new WaitCommand(INTAKE_DEACQUIRE_TIME) - ); - - // intake second piece - addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelCommandGroup( + new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative() .withStop(), + // .until(Intake.getInstance()::hasGamePiece), // interrupting IntakeScore? idk one time the intake just stopped early - new WaitCommand(INTAKE_STOP_WAIT_TIME) + new IntakeScore() + .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) - .andThen(new WaitCommand(INTAKE_WAIT_TIME)) - .andThen(new IntakeAcquire()), + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) + .andThen(new IntakeAcquire()) + .andThen(new ArmIntakeFirst() + .withTolerance(4, 10)) + ) - new ArmIntake() - .withTolerance(7, 10) - .withTimeout(6.5) - ), + // new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) + // .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))), - new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) - .alongWith(arm.runOnce(() -> arm.setWristVoltage(-2))), + // arm.runOnce(() -> arm.setWristVoltage(0)) + ); - arm.runOnce(() -> arm.setWristVoltage(0)) + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) ); - // drive to grid and score second piece + // drive to grid and score second piece :: TODO: make custom arm setpoint for this addCommands( new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetScoreSide(ScoreSide.BACK), new LEDSet(LEDColor.RED), - new SwerveDriveFollowTrajectory( - paths.get("Score Piece")) - .fieldRelative() - .withStop() - .alongWith(new ArmReady() - .withTolerance(17, 9).alongWith(new WaitCommand(0.1).andThen(new IntakeStop()))), + new ParallelCommandGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new WaitCommand(0.5).andThen(new IntakeStop()), + + new AutonMidCubeReady() + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) + ), new ManagerSetGridNode(1), // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), @@ -118,19 +222,20 @@ public ThreePieceWire() { new LEDSet(LEDColor.GREEN), - new ParallelCommandGroup( + new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Third Piece")) .fieldRelative() - .withStop(), + .withStop() + .until(Intake.getInstance()::hasGamePiece), new WaitCommand(INTAKE_STOP_WAIT_TIME) .andThen(new IntakeStop()) .andThen(new WaitCommand(INTAKE_WAIT_TIME)) .andThen(new IntakeAcquire()), - new ArmIntake() - .withTolerance(7, 10) - .withTimeout(6.5) + new ArmIntakeSecond() + // .setShoulderVelocityTolerance(10) + // .withTolerance(4, 10) ), new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) @@ -139,7 +244,14 @@ public ThreePieceWire() { arm.runOnce(() -> arm.setWristVoltage(0)) ); - // drive to grid and score second piece + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(5); + arm.setShoulderVelocityFeedbackDebounce(0.2); + }) + ); + + // drive to grid and score third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetNodeLevel(NodeLevel.HIGH), @@ -151,8 +263,7 @@ public ThreePieceWire() { paths.get("Score Third Piece")) .fieldRelative() .withStop() - .alongWith(new ArmReady() - .withTolerance(17, 9).alongWith(new WaitCommand(0.1).andThen(new IntakeStop()))), + .alongWith(new AutonHighCubeReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), new ManagerSetGridNode(1), // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), @@ -161,8 +272,14 @@ public ThreePieceWire() { new IntakeStop() ); + // back away addCommands( - new LEDSet(LEDColor.RAINBOW) + new LEDSet(LEDColor.RAINBOW), + + new SwerveDriveFollowTrajectory( + paths.get("Back Away")) + .fieldRelative() + .withStop() ); } } From 36bd6cfee0046c89efd605d7dba693bb8960a9ce Mon Sep 17 00:00:00 2001 From: vincentw Date: Mon, 10 Apr 2023 16:42:13 -0400 Subject: [PATCH 31/40] Align against grid detection (#183) * Add against grid debouncer in AlignThenScore * Copy derivative into util and add getOutput * Fixes --------- Co-authored-by: BenG49 --- .../robot/commands/RobotAlignThenScore.java | 25 ++++++++++-- .../stuypulse/robot/constants/Settings.java | 3 ++ .../com/stuypulse/robot/util/Derivative.java | 40 +++++++++++++++++++ 3 files changed, 64 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/util/Derivative.java diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java index f6eeab56..0531cc16 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java @@ -9,7 +9,7 @@ import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; - +import com.stuypulse.stuylib.streams.filters.IFilter; import com.stuypulse.robot.constants.ArmTrajectories; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.LEDController; @@ -23,6 +23,7 @@ import com.stuypulse.robot.subsystems.intake.*; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.Derivative; import com.stuypulse.robot.util.HolonomicController; import com.stuypulse.robot.util.LEDColor; @@ -48,10 +49,14 @@ public class RobotAlignThenScore extends CommandBase { private final BStream aligned; private boolean movingWhileScoring; // when we have aligned and ready to score and move back + // Against grid debounce + private final Derivative xErrorChange; + private final BStream stoppedByGrid; + // Logging private final FieldObject2d targetPose2d; - public RobotAlignThenScore(){ + public RobotAlignThenScore() { this.swerve = SwerveDrive.getInstance(); this.arm = Arm.getInstance(); this.intake = Intake.getInstance(); @@ -66,18 +71,26 @@ public RobotAlignThenScore(){ aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); + xErrorChange = new Derivative(); + stoppedByGrid = BStream.create(this::isAgainstGrid).filtered(new BDebounceRC.Both(Alignment.AGAINST_GRID_DEBOUNCE)); + targetPose2d = Odometry.getInstance().getField().getObject("Target Pose"); + addRequirements(swerve, arm, intake); } + private boolean isAgainstGrid() { + return xErrorChange.getOutput() < Alignment.AGAINST_GRID_VEL_X.get(); + } + private boolean isAligned() { if (Manager.getInstance().getGamePiece().isCone()) { - return controller.isDone( + return stoppedByGrid.get() || controller.isDone( Alignment.ALIGNED_CONE_THRESHOLD_X.get(), Alignment.ALIGNED_CONE_THRESHOLD_Y.get(), Alignment.ALIGNED_CONE_THRESHOLD_ANGLE.get()); } else { - return controller.isDone( + return stoppedByGrid.get() || controller.isDone( Alignment.ALIGNED_CUBE_THRESHOLD_X.get(), Alignment.ALIGNED_CUBE_THRESHOLD_Y.get(), Alignment.ALIGNED_CUBE_THRESHOLD_ANGLE.get()); @@ -90,6 +103,8 @@ public void initialize() { intake.enableBreak(); Odometry.USE_VISION_ANGLE.set(true); + xErrorChange.reset(); + LEDController.getInstance().setColor(LEDColor.BLUE, 694000000); } @@ -99,6 +114,8 @@ public void execute() { Pose2d targetPose = Manager.getInstance().getScorePose(); targetPose2d.setPose(targetPose); + xErrorChange.get(targetPose.getX() - currentPose.getX()); + controller.update(targetPose, currentPose); if (aligned.get() || movingWhileScoring) { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 48b4bff1..22deba4b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -368,6 +368,9 @@ public interface Alignment { SmartNumber ALIGNED_CONE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cone Threshold", 0.05); SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 1); + SmartNumber AGAINST_GRID_VEL_X = new SmartNumber("Alignment/Against Grid X Velocity (m per s)", 0.02); + SmartNumber AGAINST_GRID_DEBOUNCE = new SmartNumber("Alignment/Against Grid Debounce", 0.3); + public interface Translation { SmartNumber P = new SmartNumber("Alignment/Translation/kP", 2); SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0); diff --git a/src/main/java/com/stuypulse/robot/util/Derivative.java b/src/main/java/com/stuypulse/robot/util/Derivative.java new file mode 100644 index 00000000..31ba4f7b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/Derivative.java @@ -0,0 +1,40 @@ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */ +/* This work is licensed under the terms of the MIT license */ +/* found in the root directory of this project. */ + +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.streams.filters.IFilter; +import com.stuypulse.stuylib.util.StopWatch; + +/** + * This class takes an IStream and gives you the derivative with respect to time. + * + * @author Sam (sam.belliveau@gmail.com) + */ +public class Derivative implements IFilter { + + private final StopWatch mTimer; + private double mLastValue; + private double mOutput; + + public Derivative() { + mTimer = new StopWatch(); + mLastValue = 0.0; + } + + public double get(double next) { + double diff = next - mLastValue; + mLastValue = next; + mOutput = diff / mTimer.reset(); + return mOutput; + } + + public double getOutput() { + return mOutput; + } + + public void reset() { + mOutput = Double.POSITIVE_INFINITY; + } +} From c0d51ccade13db86fc82319b2ef672cbf0896e7b Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Thu, 13 Apr 2023 19:33:25 -0400 Subject: [PATCH 32/40] LED code cleanup + LEDController periodic fix --- .../commands/RobotAlignThenScoreCubes.java | 2 +- .../stuypulse/robot/commands/leds/LEDSet.java | 9 +---- .../robot/subsystems/leds/LEDController.java | 30 +-------------- .../robot/subsystems/leds/LEDRainbow.java | 37 +++++++++++-------- 4 files changed, 25 insertions(+), 53 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java index 68f53635..56a40637 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java @@ -12,7 +12,7 @@ import com.stuypulse.robot.constants.ArmTrajectories; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.LEDController; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Alignment.Rotation; import com.stuypulse.robot.constants.Settings.Alignment.Translation; diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java index 9d12d20a..3d32bc7f 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java @@ -8,6 +8,7 @@ * - commands for LEDs * @author Richie Xue * @author Jo Walkup + * @author Naowal Rahman */ package com.stuypulse.robot.commands.leds; @@ -19,20 +20,14 @@ public class LEDSet extends InstantCommand { - private double updateTime; private LEDController controller; private LEDInstruction instruction; - public LEDSet(LEDInstruction instruction, double updateTime) { + public LEDSet(LEDInstruction instruction) { this.controller = LEDController.getInstance(); - this.updateTime = updateTime; this.instruction = instruction; } - public LEDSet(LEDInstruction instruction) { - this(instruction, Settings.LED.MANUAL_UPDATE_TIME); - } - @Override public void initialize() { controller.forceSetLED(instruction); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 90f922fd..70e440b2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -5,8 +5,6 @@ package com.stuypulse.robot.subsystems.leds; -import com.stuypulse.stuylib.util.StopWatch; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.MatchState; import com.stuypulse.robot.subsystems.Manager; @@ -36,28 +34,8 @@ public static LEDController getInstance() { return instance; } - // Stopwatch to check when to start overriding manual updates - private final StopWatch lastUpdate; - private double manualTime; - - // The current color to set the LEDs to - private LEDInstruction manualColor; - - public LEDController() { - this.lastUpdate = new StopWatch(); - } - - public void setColor(LEDInstruction color, double time) { - manualColor = color; - manualTime = time; - lastUpdate.reset(); - } - public abstract void forceSetLED(LEDInstruction instruction); - public void setLEDConditions() { - } - public LEDInstruction getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { case CUBE: return LEDColor.PURPLE; @@ -70,13 +48,7 @@ public LEDInstruction getDefaultColor() { @Override public void periodic() { - // If we called .setColor() recently, use that value - if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { - forceSetLED(manualColor); - } - - // Otherwise use the default color - else { + if (Robot.getMatchState() == MatchState.TELEOP) { forceSetLED(getDefaultColor()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index 0193d644..c81a834e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -9,11 +9,29 @@ * - setLED() : sets LEDs to rainbow colors * @author Richie Xue * @author Jo Walkup + * @author Naowal Rahman */ public class LEDRainbow implements LEDInstruction { StopWatch timer; int counter; - + + private static SLColor[] rainbow = new SLColor[]{ + new SLColor (218,165,32), + new SLColor(255, 83, 73), + new SLColor(255, 128, 0), + new SLColor(255, 255, 0), + new SLColor(102, 204, 0), + new SLColor(0, 255, 0), + new SLColor(0, 153, 0), + new SLColor(0, 255, 128), + new SLColor(0, 128, 255), + new SLColor(0, 0, 204), + new SLColor(51, 51, 255), + new SLColor(127, 0, 255), + new SLColor(160, 32, 240), + new SLColor(255, 192, 203) + }; + public LEDRainbow() { timer = new StopWatch(); counter = 0; @@ -21,26 +39,13 @@ public LEDRainbow() { @Override public void setLED(AddressableLEDBuffer ledsBuffer) { - SLColor[] rainbow = new SLColor[]{new SLColor (218,165,32), - new SLColor(255, 83, 73), - new SLColor(255, 128, 0), - new SLColor(255, 255, 0), - new SLColor(102, 204, 0), - new SLColor(0, 255, 0), - new SLColor(0, 153, 0), - new SLColor(0, 255, 128), - new SLColor(0, 128, 255), - new SLColor(0, 0, 204), - new SLColor(51, 51, 255), - new SLColor(127, 0, 255), - new SLColor(160, 32, 240), - new SLColor(255, 192, 203)}; // if(timer.getTime() > 3 * Math.sin(colorCounter * 2.0) + 1) { + SLColor color; if(timer.getTime() > 1.0) { timer.reset(); for(int i = 0; i < ledsBuffer.getLength(); i++) { - SLColor color = rainbow[(i + counter) % rainbow.length]; + color = rainbow[(i + counter) % rainbow.length]; ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); ++counter; if(counter == rainbow.length) counter = 0; From aa3daf5b84caa5a12329489438c82751a3181fb8 Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Wed, 13 Dec 2023 13:03:34 -0500 Subject: [PATCH 33/40] Add AddressableLED sim --- .../com/stuypulse/robot/RobotContainer.java | 4 +- .../robot/commands/RobotAlignThenScore.java | 6 +- .../commands/RobotAlignThenScoreCubes.java | 10 +- .../balance/SwerveDriveBalanceBlay.java | 4 +- .../robot/subsystems/leds/LEDController.java | 6 +- .../robot/subsystems/leds/LEDVisualizer.java | 121 ++++++++++++++++++ 6 files changed, 142 insertions(+), 9 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1899f515..9884506b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -238,6 +238,8 @@ private void configureOperatorBindings() { /**************/ public void configureAutons() { + autonChooser.setDefaultOption("Do Nothing", new DoNothing()); + /* autonChooser.addOption("Do Nothing", new DoNothing()); autonChooser.addOption("Mobility", new Mobility()); @@ -268,7 +270,7 @@ public void configureAutons() { // autonChooser.addOption("Three Piece Bump Removed Blue", new BCThreePieceBumpBlue()); // autonChooser.addOption("Three Piece Bump Removed Red", new BCThreePieceBumpRed()); autonChooser.addOption("Three Piece Bump", new ThreePieceBump()); - + */ SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java index c6cc491f..be102868 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java @@ -41,6 +41,7 @@ public class RobotAlignThenScore extends CommandBase { private final SwerveDrive swerve; private final Arm arm; private final Intake intake; + private final LEDController leds; private final Manager manager; @@ -61,6 +62,7 @@ public RobotAlignThenScore() { this.arm = Arm.getInstance(); this.intake = Intake.getInstance(); manager = Manager.getInstance(); + this.leds = LEDController.getInstance(); controller = new HolonomicController( new PIDController(Translation.P,Translation.I,Translation.D), @@ -111,7 +113,7 @@ public void initialize() { xErrorChange.reset(); - LEDController.getInstance().setColor(LEDColor.BLUE, 694000000); + leds.forceSetLED(LEDColor.BLUE); } @Override @@ -179,7 +181,7 @@ public void end(boolean interupted) { intake.stop(); targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); - LEDController.getInstance().setColor(LEDController.getInstance().getDefaultColor(), 0); + leds.forceSetLED(leds.getDefaultColor()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java index 61ca602e..20284962 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java @@ -35,6 +35,7 @@ public class RobotAlignThenScoreCubes extends CommandBase { // Subsystems private final SwerveDrive swerve; private final Intake intake; + private final LEDController leds; private final Manager manager; @@ -50,6 +51,7 @@ public RobotAlignThenScoreCubes(){ this.swerve = SwerveDrive.getInstance(); this.intake = Intake.getInstance(); manager = Manager.getInstance(); + this.leds = LEDController.getInstance(); controller = new HolonomicController( new PIDController(Translation.P,Translation.I,Translation.D), @@ -93,7 +95,7 @@ public void initialize() { intake.enableBreak(); Odometry.USE_VISION_ANGLE.set(true); - LEDController.getInstance().setColor(LEDColor.BLUE, 694000000); + leds.forceSetLED(LEDColor.BLUE); } @Override @@ -111,7 +113,7 @@ public void execute() { // simply outtake when low if (manager.getNodeLevel() == NodeLevel.LOW) { - LEDController.getInstance().setColor(LEDColor.GREEN, 694000000); + leds.forceSetLED(LEDColor.GREEN); intake.deacquire(); } @@ -121,7 +123,7 @@ public void execute() { // only score for cubes if (manager.getGamePiece().isCube()) { - LEDController.getInstance().setColor(LEDColor.GREEN, 694000000); + leds.forceSetLED(LEDColor.GREEN); intake.deacquire(); } } @@ -146,7 +148,7 @@ public void end(boolean interupted) { intake.stop(); targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); - LEDController.getInstance().setColor(LEDController.getInstance().getDefaultColor(), 0); + leds.forceSetLED(leds.getDefaultColor()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java b/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java index 48d4d4df..927c5eb6 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/balance/SwerveDriveBalanceBlay.java @@ -38,6 +38,7 @@ public class SwerveDriveBalanceBlay extends CommandBase { private SwerveDrive swerve; private Odometry odometry; private Plant plant; + private LEDController leds; public SwerveDriveBalanceBlay() { maxSpeed = AutoBalance.MAX_SPEED; @@ -47,6 +48,7 @@ public SwerveDriveBalanceBlay() { swerve = SwerveDrive.getInstance(); odometry = Odometry.getInstance(); plant = Plant.getInstance(); + leds = LEDController.getInstance(); control = new PIDController(kP, 0, kD).setOutputFilter(x -> -x); addRequirements(swerve, plant); @@ -82,7 +84,7 @@ public void end(boolean interrupted) { plant.engage(); if (timedOut) { - LEDController.getInstance().setColor(LEDColor.YELLOW, 1); + leds.forceSetLED(LEDColor.BLUE); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 70e440b2..e6dff51f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -34,11 +34,14 @@ public static LEDController getInstance() { return instance; } + // LEDVisualizer + LEDVisualizer visualizer = new LEDVisualizer(); + public abstract void forceSetLED(LEDInstruction instruction); public LEDInstruction getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PURPLE; + case CUBE: return LEDColor.PULSE_RED_BLUE; case CONE_TIP_IN: return LEDColor.YELLOW; case CONE_TIP_UP: return LEDColor.GREEN; case CONE_TIP_OUT: return LEDColor.ORANGE; @@ -51,5 +54,6 @@ public void periodic() { if (Robot.getMatchState() == MatchState.TELEOP) { forceSetLED(getDefaultColor()); } + visualizer.setColor(LEDColor.BLUE, 10); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java new file mode 100644 index 00000000..1af314f6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java @@ -0,0 +1,121 @@ +package com.stuypulse.robot.subsystems.leds; + +import com.stuypulse.stuylib.util.StopWatch; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.Robot.MatchState; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +public class LEDVisualizer { + // Simulation Constants + private final double WINDOW_WIDTH = 55; + private final double WINDOW_HEIGHT = 10; + private final double WINDOW_X_PADDING = 0.5; + private final double LED_WIDTH = 15; + + // Mechanism2ds that displays the LEDs + private final Mechanism2d sim; + private final MechanismRoot2d startRoot; + private final MechanismLigament2d[] leds; + private final AddressableLEDBuffer ledsBuffer; + + // Stopwatch to check when to start overriding manual updates + private StopWatch lastUpdate; + private double manualTime; + + // The current color to set the LEDs to + private LEDColor manualColor; + + protected LEDVisualizer() { + sim = new Mechanism2d(WINDOW_WIDTH, WINDOW_HEIGHT); + leds = new MechanismLigament2d[Settings.LED.LED_LENGTH]; + ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); + + // Create a row of squares to represent the LEDs + for (int i = 0; i < Settings.LED.LED_LENGTH; i++) { + leds[i] = new MechanismLigament2d("LED " + i, WINDOW_X_PADDING + i, 0, LED_WIDTH, new Color8Bit(0, 0, 0)); + } + + // Set the root of the Mechanism2d to be the center of the row of squares + startRoot = sim.getRoot("Led Root", 0, WINDOW_HEIGHT / 2); + + // Append the squares onto each other + for (int i = 0; i < Settings.LED.LED_LENGTH; i++) { + startRoot.append(leds[i]); + } + + // Set the LEDs to be off + forceSetLED(LEDColor.BLACK); + + // Initialize the stopwatch + lastUpdate = new StopWatch(); + + SmartDashboard.putData("SimLED", sim); + + } + + public void setColor(LEDColor color, double time) { + manualColor = color; + manualTime = time; + lastUpdate.reset(); + } + + private void forceSetLED(LEDColor ledColor) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, ledColor.getRed(), ledColor.getGreen(), ledColor.getBlue()); + } + } + + public void forceSetLED(LEDInstruction instruction) { + // for (int i = 0; i < ledsBuffer.getLength(); i++) { + // ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); + // } + // leds.setData(ledsBuffer); + instruction.setLED(ledsBuffer); + } + + public void setLEDConditions() { + } + + public LEDColor getDefaultColor() { + switch (Manager.getInstance().getGamePiece()) { + case CUBE: return LEDColor.PURPLE; + case CONE_TIP_IN: return LEDColor.YELLOW; + case CONE_TIP_UP: return LEDColor.GREEN; + case CONE_TIP_OUT: return LEDColor.ORANGE; + default: return LEDColor.OFF; + } + } + + public void periodic() { + // If we called .setColor() recently, use that value + if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { + forceSetLED(manualColor); + SmartDashboard.putNumber("LED Mech2d/LED 0R", manualColor.getRed()); + SmartDashboard.putNumber("LED Mech2d/LED 0G", manualColor.getGreen()); + SmartDashboard.putNumber("LED Mech2d/LED 0B", manualColor.getBlue()); + } + + // Otherwise use the default color + else { + //forceSetLED(getDefaultColor()); + forceSetLED(LEDColor.RAINBOW); + } + + SmartDashboard.putNumber("LED Mech2d/LED Time", lastUpdate.getTime()); + } +} From 90928379b46f9fb1b95178a06cb7936b3d0e2d3c Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Wed, 13 Dec 2023 15:23:09 -0500 Subject: [PATCH 34/40] Add LEDSection --- .../robot/subsystems/leds/LEDSection.java | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java new file mode 100644 index 00000000..ff126446 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.subsystems.leds; + +import com.stuypulse.robot.util.SLColor; +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +public class LEDSection implements LEDInstruction { + + public SLColor[] sections; + + public LEDSection(SLColor[] sections) { + this.sections = sections; + } + + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + int sectionLength = ledsBuffer.getLength() / sections.length; + + for (int i = 0; i < sections.length; i++) { + for (int j = 0; j < sectionLength; j++) { + ledsBuffer.setRGB(i * sectionLength + j, sections[i].getRed(), sections[i].getGreen(), sections[i].getBlue()); + } + } + + } + +} From 31fd6bc61df6334ed97fe734626618086f6580e3 Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Wed, 13 Dec 2023 16:55:45 -0500 Subject: [PATCH 35/40] Delete SLColor, use awt.Color instead Co-authored-by: bananaeater321 --- .../robot/subsystems/leds/LEDController.java | 2 +- .../robot/subsystems/leds/LEDPulseColor.java | 12 ++-- .../robot/subsystems/leds/LEDSection.java | 6 +- .../com/stuypulse/robot/util/LEDColor.java | 68 +++++++++++-------- .../com/stuypulse/robot/util/SLColor.java | 25 ------- 5 files changed, 49 insertions(+), 64 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/util/SLColor.java diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index e6dff51f..bf3eac6e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -41,7 +41,7 @@ public static LEDController getInstance() { public LEDInstruction getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PULSE_RED_BLUE; + case CUBE: return LEDColor.PURPLE; case CONE_TIP_IN: return LEDColor.YELLOW; case CONE_TIP_UP: return LEDColor.GREEN; case CONE_TIP_OUT: return LEDColor.ORANGE; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java index 511483ca..703b43d4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java @@ -1,9 +1,9 @@ package com.stuypulse.robot.subsystems.leds; -import com.stuypulse.robot.util.SLColor; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import java.awt.Color; /** * Class that sets colour of pulsing LEDS on LEDController @@ -17,15 +17,15 @@ */ public class LEDPulseColor implements LEDInstruction { - public SLColor color; - public SLColor altcolor; + public Color color; + public Color altcolor; public StopWatch stopwatch; - public LEDPulseColor(SLColor color) { - this(color, new SLColor(0,0,0)); + public LEDPulseColor(Color color) { + this(color, new Color(0,0,0)); } - public LEDPulseColor(SLColor color1, SLColor color2) { + public LEDPulseColor(Color color1, Color color2) { this.color = color1; this.altcolor = color2; stopwatch = new StopWatch(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java index ff126446..6d62169d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java @@ -1,15 +1,15 @@ package com.stuypulse.robot.subsystems.leds; -import com.stuypulse.robot.util.SLColor; +import java.awt.Color; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; public class LEDSection implements LEDInstruction { - public SLColor[] sections; + public Color[] sections; - public LEDSection(SLColor[] sections) { + public LEDSection(Color[] sections) { this.sections = sections; } diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index 3a7fe4d8..723f6e70 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -7,8 +7,10 @@ import com.stuypulse.robot.subsystems.leds.LEDInstruction; import com.stuypulse.robot.subsystems.leds.LEDPulseColor; import com.stuypulse.robot.subsystems.leds.LEDRainbow; +import com.stuypulse.robot.subsystems.leds.LEDSection; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import java.awt.Color; /** * Class that stores all of the different RGB values for the LED Controller. @@ -37,6 +39,10 @@ public LEDColor(SLColor color) { this(color.getRed(), color.getGreen(), color.getBlue()); } + public LEDColor(Color color) { + this(color.getRed(), color.getGreen(), color.getBlue()); + } + public int getRed() { return red; } @@ -49,8 +55,12 @@ public int getBlue() { return blue; } - public SLColor getSLColor() { - return new SLColor(red, green, blue); + public static Color getAWTColor(int red, int green, int blue) { + return new Color(red, green, blue); + } + + public Color getAWTColor() { + return new Color(red, green, blue); } @Override @@ -65,35 +75,35 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { /*** COLOR CONSTANTS ***/ /***********************/ - public static final LEDColor AQUA = new LEDColor(new SLColor(0, 255, 255)); - public static final LEDColor BLACK = new LEDColor(new SLColor(0, 0, 0)); - public static final LEDColor BLUE = new LEDColor(new SLColor(0, 128, 255)); - public static final LEDColor BLUE_GREEN = new LEDColor(new SLColor(0, 255, 128)); - public static final LEDColor BLUE_VIOLET = new LEDColor(new SLColor(51, 51, 255)); - public static final LEDColor DARK_BLUE = new LEDColor(new SLColor(0, 0, 204)); - public static final LEDColor DARK_GRAY = new LEDColor(new SLColor(64, 64, 64)); - public static final LEDColor DARK_GREEN = new LEDColor(new SLColor(0, 153, 0)); - public static final LEDColor DARK_RED = new LEDColor(new SLColor(204, 0, 0)); - public static final LEDColor GOLD = new LEDColor(new SLColor(218,165,32)); - public static final LEDColor GRAY = new LEDColor(new SLColor(128, 128, 128)); - public static final LEDColor GREEN = new LEDColor(new SLColor(0, 255, 0)); - public static final LEDColor HOT_PINK = new LEDColor(new SLColor(255, 105, 180)); - public static final LEDColor LAWN_GREEN = new LEDColor(new SLColor(102, 204, 0)); - public static final LEDColor LIME = new LEDColor(new SLColor(191, 255, 0)); - public static final LEDColor ORANGE = new LEDColor(new SLColor(255, 128, 0)); - public static final LEDColor PINK = new LEDColor(new SLColor(255, 192, 203)); - public static final LEDColor PURPLE = new LEDColor(new SLColor(160, 32, 240)); - public static final LEDColor RED = new LEDColor(new SLColor(255, 0 , 0)); - public static final LEDColor RED_ORANGE = new LEDColor(new SLColor(255, 83, 73)); - public static final LEDColor VIOLET = new LEDColor(new SLColor(127, 0, 255)); - public static final LEDColor WHITE = new LEDColor(new SLColor(255, 255, 255)); - public static final LEDColor YELLOW = new LEDColor(new SLColor(255, 255, 0)); + public static final LEDColor AQUA = new LEDColor(getAWTColor(0, 255, 255)); + public static final LEDColor BLACK = new LEDColor(getAWTColor(0, 0, 0)); + public static final LEDColor BLUE = new LEDColor(getAWTColor(0, 128, 255)); + public static final LEDColor BLUE_GREEN = new LEDColor(getAWTColor(0, 255, 128)); + public static final LEDColor BLUE_VIOLET = new LEDColor(getAWTColor(51, 51, 255)); + public static final LEDColor DARK_BLUE = new LEDColor(getAWTColor(0, 0, 204)); + public static final LEDColor DARK_GRAY = new LEDColor(getAWTColor(64, 64, 64)); + public static final LEDColor DARK_GREEN = new LEDColor(getAWTColor(0, 153, 0)); + public static final LEDColor DARK_RED = new LEDColor(getAWTColor(204, 0, 0)); + public static final LEDColor GOLD = new LEDColor(getAWTColor(218, 165, 32)); + public static final LEDColor GRAY = new LEDColor(getAWTColor(128, 128, 128)); + public static final LEDColor GREEN = new LEDColor(getAWTColor(0, 255, 0)); + public static final LEDColor HOT_PINK = new LEDColor(getAWTColor(255, 105, 180)); + public static final LEDColor LAWN_GREEN = new LEDColor(getAWTColor(102, 204, 0)); + public static final LEDColor LIME = new LEDColor(getAWTColor(191, 255, 0)); + public static final LEDColor ORANGE = new LEDColor(getAWTColor(255, 128, 0)); + public static final LEDColor PINK = new LEDColor(getAWTColor(255, 192, 203)); + public static final LEDColor PURPLE = new LEDColor(getAWTColor(160, 32, 240)); + public static final LEDColor RED = new LEDColor(getAWTColor(255, 0 , 0)); + public static final LEDColor RED_ORANGE = new LEDColor(getAWTColor(255, 83, 73)); + public static final LEDColor VIOLET = new LEDColor(getAWTColor(127, 0, 255)); + public static final LEDColor WHITE = new LEDColor(getAWTColor(255, 255, 255)); + public static final LEDColor YELLOW = new LEDColor(getAWTColor(255, 255, 0)); - public static final LEDColor OFF = new LEDColor(new SLColor(0, 0, 0)); + public static final LEDColor OFF = new LEDColor(getAWTColor(0, 0, 0)); public static final LEDInstruction RAINBOW = new LEDRainbow(); - public static final LEDInstruction PULSE_RED = new LEDPulseColor(RED.getSLColor()); - public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(RED.getSLColor(), BLUE.getSLColor()); - + public static final LEDInstruction PULSE_RED = new LEDPulseColor(RED.getAWTColor()); + public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(RED.getAWTColor(), BLUE.getAWTColor()); + public static final LEDInstruction BANGLADESH = new LEDSection(new Color[] {RED.getAWTColor(), BLACK.getAWTColor(), DARK_GREEN.getAWTColor()}); } diff --git a/src/main/java/com/stuypulse/robot/util/SLColor.java b/src/main/java/com/stuypulse/robot/util/SLColor.java deleted file mode 100644 index 1a9242e7..00000000 --- a/src/main/java/com/stuypulse/robot/util/SLColor.java +++ /dev/null @@ -1,25 +0,0 @@ -package com.stuypulse.robot.util; - -public class SLColor { - private final int red; - private final int green; - private final int blue; - - public SLColor(int red, int green, int blue) { - this.red = red; - this.green = green; - this.blue = blue; - } - - public int getRed() { - return red; - } - - public int getGreen() { - return green; - } - - public int getBlue() { - return blue; - } -} From 235225eabd903215fcc53dd218e434856e63123b Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Wed, 13 Dec 2023 17:08:53 -0500 Subject: [PATCH 36/40] Fix errors in LEDInstruction implementations --- .../robot/subsystems/leds/LEDController.java | 2 +- .../robot/subsystems/leds/LEDPulseColor.java | 12 +++---- .../robot/subsystems/leds/LEDRainbow.java | 34 +++++++++---------- .../robot/subsystems/leds/RichieMode.java | 6 ++-- .../com/stuypulse/robot/util/LEDColor.java | 8 ++--- 5 files changed, 30 insertions(+), 32 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index bf3eac6e..580b5f7f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -41,7 +41,7 @@ public static LEDController getInstance() { public LEDInstruction getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PURPLE; + case CUBE: return LEDColor.RAINBOW; case CONE_TIP_IN: return LEDColor.YELLOW; case CONE_TIP_UP: return LEDColor.GREEN; case CONE_TIP_OUT: return LEDColor.ORANGE; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java index 703b43d4..94b32e80 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java @@ -21,17 +21,17 @@ public class LEDPulseColor implements LEDInstruction { public Color altcolor; public StopWatch stopwatch; - public LEDPulseColor(Color color) { - this(color, new Color(0,0,0)); - } - public LEDPulseColor(Color color1, Color color2) { this.color = color1; this.altcolor = color2; - stopwatch = new StopWatch(); - + stopwatch = new StopWatch(); } + public LEDPulseColor(Color color) { + this(color, new Color(0,0,0)); + } + + @Override public void setLED(AddressableLEDBuffer ledsBuffer) { double time = stopwatch.getTime(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index c81a834e..8a967dbf 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -1,9 +1,9 @@ package com.stuypulse.robot.subsystems.leds; -import com.stuypulse.robot.util.SLColor; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import java.awt.Color; /*- * Contains: * - setLED() : sets LEDs to rainbow colors @@ -15,21 +15,21 @@ public class LEDRainbow implements LEDInstruction { StopWatch timer; int counter; - private static SLColor[] rainbow = new SLColor[]{ - new SLColor (218,165,32), - new SLColor(255, 83, 73), - new SLColor(255, 128, 0), - new SLColor(255, 255, 0), - new SLColor(102, 204, 0), - new SLColor(0, 255, 0), - new SLColor(0, 153, 0), - new SLColor(0, 255, 128), - new SLColor(0, 128, 255), - new SLColor(0, 0, 204), - new SLColor(51, 51, 255), - new SLColor(127, 0, 255), - new SLColor(160, 32, 240), - new SLColor(255, 192, 203) + private static Color[] rainbow = new Color[]{ + new Color(218,165,32), + new Color(255, 83, 73), + new Color(255, 128, 0), + new Color(255, 255, 0), + new Color(102, 204, 0), + new Color(0, 255, 0), + new Color(0, 153, 0), + new Color(0, 255, 128), + new Color(0, 128, 255), + new Color(0, 0, 204), + new Color(51, 51, 255), + new Color(127, 0, 255), + new Color(160, 32, 240), + new Color(255, 192, 203) }; public LEDRainbow() { @@ -41,7 +41,7 @@ public LEDRainbow() { public void setLED(AddressableLEDBuffer ledsBuffer) { // if(timer.getTime() > 3 * Math.sin(colorCounter * 2.0) + 1) { - SLColor color; + Color color; if(timer.getTime() > 1.0) { timer.reset(); for(int i = 0; i < ledsBuffer.getLength(); i++) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java index 764885c1..b4fa4438 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java @@ -1,6 +1,6 @@ package com.stuypulse.robot.subsystems.leds; -import com.stuypulse.robot.util.SLColor; +import java.awt.Color; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -16,12 +16,12 @@ */ public class RichieMode implements LEDInstruction { - public SLColor color; + public Color color; private StopWatch stopwatch; private int index; private int prevIndex; - public RichieMode(SLColor color) { + public RichieMode(Color color) { this.color = color; stopwatch = new StopWatch(); } diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index 723f6e70..2b79c6bc 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.subsystems.leds.LEDPulseColor; import com.stuypulse.robot.subsystems.leds.LEDRainbow; import com.stuypulse.robot.subsystems.leds.LEDSection; +import com.stuypulse.robot.subsystems.leds.RichieMode; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import java.awt.Color; @@ -35,10 +36,6 @@ public LEDColor(int red, int green, int blue) { this.blue = blue; } - public LEDColor(SLColor color) { - this(color.getRed(), color.getGreen(), color.getBlue()); - } - public LEDColor(Color color) { this(color.getRed(), color.getGreen(), color.getBlue()); } @@ -60,7 +57,7 @@ public static Color getAWTColor(int red, int green, int blue) { } public Color getAWTColor() { - return new Color(red, green, blue); + return new Color(this.red, this.green, this.blue); } @Override @@ -104,6 +101,7 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { public static final LEDInstruction RAINBOW = new LEDRainbow(); public static final LEDInstruction PULSE_RED = new LEDPulseColor(RED.getAWTColor()); public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(RED.getAWTColor(), BLUE.getAWTColor()); + public static final LEDInstruction RICHIE = new RichieMode(RED.getAWTColor()); public static final LEDInstruction BANGLADESH = new LEDSection(new Color[] {RED.getAWTColor(), BLACK.getAWTColor(), DARK_GREEN.getAWTColor()}); } From 2c9f6f48ed16f7606c66d044ffbe5a67c740a75f Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Wed, 13 Dec 2023 23:54:25 -0500 Subject: [PATCH 37/40] INSANELY SMOOTH LEDRAINBOW --- .../robot/subsystems/leds/LEDRainbow.java | 44 +++---------------- 1 file changed, 6 insertions(+), 38 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index 8a967dbf..d7563634 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -3,53 +3,21 @@ import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import java.awt.Color; /*- * Contains: * - setLED() : sets LEDs to rainbow colors * @author Richie Xue - * @author Jo Walkup - * @author Naowal Rahman */ public class LEDRainbow implements LEDInstruction { - StopWatch timer; - int counter; - - private static Color[] rainbow = new Color[]{ - new Color(218,165,32), - new Color(255, 83, 73), - new Color(255, 128, 0), - new Color(255, 255, 0), - new Color(102, 204, 0), - new Color(0, 255, 0), - new Color(0, 153, 0), - new Color(0, 255, 128), - new Color(0, 128, 255), - new Color(0, 0, 204), - new Color(51, 51, 255), - new Color(127, 0, 255), - new Color(160, 32, 240), - new Color(255, 192, 203) - }; - - public LEDRainbow() { - timer = new StopWatch(); - counter = 0; - } + private int m_rainbowFirstPixelHue = 0; @Override public void setLED(AddressableLEDBuffer ledsBuffer) { - - // if(timer.getTime() > 3 * Math.sin(colorCounter * 2.0) + 1) { - Color color; - if(timer.getTime() > 1.0) { - timer.reset(); - for(int i = 0; i < ledsBuffer.getLength(); i++) { - color = rainbow[(i + counter) % rainbow.length]; - ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); - ++counter; - if(counter == rainbow.length) counter = 0; - } + for (int i = 0; i < ledsBuffer.getLength(); i++) { + final int hue = (m_rainbowFirstPixelHue + (i * 180 / ledsBuffer.getLength())) % 180; + ledsBuffer.setHSV(i, hue, 255, 128); } + m_rainbowFirstPixelHue += 3; + m_rainbowFirstPixelHue %= 180; } } \ No newline at end of file From fc851c511b2ed762cef4245620c2d4e21caed748 Mon Sep 17 00:00:00 2001 From: Richie_Xue Date: Fri, 15 Dec 2023 23:11:41 -0500 Subject: [PATCH 38/40] Greatly simplified LEDController subsystem --- .../com/stuypulse/robot/RobotContainer.java | 4 - .../robot/subsystems/leds/LEDController.java | 37 ++++-- .../subsystems/leds/LEDControllerImpl.java | 45 ------- .../robot/subsystems/leds/LEDVisualizer.java | 121 ------------------ .../robot/subsystems/leds/SimLED.java | 99 -------------- 5 files changed, 27 insertions(+), 279 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java delete mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9884506b..7003e134 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,10 +13,6 @@ import com.stuypulse.robot.commands.arm.*; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.auton.*; -import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceBumpBlue; -import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceBumpRed; -import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockBumpBlue; -import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockBumpRed; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.manager.*; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 580b5f7f..2f92cb30 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -7,8 +7,15 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.MatchState; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.Manager; import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; /*- @@ -21,23 +28,34 @@ * @author Richie Xue * @author Jo Walkup */ -public abstract class LEDController extends SubsystemBase { +public class LEDController extends SubsystemBase { // singleton - private static LEDController instance; + private static LEDController instance = new LEDController(); - static { - instance = new LEDControllerImpl(); - } - public static LEDController getInstance() { return instance; } - // LEDVisualizer - LEDVisualizer visualizer = new LEDVisualizer(); + private AddressableLED leds; + private AddressableLEDBuffer ledsBuffer; + + public LEDController() { + leds = new AddressableLED(Ports.LEDController.PORT); + ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); + + leds.setLength(ledsBuffer.getLength()); + leds.setData(ledsBuffer); + leds.start(); - public abstract void forceSetLED(LEDInstruction instruction); + SmartDashboard.putData(instance); + } + + + public void forceSetLED(LEDInstruction instruction) { + instruction.setLED(ledsBuffer); + leds.setData(ledsBuffer); + } public LEDInstruction getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { @@ -54,6 +72,5 @@ public void periodic() { if (Robot.getMatchState() == MatchState.TELEOP) { forceSetLED(getDefaultColor()); } - visualizer.setColor(LEDColor.BLUE, 10); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java deleted file mode 100644 index 73579afa..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDControllerImpl.java +++ /dev/null @@ -1,45 +0,0 @@ -/************************ PROJECT DORCAS ************************/ -/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved. */ -/* This work is licensed under the terms of the MIT license. */ -/****************************************************************/ - -package com.stuypulse.robot.subsystems.leds; - -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -/*- - * Contains: - * - setColor() : sets color of LEDs for short time - * - getDefaultColor() : determines LED color if it is not set - * -* @author Sam Belliveau - * @author Andrew Liu - * @author Richie Xue - * @author Jo Walkup - */ -public class LEDControllerImpl extends LEDController { - - // Motor that controlls the LEDs - private AddressableLED leds; - private AddressableLEDBuffer ledsBuffer; - - public LEDControllerImpl() { - leds = new AddressableLED(Ports.LEDController.PORT); - ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); // get length of led strip ? - - // set data - leds.setLength(ledsBuffer.getLength()); - leds.setData(ledsBuffer); - leds.start(); - } - - @Override - public void forceSetLED(LEDInstruction instruction) { - instruction.setLED(ledsBuffer); - leds.setData(ledsBuffer); - } - -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java deleted file mode 100644 index 1af314f6..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDVisualizer.java +++ /dev/null @@ -1,121 +0,0 @@ -package com.stuypulse.robot.subsystems.leds; - -import com.stuypulse.stuylib.util.StopWatch; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.Robot.MatchState; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.Manager; -import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -public class LEDVisualizer { - // Simulation Constants - private final double WINDOW_WIDTH = 55; - private final double WINDOW_HEIGHT = 10; - private final double WINDOW_X_PADDING = 0.5; - private final double LED_WIDTH = 15; - - // Mechanism2ds that displays the LEDs - private final Mechanism2d sim; - private final MechanismRoot2d startRoot; - private final MechanismLigament2d[] leds; - private final AddressableLEDBuffer ledsBuffer; - - // Stopwatch to check when to start overriding manual updates - private StopWatch lastUpdate; - private double manualTime; - - // The current color to set the LEDs to - private LEDColor manualColor; - - protected LEDVisualizer() { - sim = new Mechanism2d(WINDOW_WIDTH, WINDOW_HEIGHT); - leds = new MechanismLigament2d[Settings.LED.LED_LENGTH]; - ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); - - // Create a row of squares to represent the LEDs - for (int i = 0; i < Settings.LED.LED_LENGTH; i++) { - leds[i] = new MechanismLigament2d("LED " + i, WINDOW_X_PADDING + i, 0, LED_WIDTH, new Color8Bit(0, 0, 0)); - } - - // Set the root of the Mechanism2d to be the center of the row of squares - startRoot = sim.getRoot("Led Root", 0, WINDOW_HEIGHT / 2); - - // Append the squares onto each other - for (int i = 0; i < Settings.LED.LED_LENGTH; i++) { - startRoot.append(leds[i]); - } - - // Set the LEDs to be off - forceSetLED(LEDColor.BLACK); - - // Initialize the stopwatch - lastUpdate = new StopWatch(); - - SmartDashboard.putData("SimLED", sim); - - } - - public void setColor(LEDColor color, double time) { - manualColor = color; - manualTime = time; - lastUpdate.reset(); - } - - private void forceSetLED(LEDColor ledColor) { - for (int i = 0; i < ledsBuffer.getLength(); i++) { - ledsBuffer.setRGB(i, ledColor.getRed(), ledColor.getGreen(), ledColor.getBlue()); - } - } - - public void forceSetLED(LEDInstruction instruction) { - // for (int i = 0; i < ledsBuffer.getLength(); i++) { - // ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); - // } - // leds.setData(ledsBuffer); - instruction.setLED(ledsBuffer); - } - - public void setLEDConditions() { - } - - public LEDColor getDefaultColor() { - switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PURPLE; - case CONE_TIP_IN: return LEDColor.YELLOW; - case CONE_TIP_UP: return LEDColor.GREEN; - case CONE_TIP_OUT: return LEDColor.ORANGE; - default: return LEDColor.OFF; - } - } - - public void periodic() { - // If we called .setColor() recently, use that value - if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { - forceSetLED(manualColor); - SmartDashboard.putNumber("LED Mech2d/LED 0R", manualColor.getRed()); - SmartDashboard.putNumber("LED Mech2d/LED 0G", manualColor.getGreen()); - SmartDashboard.putNumber("LED Mech2d/LED 0B", manualColor.getBlue()); - } - - // Otherwise use the default color - else { - //forceSetLED(getDefaultColor()); - forceSetLED(LEDColor.RAINBOW); - } - - SmartDashboard.putNumber("LED Mech2d/LED Time", lastUpdate.getTime()); - } -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java b/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java deleted file mode 100644 index d4314549..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/SimLED.java +++ /dev/null @@ -1,99 +0,0 @@ -package com.stuypulse.robot.subsystems.leds; - -import com.stuypulse.stuylib.util.StopWatch; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.Robot.MatchState; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.Manager; -import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -public class SimLED extends LEDController{ - private static SimLED simulation; - - static { - simulation = new SimLED(); - } - - //@Override - public static SimLED getInstance() { - return simulation; - } - - // Motor that controlls the LEDs - private MechanismRoot2d leds; - private MechanismRoot2d ledsBuffer; - - // Stopwatch to check when to start overriding manual updates - private StopWatch lastUpdate; - private double manualTime; - - // The current color to set the LEDs to - private LEDColor manualColor; - - protected SimLED() { - // leds = new MechanismRoot2d("leds", Ports.LEDController.PORT, Ports.LEDController.PORT); - // ledsBuffer = new MechanismRoot2d("ledsBuffer", Settings.LED.LED_LENGTH, Settings.LED.LED_LENGTH); // get length of led strip ? - - // // set data - // leds.setLength(ledsBuffer.getLength()); - // leds.setData(ledsBuffer); - // leds.start(); - - this.lastUpdate = new StopWatch(); - } - - public void setColor(LEDColor color, double time) { - manualColor = color; - manualTime = time; - lastUpdate.reset(); - } - - private void forceSetLEDs(LEDColor color) { - // for (int i = 0; i < ledsBuffer.getLength(); i++) { - // ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); - // } - // leds.setData(ledsBuffer); - } - - public void setLEDConditions() { - } - - public LEDColor getDefaultColor() { - switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PURPLE; - case CONE_TIP_IN: return LEDColor.YELLOW; - case CONE_TIP_UP: return LEDColor.GREEN; - case CONE_TIP_OUT: return LEDColor.ORANGE; - default: return LEDColor.OFF; - } - } - - @Override - public void periodic() { - // If we called .setColor() recently, use that value - if (Robot.getMatchState() == MatchState.AUTO || lastUpdate.getTime() < manualTime) { - forceSetLEDs(manualColor); - } - - // Otherwise use the default color - else { - forceSetLEDs(getDefaultColor()); - } - } - - @Override - public void forceSetLED(LEDInstruction instruction) { - // TODO Auto-generated method stub - - } -} From 6d067159270ce17ca2afd3dd422d685534e8a2bd Mon Sep 17 00:00:00 2001 From: Richie_Xue Date: Sat, 23 Dec 2023 16:17:21 -0500 Subject: [PATCH 39/40] Revamped LED subsystem - SLColor (to be added to StuyLib) as util color class - constants/LEDColor to store color consts - LEDSingleColor instead for single color LEDS --- .../stuypulse/robot/constants/LEDColor.java | 49 +++++++ .../robot/subsystems/leds/LEDController.java | 5 +- .../robot/subsystems/leds/LEDPulseColor.java | 12 +- .../robot/subsystems/leds/LEDRainbow.java | 2 - .../robot/subsystems/leds/LEDSection.java | 8 +- .../robot/subsystems/leds/LEDSingleColor.java | 19 +++ .../robot/subsystems/leds/RichieMode.java | 6 +- .../com/stuypulse/robot/util/LEDColor.java | 12 +- .../com/stuypulse/robot/util/SLColor.java | 132 ++++++++++++++++++ 9 files changed, 219 insertions(+), 26 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/constants/LEDColor.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/leds/LEDSingleColor.java create mode 100644 src/main/java/com/stuypulse/robot/util/SLColor.java diff --git a/src/main/java/com/stuypulse/robot/constants/LEDColor.java b/src/main/java/com/stuypulse/robot/constants/LEDColor.java new file mode 100644 index 00000000..9e611e33 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/LEDColor.java @@ -0,0 +1,49 @@ +package com.stuypulse.robot.constants; + +import com.stuypulse.robot.subsystems.leds.LEDInstruction; +import com.stuypulse.robot.subsystems.leds.LEDPulseColor; +import com.stuypulse.robot.subsystems.leds.LEDRainbow; +import com.stuypulse.robot.subsystems.leds.LEDSection; +import com.stuypulse.robot.subsystems.leds.LEDSingleColor; +import com.stuypulse.robot.subsystems.leds.LEDInstruction; +import com.stuypulse.robot.subsystems.leds.RichieMode; +import com.stuypulse.robot.util.SLColor; + +public interface LEDColor { + /***********************/ + /*** COLOR CONSTANTS ***/ + /***********************/ + + public static final LEDInstruction AQUA = new LEDSingleColor(new SLColor(0, 255, 255)); + public static final LEDInstruction BLACK = new LEDSingleColor(new SLColor(0, 0, 0)); + public static final LEDInstruction BLUE = new LEDSingleColor(new SLColor(0, 128, 255)); + public static final LEDInstruction BLUE_GREEN = new LEDSingleColor(new SLColor(0, 255, 128)); + public static final LEDInstruction BLUE_VIOLET = new LEDSingleColor(new SLColor(51, 51, 255)); + public static final LEDInstruction DARK_BLUE = new LEDSingleColor(new SLColor(0, 0, 204)); + public static final LEDInstruction DARK_GRAY = new LEDSingleColor(new SLColor(64, 64, 64)); + public static final LEDInstruction DARK_GREEN = new LEDSingleColor(new SLColor(0, 153, 0)); + public static final LEDInstruction DARK_RED = new LEDSingleColor(new SLColor(204, 0, 0)); + public static final LEDInstruction GOLD = new LEDSingleColor(new SLColor(218, 165, 32)); + public static final LEDInstruction GRAY = new LEDSingleColor(new SLColor(128, 128, 128)); + public static final LEDInstruction GREEN = new LEDSingleColor(new SLColor(0, 255, 0)); + public static final LEDInstruction HOT_PINK = new LEDSingleColor(new SLColor(255, 105, 180)); + public static final LEDInstruction LAWN_GREEN = new LEDSingleColor(new SLColor(102, 204, 0)); + public static final LEDInstruction LIME = new LEDSingleColor(new SLColor(191, 255, 0)); + public static final LEDInstruction ORANGE = new LEDSingleColor(new SLColor(255, 128, 0)); + public static final LEDInstruction PINK = new LEDSingleColor(new SLColor(255, 192, 203)); + public static final LEDInstruction PURPLE = new LEDSingleColor(new SLColor(160, 32, 240)); + public static final LEDInstruction RED = new LEDSingleColor(new SLColor(255, 0 , 0)); + public static final LEDInstruction RED_ORANGE = new LEDSingleColor(new SLColor(255, 83, 73)); + public static final LEDInstruction VIOLET = new LEDSingleColor(new SLColor(127, 0, 255)); + public static final LEDInstruction WHITE = new LEDSingleColor(new SLColor(255, 255, 255)); + public static final LEDInstruction YELLOW = new LEDSingleColor(new SLColor(255, 255, 0)); + + public static final LEDInstruction OFF = new LEDSingleColor(new SLColor(0, 0, 0)); + + public static final LEDInstruction RAINBOW = new LEDRainbow(); + public static final LEDInstruction PULSE_RED = new LEDPulseColor(SLColor.RED); + public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(SLColor.RED, SLColor.BLUE); + public static final LEDInstruction RICHIE = new RichieMode(SLColor.RED); + public static final LEDInstruction BANGLADESH = new LEDSection(new SLColor[] {SLColor.RED, SLColor.BLACK, SLColor.DARK_GREEN}); + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 2f92cb30..d8ad205e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -10,9 +10,8 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.Manager; -import com.stuypulse.robot.util.LEDColor; +import com.stuypulse.robot.constants.LEDColor; -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -59,7 +58,7 @@ public void forceSetLED(LEDInstruction instruction) { public LEDInstruction getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.RAINBOW; + case CUBE: return LEDColor.RED; case CONE_TIP_IN: return LEDColor.YELLOW; case CONE_TIP_UP: return LEDColor.GREEN; case CONE_TIP_OUT: return LEDColor.ORANGE; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java index 94b32e80..a91f57d1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java @@ -1,9 +1,9 @@ package com.stuypulse.robot.subsystems.leds; +import com.stuypulse.robot.util.SLColor; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import java.awt.Color; /** * Class that sets colour of pulsing LEDS on LEDController @@ -17,18 +17,18 @@ */ public class LEDPulseColor implements LEDInstruction { - public Color color; - public Color altcolor; + public SLColor color; + public SLColor altcolor; public StopWatch stopwatch; - public LEDPulseColor(Color color1, Color color2) { + public LEDPulseColor(SLColor color1, SLColor color2) { this.color = color1; this.altcolor = color2; stopwatch = new StopWatch(); } - public LEDPulseColor(Color color) { - this(color, new Color(0,0,0)); + public LEDPulseColor(SLColor color) { + this(color, SLColor.BLACK); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java index d7563634..9a80d96b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -1,7 +1,5 @@ package com.stuypulse.robot.subsystems.leds; -import com.stuypulse.stuylib.util.StopWatch; - import edu.wpi.first.wpilibj.AddressableLEDBuffer; /*- * Contains: diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java index 6d62169d..ba749b77 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSection.java @@ -1,15 +1,15 @@ package com.stuypulse.robot.subsystems.leds; -import java.awt.Color; -import com.stuypulse.stuylib.util.StopWatch; + +import com.stuypulse.robot.util.SLColor; import edu.wpi.first.wpilibj.AddressableLEDBuffer; public class LEDSection implements LEDInstruction { - public Color[] sections; + public SLColor[] sections; - public LEDSection(Color[] sections) { + public LEDSection(SLColor[] sections) { this.sections = sections; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSingleColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSingleColor.java new file mode 100644 index 00000000..007a51c3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDSingleColor.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import com.stuypulse.robot.util.SLColor; + +public class LEDSingleColor implements LEDInstruction { + SLColor color; + + public LEDSingleColor(SLColor color) { + this.color = color; + } + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, color.getRed(), color.getGreen(), color.getBlue()); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java index b4fa4438..764885c1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java @@ -1,6 +1,6 @@ package com.stuypulse.robot.subsystems.leds; -import java.awt.Color; +import com.stuypulse.robot.util.SLColor; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -16,12 +16,12 @@ */ public class RichieMode implements LEDInstruction { - public Color color; + public SLColor color; private StopWatch stopwatch; private int index; private int prevIndex; - public RichieMode(Color color) { + public RichieMode(SLColor color) { this.color = color; stopwatch = new StopWatch(); } diff --git a/src/main/java/com/stuypulse/robot/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index 2b79c6bc..a6b5429c 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -5,10 +5,7 @@ package com.stuypulse.robot.util; import com.stuypulse.robot.subsystems.leds.LEDInstruction; -import com.stuypulse.robot.subsystems.leds.LEDPulseColor; import com.stuypulse.robot.subsystems.leds.LEDRainbow; -import com.stuypulse.robot.subsystems.leds.LEDSection; -import com.stuypulse.robot.subsystems.leds.RichieMode; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import java.awt.Color; @@ -99,9 +96,8 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { public static final LEDColor OFF = new LEDColor(getAWTColor(0, 0, 0)); public static final LEDInstruction RAINBOW = new LEDRainbow(); - public static final LEDInstruction PULSE_RED = new LEDPulseColor(RED.getAWTColor()); - public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(RED.getAWTColor(), BLUE.getAWTColor()); - public static final LEDInstruction RICHIE = new RichieMode(RED.getAWTColor()); - public static final LEDInstruction BANGLADESH = new LEDSection(new Color[] {RED.getAWTColor(), BLACK.getAWTColor(), DARK_GREEN.getAWTColor()}); - + // public static final LEDInstruction PULSE_RED = new LEDPulseColor(RED.getAWTColor()); + // public static final LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(RED.getAWTColor(), BLUE.getAWTColor()); + // public static final LEDInstruction RICHIE = new RichieMode(RED.getAWTColor()); + // public static final LEDInstruction BANGLADESH = new LEDSection(new Color[] {RED.getAWTColor(), BLACK.getAWTColor(), DARK_GREEN.getAWTColor()}); } diff --git a/src/main/java/com/stuypulse/robot/util/SLColor.java b/src/main/java/com/stuypulse/robot/util/SLColor.java new file mode 100644 index 00000000..047ffd90 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SLColor.java @@ -0,0 +1,132 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.util; +import edu.wpi.first.wpilibj.util.Color8Bit; +import java.awt.Color; +// class is also importing edu.wpi.first.wpilibj.util.Color8 implicitly + +import com.stuypulse.stuylib.math.SLMath; + +/** + * StuyLib wrapper class for colors that handles various Color classes used in FRC (java.awt.Color, wpilibj.util.Color, wpilibj.util.Color8Bit) + * @author Richie Xue + */ +public class SLColor { + + private final int red; + private final int green; + private final int blue; + + /** + * Create an LEDColor object using RGB values + * + * @param r the r value [0-255] + * @param g the g value [0-255] + * @param b the b value [0-255] + */ + public SLColor(int red, int green, int blue) { + this.red = (int) SLMath.clamp(red, 0, 255); + this.green = (int) SLMath.clamp(green, 0, 255); + this.blue = (int) SLMath.clamp(blue, 0, 255); + } + + /** + * Create an LEDColor object using java.awt.Color + * + * @param color The java.awt.Color object + */ + public SLColor(Color color) { + this(color.getRed(), color.getGreen(), color.getBlue()); + } + + /** + * Create an LEDColor object using edu.wpi.first.wpilibj.util.Color + * + * @param color The edu.wpi.first.wpilibj.util.Color object + */ + public SLColor(edu.wpi.first.wpilibj.util.Color color) { + this((int) color.red * 255, (int) color.green * 255, (int) color.green * 255); + } + + /** + * Create an LEDColor object using edu.wpi.first.wpilibj.util.Color8Bit + * + * @param color The edu.wpi.first.wpilibj.util.Color8Bit object + */ + public SLColor(Color8Bit color) { + this(color.red, color.blue, color.green); + } + + /** + * Gets the r value [0-255] + * + * @return the LED color's red value + */ + public int getRed() { + return red; + } + + /** + * Gets the g value [0-255] + * + * @return the LED color's green value + */ + public int getGreen() { + return green; + } + + /** + * Gets the b value [0-255] + * + * @return the LED color's blue value + */ + public int getBlue() { + return blue; + } + + /* Getters to convert LEDColor objects into other Color types */ + + public Color getAWTColor() { + return new Color(this.red, this.green, this.blue); + } + + public edu.wpi.first.wpilibj.util.Color getWPILibColor() { + return new edu.wpi.first.wpilibj.util.Color(this.red / 255.0, this.green / 255.0, this.blue / 255.0); + } + + public Color8Bit getColor8Bit() { + return new Color8Bit(red, green, blue); + } + + /***********************/ + /*** COLOR CONSTANTS ***/ + /***********************/ + + public static final SLColor AQUA = new SLColor(0, 255, 255); + public static final SLColor BLACK = new SLColor(0, 0, 0); + public static final SLColor BLUE = new SLColor(0, 128, 255); + public static final SLColor BLUE_GREEN = new SLColor(0, 255, 128); + public static final SLColor BLUE_VIOLET = new SLColor(51, 51, 255); + public static final SLColor DARK_BLUE = new SLColor(0, 0, 204); + public static final SLColor DARK_GRAY = new SLColor(64, 64, 64); + public static final SLColor DARK_GREEN = new SLColor(0, 153, 0); + public static final SLColor DARK_RED = new SLColor(204, 0, 0); + public static final SLColor GOLD = new SLColor(218, 165, 32); + public static final SLColor GRAY = new SLColor(128, 128, 128); + public static final SLColor GREEN = new SLColor(0, 255, 0); + public static final SLColor HOT_PINK = new SLColor(255, 105, 180); + public static final SLColor LAWN_GREEN = new SLColor(102, 204, 0); + public static final SLColor LIME = new SLColor(191, 255, 0); + public static final SLColor ORANGE = new SLColor(255, 128, 0); + public static final SLColor PINK = new SLColor(255, 192, 203); + public static final SLColor PURPLE = new SLColor(160, 32, 240); + public static final SLColor RED = new SLColor(255, 0 , 0); + public static final SLColor RED_ORANGE = new SLColor(255, 83, 73); + public static final SLColor VIOLET = new SLColor(127, 0, 255); + public static final SLColor WHITE = new SLColor(255, 255, 255); + public static final SLColor YELLOW = new SLColor(255, 255, 0); + +} From 70ceec7b3d87dc47586e9f920d7df24824a75bb3 Mon Sep 17 00:00:00 2001 From: Richie_Xue Date: Sat, 23 Dec 2023 18:50:30 -0500 Subject: [PATCH 40/40] Prep SLColor for StuyLib --- .../stuypulse/robot/constants/LEDColor.java | 1 - .../com/stuypulse/robot/util/SLColor.java | 67 +++++++++++++++---- 2 files changed, 53 insertions(+), 15 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/LEDColor.java b/src/main/java/com/stuypulse/robot/constants/LEDColor.java index 9e611e33..dfbb1181 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDColor.java @@ -5,7 +5,6 @@ import com.stuypulse.robot.subsystems.leds.LEDRainbow; import com.stuypulse.robot.subsystems.leds.LEDSection; import com.stuypulse.robot.subsystems.leds.LEDSingleColor; -import com.stuypulse.robot.subsystems.leds.LEDInstruction; import com.stuypulse.robot.subsystems.leds.RichieMode; import com.stuypulse.robot.util.SLColor; diff --git a/src/main/java/com/stuypulse/robot/util/SLColor.java b/src/main/java/com/stuypulse/robot/util/SLColor.java index 047ffd90..00e3976f 100644 --- a/src/main/java/com/stuypulse/robot/util/SLColor.java +++ b/src/main/java/com/stuypulse/robot/util/SLColor.java @@ -1,12 +1,10 @@ -/************************ PROJECT JIM *************************/ -/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ -/* This work is licensed under the terms of the MIT license. */ -/**************************************************************/ - package com.stuypulse.robot.util; + import edu.wpi.first.wpilibj.util.Color8Bit; +//import edu.wpi.first.wpilibj.util.Color8 implicitly in code + import java.awt.Color; -// class is also importing edu.wpi.first.wpilibj.util.Color8 implicitly +import java.util.Objects; import com.stuypulse.stuylib.math.SLMath; @@ -14,27 +12,28 @@ * StuyLib wrapper class for colors that handles various Color classes used in FRC (java.awt.Color, wpilibj.util.Color, wpilibj.util.Color8Bit) * @author Richie Xue */ -public class SLColor { +public class SLColor extends edu.wpi.first.wpilibj.util.Color { private final int red; private final int green; private final int blue; /** - * Create an LEDColor object using RGB values + * Constructs an LEDColor object from RGB values * * @param r the r value [0-255] * @param g the g value [0-255] * @param b the b value [0-255] */ public SLColor(int red, int green, int blue) { + super(red, green, blue); this.red = (int) SLMath.clamp(red, 0, 255); this.green = (int) SLMath.clamp(green, 0, 255); this.blue = (int) SLMath.clamp(blue, 0, 255); } /** - * Create an LEDColor object using java.awt.Color + * Constructs an LEDColor object from java.awt.Color objects * * @param color The java.awt.Color object */ @@ -43,7 +42,7 @@ public SLColor(Color color) { } /** - * Create an LEDColor object using edu.wpi.first.wpilibj.util.Color + * Constructs an LEDColor object from edu.wpi.first.wpilibj.util.Color objects * * @param color The edu.wpi.first.wpilibj.util.Color object */ @@ -52,7 +51,7 @@ public SLColor(edu.wpi.first.wpilibj.util.Color color) { } /** - * Create an LEDColor object using edu.wpi.first.wpilibj.util.Color8Bit + * Constructs an LEDColor object from edu.wpi.first.wpilibj.util.Color8Bit objects * * @param color The edu.wpi.first.wpilibj.util.Color8Bit object */ @@ -88,19 +87,59 @@ public int getBlue() { } /* Getters to convert LEDColor objects into other Color types */ - + + /** + * @return the SLColor object as the java.awt.Color object equivalent + */ public Color getAWTColor() { - return new Color(this.red, this.green, this.blue); + return new Color(red, green, blue); } + /** + * @return the SLColor object as the edu.wpi.first.wpilibj.util.Color object equivalent + */ public edu.wpi.first.wpilibj.util.Color getWPILibColor() { - return new edu.wpi.first.wpilibj.util.Color(this.red / 255.0, this.green / 255.0, this.blue / 255.0); + return new edu.wpi.first.wpilibj.util.Color(red / 255.0, green / 255.0, blue / 255.0); } + /** + * @return the SLColor object as the edu.wpi.first.wpilibj.util.Color8Bit object equivalent + */ public Color8Bit getColor8Bit() { return new Color8Bit(red, green, blue); } + @Override + public boolean equals(Object other) { + if (this == other) { + return true; + } + if (other == null || getClass() != other.getClass()) { + return false; + } + + SLColor color = (SLColor) other; + return Integer.compare(color.red, red) == 0 + && Integer.compare(color.green, green) == 0 + && Integer.compare(color.blue, blue) == 0; + } + + @Override + public int hashCode() { + return Objects.hash(red, green, blue); + } + + /** + * Return this color represented as a hex string. + * + * @return a string of the format #RRGGBB + */ + @Override + public String toString() { + return String.format( + "#%02X%02X%02X", (int) (red * 255), (int) (green * 255), (int) (blue * 255)); + } + /***********************/ /*** COLOR CONSTANTS ***/ /***********************/