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..2d09f50b --- /dev/null +++ b/src/main/deploy/pathplanner/3 Piece W Low.path @@ -0,0 +1,202 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8, + "y": 4.6 + }, + "prevControl": null, + "nextControl": { + "x": 3.1835287054512387, + "y": 4.6 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.187741628152297, + "y": 4.44 + }, + "prevControl": { + "x": 5.435011671065698, + "y": 4.44 + }, + "nextControl": { + "x": 6.396944039439681, + "y": 4.44 + }, + "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.4 + }, + "prevControl": { + "x": 6.501547629778108, + "y": 4.431296466187375 + }, + "nextControl": { + "x": 6.501547629778108, + "y": 4.431296466187375 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.1599999999999997, + "y": 4.6 + }, + "prevControl": { + "x": 3.4298747164225913, + "y": 4.6 + }, + "nextControl": { + "x": 3.4298747164225913, + "y": 4.6 + }, + "holonomicAngle": 6.54, + "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": 4.903826385233523, + "y": 4.751390831153475 + }, + "nextControl": { + "x": 6.7315776729384345, + "y": 3.419548684990858 + }, + "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.7999999999999998, + "y": 4.5 + }, + "prevControl": { + "x": 5.794373267068212, + "y": 4.905051768145495 + }, + "nextControl": { + "x": 5.794373267068212, + "y": 4.905051768145495 + }, + "holonomicAngle": 10.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.494791213358649, + "y": 5.540372759067196 + }, + "prevControl": { + "x": 6.738011797701916, + "y": 5.2881129538482865 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "isReversed": null, + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Main.java b/src/main/java/com/stuypulse/robot/Main.java index 8f5987ac..210730bf 100644 --- a/src/main/java/com/stuypulse/robot/Main.java +++ b/src/main/java/com/stuypulse/robot/Main.java @@ -5,6 +5,7 @@ package com.stuypulse.robot; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.RobotBase; diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index c3bb8f11..68a100bf 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -49,6 +49,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 90b0aa37..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.*; @@ -31,12 +27,14 @@ import com.stuypulse.robot.subsystems.Manager.*; 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.plant.*; import com.stuypulse.robot.subsystems.swerve.*; import com.stuypulse.robot.subsystems.vision.*; import com.stuypulse.robot.subsystems.wing.*; import com.stuypulse.robot.util.*; + import com.stuypulse.robot.util.BootlegXbox; import edu.wpi.first.cameraserver.CameraServer; @@ -236,6 +234,8 @@ private void configureOperatorBindings() { /**************/ public void configureAutons() { + autonChooser.setDefaultOption("Do Nothing", new DoNothing()); + /* autonChooser.addOption("Do Nothing", new DoNothing()); autonChooser.addOption("Mobility", new Mobility()); @@ -266,7 +266,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 a8417a04..be102868 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java @@ -12,7 +12,6 @@ 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; import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Alignment.Rotation; import com.stuypulse.robot.constants.Settings.Alignment.Translation; @@ -21,6 +20,7 @@ import com.stuypulse.robot.subsystems.Manager.NodeLevel; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.*; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.util.Derivative; @@ -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 a60fdbfb..20284962 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java @@ -10,7 +10,9 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; -import com.stuypulse.robot.subsystems.LEDController; +import com.stuypulse.robot.constants.ArmTrajectories; +import com.stuypulse.robot.constants.Settings; +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; @@ -33,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; @@ -48,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), @@ -91,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 @@ -109,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(); } @@ -119,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(); } } @@ -144,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/auton/OnePieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java index fda7ae96..37b65233 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java @@ -7,7 +7,6 @@ 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.*; @@ -16,7 +15,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.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -56,7 +54,6 @@ public OnePieceDock() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -65,14 +62,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(), @@ -82,7 +77,6 @@ public OnePieceDock() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), 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 8c8fc521..fb8a168e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -7,7 +7,6 @@ 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.*; @@ -16,7 +15,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.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -62,7 +60,7 @@ public OnePieceMobilityDock() { // score first piece addCommands( - new LEDSet(LEDColor.RED), + new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -71,14 +69,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(), @@ -89,15 +85,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 LEDSet(LEDColor.RAINBOW), - 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 a411639e..75da164b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java @@ -7,7 +7,6 @@ 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.*; @@ -15,7 +14,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.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -52,7 +50,6 @@ public OnePiecePickupDock() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -61,7 +58,6 @@ public OnePiecePickupDock() { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -70,8 +66,6 @@ public OnePiecePickupDock() { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative().withStop(), @@ -94,7 +88,6 @@ public OnePiecePickupDock() { // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Dock")) .fieldRelative().withStop(), @@ -104,7 +97,6 @@ public OnePiecePickupDock() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), new SwerveDriveBalanceBlay() .withMaxSpeed(0.7) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java index ed3bcad0..a4984314 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java @@ -17,6 +17,8 @@ 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.WaitCommand; @@ -53,7 +55,6 @@ public OnePiecePickupDockBump() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -71,8 +72,6 @@ public OnePiecePickupDockBump() { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative().withStop(), @@ -95,7 +94,6 @@ public OnePiecePickupDockBump() { // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Dock")) .fieldRelative().withStop(), @@ -105,7 +103,6 @@ public OnePiecePickupDockBump() { ); addCommands( - new LEDSet(LEDColor.RAINBOW), new SwerveDriveBalanceBlay() .withMaxSpeed(0.5) 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..2a0b0e1e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -0,0 +1,301 @@ +/************************ 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.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 ThreePieceWLow 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.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_UP), + new ManagerSetScoreSide(ScoreSide.BACK) + ); + + // score first piece + 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()) + .andThen(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)) + ); + + 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), + new ManagerSetScoreSide(ScoreSide.BACK), + + new LEDSet(LEDColor.RED), + + 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), + new IntakeDeacquire(), + new WaitCommand(INTAKE_DEACQUIRE_TIME), + new IntakeStop() + ); + + // 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/commands/auton/TwoPiece.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java index 189c8c7a..1be159ff 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java @@ -7,14 +7,13 @@ 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.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.WaitCommand; @@ -49,7 +48,6 @@ public TwoPiece () { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) @@ -58,7 +56,6 @@ public TwoPiece () { ); addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -67,8 +64,6 @@ public TwoPiece () { addCommands( new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelCommandGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative() @@ -95,8 +90,6 @@ public TwoPiece () { new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetScoreSide(ScoreSide.BACK), - new LEDSet(LEDColor.RED), - new SwerveDriveFollowTrajectory( paths.get("Score Piece")) .fieldRelative() @@ -112,7 +105,6 @@ public TwoPiece () { ); addCommands( - new LEDSet(LEDColor.RAINBOW), new SwerveDriveFollowTrajectory( paths.get("Back Away")) .fieldRelative() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpRed.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpRed.java index 512f642a..d4a348c8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpRed.java @@ -7,7 +7,6 @@ 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.*; @@ -22,7 +21,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; @@ -117,7 +115,6 @@ public BCTwoPieceDockBumpRed() { // score first piece addCommands( - new LEDSet(LEDColor.RED), new ArmReady() .withTolerance(7, 9) .setShoulderVelocityTolerance(25) @@ -126,14 +123,6 @@ public BCTwoPieceDockBumpRed() { ); addCommands( - arm.runOnce(() -> { - arm.setShoulderConstraints(Shoulder.TELEOP_MAX_VELOCITY, Shoulder.TELEOP_MAX_ACCELERATION); - arm.setWristConstraints(Wrist.TELEOP_MAX_VELOCITY, Wrist.TELEOP_MAX_ACCELERATION); - }) - ); - - addCommands( - new LEDSet(LEDColor.BLUE), new IntakeScore(), new WaitCommand(INTAKE_DEACQUIRE_TIME) ); @@ -141,8 +130,6 @@ public BCTwoPieceDockBumpRed() { // intake second piece addCommands( - new LEDSet(LEDColor.GREEN), - new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative().withStop(), @@ -177,10 +164,6 @@ public BCTwoPieceDockBumpRed() { new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetScoreSide(ScoreSide.BACK), - new LEDSet(LEDColor.RED), - - arm.runOnce(() -> arm.setShoulderVelocityFeedbackCutoff(10.0)), - new ParallelCommandGroup( new SwerveDriveFollowTrajectory( paths.get("Score Piece")) @@ -205,7 +188,6 @@ public BCTwoPieceDockBumpRed() { // dock and engage addCommands( - new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Dock")) .fieldRelative().withStop(), @@ -215,8 +197,6 @@ public BCTwoPieceDockBumpRed() { ); addCommands( - new LEDSet(LEDColor.GREEN), - new SwerveDriveBalanceBlay() .withMaxSpeed(0.75) .withTimeout(ENGAGE_TIME) 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 51e3adee..3d32bc7f 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java @@ -3,32 +3,33 @@ /* This work is licensed under the terms of the MIT license. */ /**************************************************************/ +/*- + * Contains: + * - commands for LEDs + * @author Richie Xue + * @author Jo Walkup + * @author Naowal Rahman + */ + package com.stuypulse.robot.commands.leds; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.LEDController; -import com.stuypulse.robot.util.LEDColor; - +//import com.stuypulse.robot.constants.Ports.LEDController; +import com.stuypulse.robot.subsystems.leds.*; 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) { + public LEDSet(LEDInstruction instruction) { this.controller = LEDController.getInstance(); - this.updateTime = updateTime; - this.color = color; - } - - public LEDSet(LEDColor color) { - this(color, Settings.LED.MANUAL_UPDATE_TIME); + this.instruction = instruction; } @Override public void initialize() { - controller.setColor(color, updateTime); + controller.forceSetLED(instruction); } } 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 ac6792ce..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 @@ -12,7 +12,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.MatchState; 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; @@ -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/constants/LEDColor.java b/src/main/java/com/stuypulse/robot/constants/LEDColor.java new file mode 100644 index 00000000..dfbb1181 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/LEDColor.java @@ -0,0 +1,48 @@ +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.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/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1db1cf5c..62c0179d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -32,13 +32,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 1d9f13b3..58944733 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/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java similarity index 51% rename from src/main/java/com/stuypulse/robot/subsystems/LEDController.java rename to src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index 772ed7e7..d8ad205e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -3,18 +3,18 @@ /* This work is licensed under the terms of the MIT license. */ /**************************************************************/ -package com.stuypulse.robot.subsystems; - -import com.stuypulse.stuylib.util.StopWatch; +package com.stuypulse.robot.subsystems.leds; 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.util.LEDColor; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.constants.LEDColor; 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; /*- @@ -24,80 +24,52 @@ * * @author Sam Belliveau * @author Andrew Liu + * @author Richie Xue + * @author Jo Walkup */ public class LEDController extends SubsystemBase { // singleton - private static LEDController instance; - - static { - instance = new LEDController(); - } - + private static LEDController instance = new LEDController(); + public static LEDController 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; - - // 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 ? + ledsBuffer = new AddressableLEDBuffer(Settings.LED.LED_LENGTH); - // set data leds.setLength(ledsBuffer.getLength()); leds.setData(ledsBuffer); leds.start(); - manualColor = LEDColor.OFF; - this.lastUpdate = new StopWatch(); + SmartDashboard.putData(instance); } - 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()); - } + public void forceSetLED(LEDInstruction instruction) { + instruction.setLED(ledsBuffer); leds.setData(ledsBuffer); } - private void setLEDConditions() { - } - - public LEDColor getDefaultColor() { + public LEDInstruction getDefaultColor() { switch (Manager.getInstance().getGamePiece()) { - case CUBE: return LEDColor.PURPLE; + 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; - default: return LEDColor.OFF; + default: return LEDColor.RED; } } @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()); + if (Robot.getMatchState() == MatchState.TELEOP) { + forceSetLED(getDefaultColor()); } } } 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..8dc8276e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDInstruction.java @@ -0,0 +1,13 @@ +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/LEDPulseColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java new file mode 100644 index 00000000..a91f57d1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDPulseColor.java @@ -0,0 +1,57 @@ +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 color1, SLColor color2) { + this.color = color1; + this.altcolor = color2; + stopwatch = new StopWatch(); + } + + public LEDPulseColor(SLColor color) { + this(color, SLColor.BLACK); + } + + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + 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 (time < 1){ + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, altcolor.getRed(), altcolor.getGreen(), altcolor.getBlue()); + } + } + + else { + stopwatch.reset(); + } + } + +} + 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..9a80d96b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDRainbow.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +/*- + * Contains: + * - setLED() : sets LEDs to rainbow colors + * @author Richie Xue + */ +public class LEDRainbow implements LEDInstruction { + private int m_rainbowFirstPixelHue = 0; + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + 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 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..ba749b77 --- /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 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()); + } + } + + } + +} 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 new file mode 100644 index 00000000..764885c1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/RichieMode.java @@ -0,0 +1,46 @@ +package com.stuypulse.robot.subsystems.leds; + +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 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/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 53ac7e6a..00793c6d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -57,13 +57,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/util/LEDColor.java b/src/main/java/com/stuypulse/robot/util/LEDColor.java index 562fc8bd..a6b5429c 100644 --- a/src/main/java/com/stuypulse/robot/util/LEDColor.java +++ b/src/main/java/com/stuypulse/robot/util/LEDColor.java @@ -4,9 +4,12 @@ /**************************************************************/ package com.stuypulse.robot.util; +import com.stuypulse.robot.subsystems.leds.LEDInstruction; +import com.stuypulse.robot.subsystems.leds.LEDRainbow; - - +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import java.awt.Color; + /** * Class that stores all of the different RGB values for the LED Controller. * @@ -14,18 +17,26 @@ * @author Andrew Liu * @author Reya Miller * @author Colyi Chen + * @author Richie Xue + * @author Jo Walkup + * @author Naowal Rahman + * @author Souvik Basak */ -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; } + public LEDColor(Color color) { + this(color.getRed(), color.getGreen(), color.getBlue()); + } + public int getRed() { return red; } @@ -38,41 +49,55 @@ public int getBlue() { return blue; } - public LEDColor setColor(int red, int green, int blue) { - return new LEDColor(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(this.red, this.green, this.blue); + } + + @Override + public void setLED(AddressableLEDBuffer ledsBuffer) { + for (int i = 0; i < ledsBuffer.getLength(); i++) { + ledsBuffer.setRGB(i, getRed(), getGreen(), getBlue()); + } } + /***********************/ /*** COLOR CONSTANTS ***/ /***********************/ + 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 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 RAINBOW = OFF; - + 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()}); } 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..00e3976f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SLColor.java @@ -0,0 +1,171 @@ +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; +import java.util.Objects; + +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 extends edu.wpi.first.wpilibj.util.Color { + + private final int red; + private final int green; + private final int blue; + + /** + * 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); + } + + /** + * Constructs an LEDColor object from java.awt.Color objects + * + * @param color The java.awt.Color object + */ + public SLColor(Color color) { + this(color.getRed(), color.getGreen(), color.getBlue()); + } + + /** + * Constructs an LEDColor object from edu.wpi.first.wpilibj.util.Color objects + * + * @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); + } + + /** + * Constructs an LEDColor object from edu.wpi.first.wpilibj.util.Color8Bit objects + * + * @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 */ + + /** + * @return the SLColor object as the java.awt.Color object equivalent + */ + public Color getAWTColor() { + 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(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 ***/ + /***********************/ + + 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); + +}