From 2ac5d5ed4b0dee7aed294657fa728d92a18b3d2d Mon Sep 17 00:00:00 2001 From: ethan Date: Mon, 25 Mar 2024 20:44:59 -0700 Subject: [PATCH 1/3] bogo sort --- src/main/java/frc/robot/StateManager.java | 10 ++- src/main/java/frc/robot/subsystems/LEDs.java | 84 +++++++++++++++++++- 2 files changed, 92 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/StateManager.java b/src/main/java/frc/robot/StateManager.java index 4130cc2..d2d00c1 100644 --- a/src/main/java/frc/robot/StateManager.java +++ b/src/main/java/frc/robot/StateManager.java @@ -1,5 +1,7 @@ package frc.robot; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.Climb; import frc.robot.subsystems.Index; @@ -73,7 +75,13 @@ public void logging() { public void periodic() { logging(); - if(photon.isAligned() && hasGamePiece) { + if(DriverStation.isDisabled()) { + do { + Timer.delay(.4); + } + while(!leds.BogoSort() || !DriverStation.isDisabled()); + } + else if(photon.isAligned() && hasGamePiece) { leds.setState(LEDState.ALLIGNED); } else if(hasGamePiece) { diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java index 46bee07..160dd0e 100644 --- a/src/main/java/frc/robot/subsystems/LEDs.java +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.Random; + import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -9,6 +11,8 @@ public class LEDs { // Store what the last hue of the first pixel is private int firstPixelHue; + private bogo[] bogoArray; + public enum Color { RED(0,255,128), GREEN(60,255,128), @@ -25,6 +29,35 @@ private Color (int h, int s, int v) { } } + public enum bogo { + RED1(0,255, 128, 0b111111110000000000000000), + RED2(0, 153, 128, 0b11111111110010101100110), + RED3(0, 51, 128, 0b111111110011001100110011), + ORANGE1(36, 255, 128, 0b001001001111111111111111), + ORANGE2(30, 153, 128, 0b111111111011001001100110), + ORANGE3(29, 51, 128, 0b111111111110010111001100), + YELLOW1(60, 255, 128, 0b111111111111111100000000), + YELLOW2(60, 153, 128, 0b111111111111111101100110), + YELLOW3(60, 51, 128, 0b111111111111111111001100), + GREEN1(120, 255, 128, 0b000000001111111100000000), + GREEN2(120, 153, 128, 0b011001101111111101100110), + GREEN3(120, 51, 128, 0b110011001111111111001100), + BLUE1(240, 255, 128, 0b000000000000000011111111), + BLUE2(240, 153, 128, 0b011001100110011011111111); + + int h; + int s; + int v; + int sorting; + + private bogo (int h, int s, int v, int sorting) { + this.h = h; + this.s = s; + this.v = v; + this.sorting = sorting; + } + } + public enum LEDState { ALLIGNED, HASPEICE, @@ -37,14 +70,41 @@ public LEDs() { LED = new AddressableLED(0); LEDBuffer = new AddressableLEDBuffer(27); LED.setLength(LEDBuffer.getLength()); + currentState = LEDState.NOPEICE; LED.setData(LEDBuffer); LED.start(); - } + bogoArray = new bogo[]{bogo.RED1, + bogo.RED2, + bogo.RED3, + bogo.ORANGE1, + bogo.ORANGE2, + bogo.ORANGE3, + bogo.YELLOW1, + bogo.YELLOW2, + bogo.YELLOW3, + bogo.GREEN1, + bogo.GREEN2, + bogo.GREEN3, + bogo.BLUE1, + bogo.BLUE2}; + ShuffleArray(bogoArray); + } + private void ShuffleArray(bogo[] ar) { + Random random = new Random(); + for (int i = ar.length - 1; i > 0; i--) + { + int index = random.nextInt(i + 1); + // Simple swap + bogo a = ar[index]; + ar[index] = ar[i]; + ar[i] = a; + } + } /** * makes the leds do a rainbow pattern */ @@ -61,6 +121,28 @@ public void Rainbow() { LED.start(); } + public boolean BogoSort() { + // check is sorted + boolean isSorted = true; + for(int i = 0; i < bogoArray.length - 1; i++) { + if(bogoArray[i].sorting > bogoArray[i+1].sorting) { + isSorted = false; + } + } + + // randomize if not sorted + if (!isSorted) { + ShuffleArray(bogoArray); + } + + // apply array + for (int i = 0; i < getBufferLength(); i++) { + setHSV(i, bogoArray[i].h, bogoArray[i].s, bogoArray[i].v); + } + + return isSorted; + } + /** * setting one of the leds a certian color in HS From cbae6f4fc9ce6bee366f853b4167faa206bf4a4f Mon Sep 17 00:00:00 2001 From: ethan Date: Tue, 26 Mar 2024 19:10:20 -0700 Subject: [PATCH 2/3] bogo wrk --- src/main/java/frc/robot/StateManager.java | 5 +- src/main/java/frc/robot/subsystems/LEDs.java | 58 +++++++++++++------- 2 files changed, 38 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/StateManager.java b/src/main/java/frc/robot/StateManager.java index d2d00c1..253b0e5 100644 --- a/src/main/java/frc/robot/StateManager.java +++ b/src/main/java/frc/robot/StateManager.java @@ -76,10 +76,7 @@ public void logging() { public void periodic() { logging(); if(DriverStation.isDisabled()) { - do { - Timer.delay(.4); - } - while(!leds.BogoSort() || !DriverStation.isDisabled()); + leds.setState(LEDState.BOGO); } else if(photon.isAligned() && hasGamePiece) { leds.setState(LEDState.ALLIGNED); diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java index 160dd0e..f3a7185 100644 --- a/src/main/java/frc/robot/subsystems/LEDs.java +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -8,6 +8,7 @@ public class LEDs { private AddressableLED LED ; private AddressableLEDBuffer LEDBuffer; + private int bogoCycle; // Store what the last hue of the first pixel is private int firstPixelHue; @@ -30,24 +31,24 @@ private Color (int h, int s, int v) { } public enum bogo { - RED1(0,255, 128, 0b111111110000000000000000), - RED2(0, 153, 128, 0b11111111110010101100110), - RED3(0, 51, 128, 0b111111110011001100110011), - ORANGE1(36, 255, 128, 0b001001001111111111111111), - ORANGE2(30, 153, 128, 0b111111111011001001100110), - ORANGE3(29, 51, 128, 0b111111111110010111001100), - YELLOW1(60, 255, 128, 0b111111111111111100000000), - YELLOW2(60, 153, 128, 0b111111111111111101100110), - YELLOW3(60, 51, 128, 0b111111111111111111001100), - GREEN1(120, 255, 128, 0b000000001111111100000000), - GREEN2(120, 153, 128, 0b011001101111111101100110), - GREEN3(120, 51, 128, 0b110011001111111111001100), - BLUE1(240, 255, 128, 0b000000000000000011111111), - BLUE2(240, 153, 128, 0b011001100110011011111111); - - int h; - int s; - int v; + RED1(0,255, 150, 1), + RED2(13, 255, 150, 2), + RED3(26, 255, 150, 3), + ORANGE1(39, 255, 150, 4), + ORANGE2(52, 255, 150, 5), + ORANGE3(65, 255, 150, 6), + YELLOW1(78, 255, 150, 7), + YELLOW2(91, 255, 150, 8), + YELLOW3(104, 255, 150, 9), + GREEN1(117, 255, 150, 10), + GREEN2(130, 255, 150, 11), + GREEN3(143, 255, 150, 12), + BLUE1(156, 255, 150, 13), + BLUE2(169, 255, 150, 14); + + int h; // 0-180 + int s; // 0-255 + int v; // 0-255 int sorting; private bogo (int h, int s, int v, int sorting) { @@ -59,6 +60,7 @@ private bogo (int h, int s, int v, int sorting) { } public enum LEDState { + BOGO, ALLIGNED, HASPEICE, NOPEICE; @@ -77,7 +79,8 @@ public LEDs() { LED.setData(LEDBuffer); LED.start(); - bogoArray = new bogo[]{bogo.RED1, + bogoArray = new bogo[]{ + bogo.RED1, bogo.RED2, bogo.RED3, bogo.ORANGE1, @@ -92,6 +95,8 @@ public LEDs() { bogo.BLUE1, bogo.BLUE2}; ShuffleArray(bogoArray); + + bogoCycle = 0; } private void ShuffleArray(bogo[] ar) { @@ -136,8 +141,12 @@ public boolean BogoSort() { } // apply array - for (int i = 0; i < getBufferLength(); i++) { - setHSV(i, bogoArray[i].h, bogoArray[i].s, bogoArray[i].v); + for (int i = 0; i < bogoArray.length; i++) { + setHSV(i*2, bogoArray[i].h, bogoArray[i].s, bogoArray[i].v); + + if (i < bogoArray.length-1) { + setHSV(i*2+1, bogoArray[i].h, bogoArray[i].s, bogoArray[i].v); + } } return isSorted; @@ -236,6 +245,13 @@ public LEDState getState() { public void periodic() { switch (currentState) { + case BOGO: + bogoCycle++; + if(bogoCycle > 10) { + BogoSort(); + bogoCycle = 0; + } + break; case ALLIGNED: setHSV(Color.GREEN.h, Color.GREEN.s, Color.GREEN.v); break; From 415ab9ab692e386b584258d15cbd07d38464d7cf Mon Sep 17 00:00:00 2001 From: ethan Date: Wed, 10 Apr 2024 17:56:20 -0700 Subject: [PATCH 3/3] final comp code --- src/main/java/frc/robot/Robot.java | 10 +++-- src/main/java/frc/robot/StateManager.java | 12 ++++- .../java/frc/robot/auto/modes/AimShoot.java | 45 +++++++++++++++++++ .../java/frc/robot/auto/modes/FixedShoot.java | 42 +++++++++++++++++ src/main/java/frc/robot/auto/modes/Shoot.java | 4 +- trajectories.chor | 26 +++++------ 6 files changed, 119 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/auto/modes/AimShoot.java create mode 100644 src/main/java/frc/robot/auto/modes/FixedShoot.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cb9bdb7..38e2ea7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,8 +16,10 @@ import frc.lib.autoUtil.AutoModeRunner; import frc.lib.controllers.PlasmaJoystick; import frc.robot.StateManager.robotState; +import frc.robot.auto.modes.AimShoot; import frc.robot.auto.modes.DriveAndTurn; import frc.robot.auto.modes.DriveY; +import frc.robot.auto.modes.FixedShoot; import frc.robot.auto.modes.FourNear; import frc.robot.auto.modes.DriveX; import frc.robot.auto.modes.Nothing; @@ -107,11 +109,13 @@ public void robotInit() { autoModes[10] = new ThreeCenterFar(swerve, stateManager, photon); autoModes[11] = new ThreeCenterNear(swerve, stateManager, photon); autoModes[12] = new FourNear(swerve, stateManager, photon); - autoModes[13] = new Shoot(stateManager, photon); - + autoModes[13] = new FixedShoot(stateManager, photon); + autoModes[14] = new AimShoot(stateManager, photon); + m_chooser.setDefaultOption("Nothing Auto", autoModes[0]); - m_chooser.addOption("Shoot", autoModes[13]); + m_chooser.addOption("Shoot (1)", autoModes[13]); + m_chooser.addOption("Aim Shoot (1)", m_autoSelected); m_chooser.addOption("Middle Auto (2)", autoModes[5]); m_chooser.addOption("Far Auto (2)", autoModes[6]); m_chooser.addOption("Near Auto (2)", autoModes[7]); diff --git a/src/main/java/frc/robot/StateManager.java b/src/main/java/frc/robot/StateManager.java index 253b0e5..d9d1283 100644 --- a/src/main/java/frc/robot/StateManager.java +++ b/src/main/java/frc/robot/StateManager.java @@ -41,7 +41,8 @@ public enum robotState{ CLIMB_HOOKS_DOWN, CLIMBFALSE, INDEXAUTO, - SHOOTAUTO + SHOOTAUTO, + FIXEDSHOOTAUTO } public StateManager(Intake intake, Shooter shooter, Index index, Climb climb, LEDs leds, Photon photon, Swerve swerve) { @@ -219,10 +220,19 @@ else if(hasGamePiece) { hasGamePiece = false; gamePieceInPos = false; break; + + case FIXEDSHOOTAUTO: + shooter.setState(shooterState.STATICSHOOTFRONT); + intake.setState(intakeState.STOW); + hasGamePiece = false; + gamePieceInPos = false; + break; + case INDEXAUTO: index.setState(indexState.SHOOT); hasGamePiece = false; gamePieceInPos = false; + break; } } } diff --git a/src/main/java/frc/robot/auto/modes/AimShoot.java b/src/main/java/frc/robot/auto/modes/AimShoot.java new file mode 100644 index 0000000..736c51a --- /dev/null +++ b/src/main/java/frc/robot/auto/modes/AimShoot.java @@ -0,0 +1,45 @@ +package frc.robot.auto.modes; + +import java.sql.DriverAction; +import java.util.Optional; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.lib.autoUtil.AutoMode; +import frc.lib.autoUtil.AutoModeEndedException; +import frc.robot.StateManager; +import frc.robot.StateManager.robotState; +import frc.robot.auto.actions.AutoAllign; +import frc.robot.auto.actions.AutoRobotState; +import frc.robot.auto.actions.FollowTrejectory; +import frc.robot.auto.actions.Wait; +import frc.robot.subsystems.Photon; +import frc.robot.subsystems.Swerve; + +public class AimShoot extends AutoMode { + private StateManager manager; + private String pathRed; + private String pathBlue; + private Photon photon; + + public AimShoot(StateManager manager, Photon photon) { + this.manager = manager; + this.photon = photon; + + + + } + + @Override + protected void routine() throws AutoModeEndedException { + DriverStation.reportWarning("Starting Auto run", false); + runAction(new AutoRobotState(manager, robotState.SHOOTAUTO)); + runAction(new Wait(1)); + runAction(new AutoRobotState(manager, robotState.INDEXAUTO)); + runAction(new Wait(1)); + runAction(new AutoRobotState(manager, robotState.IDLE)); + + DriverStation.reportWarning("Ending Auto run", false); + + } +} diff --git a/src/main/java/frc/robot/auto/modes/FixedShoot.java b/src/main/java/frc/robot/auto/modes/FixedShoot.java new file mode 100644 index 0000000..086d9ff --- /dev/null +++ b/src/main/java/frc/robot/auto/modes/FixedShoot.java @@ -0,0 +1,42 @@ +package frc.robot.auto.modes; + +import java.sql.DriverAction; +import java.util.Optional; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.lib.autoUtil.AutoMode; +import frc.lib.autoUtil.AutoModeEndedException; +import frc.robot.StateManager; +import frc.robot.StateManager.robotState; +import frc.robot.auto.actions.AutoAllign; +import frc.robot.auto.actions.AutoRobotState; +import frc.robot.auto.actions.FollowTrejectory; +import frc.robot.auto.actions.Wait; +import frc.robot.subsystems.Photon; +import frc.robot.subsystems.Swerve; + +public class FixedShoot extends AutoMode { + private StateManager manager; + private String pathRed; + private String pathBlue; + private Photon photon; + + public FixedShoot(StateManager manager, Photon photon) { + this.manager = manager; + this.photon = photon; + } + + @Override + protected void routine() throws AutoModeEndedException { + DriverStation.reportWarning("Starting Auto run", false); + runAction(new AutoRobotState(manager, robotState.FIXEDSHOOTAUTO)); + runAction(new Wait(1)); + runAction(new AutoRobotState(manager, robotState.INDEXAUTO)); + runAction(new Wait(1)); + runAction(new AutoRobotState(manager, robotState.IDLE)); + + DriverStation.reportWarning("Ending Auto run", false); + + } +} diff --git a/src/main/java/frc/robot/auto/modes/Shoot.java b/src/main/java/frc/robot/auto/modes/Shoot.java index 605ef0e..0a98a23 100644 --- a/src/main/java/frc/robot/auto/modes/Shoot.java +++ b/src/main/java/frc/robot/auto/modes/Shoot.java @@ -33,9 +33,7 @@ public Shoot(StateManager manager, Photon photon) { @Override protected void routine() throws AutoModeEndedException { DriverStation.reportWarning("Starting Auto run", false); - runAction(new AutoRobotState(manager, robotState.SHOOTAUTO)); - runAction(new Wait(1)); - runAction(new AutoRobotState(manager, robotState.INDEXAUTO)); + runAction(new AutoRobotState(manager, robotState.SHOOT)); runAction(new Wait(1)); runAction(new AutoRobotState(manager, robotState.IDLE)); diff --git a/trajectories.chor b/trajectories.chor index bf40d25..81007d8 100644 --- a/trajectories.chor +++ b/trajectories.chor @@ -3675,7 +3675,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeNearRed2": { "waypoints": [ @@ -7037,7 +7037,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "FourNearBlue1": { "waypoints": [ @@ -8222,7 +8222,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "FourNearRed2": { "waypoints": [ @@ -8635,7 +8635,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeCenterFarBlue2": { "waypoints": [ @@ -8858,7 +8858,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeCenterFarRed1": { "waypoints": [ @@ -9007,7 +9007,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeCenterFarRed2": { "waypoints": [ @@ -9239,7 +9239,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeCenterNearBlue1": { "waypoints": [ @@ -9397,7 +9397,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeCenterNearBlue2": { "waypoints": [ @@ -9786,7 +9786,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeCenterNearRed1": { "waypoints": [ @@ -9935,7 +9935,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ThreeCenterNearRed2": { "waypoints": [ @@ -10268,7 +10268,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "FourNearBlue3": { "waypoints": [ @@ -10732,7 +10732,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "TwoNearRed": { "waypoints": [ @@ -11189,7 +11189,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true } }, "splitTrajectoriesAtStopPoints": false,