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 4130cc2..d9d1283 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; @@ -39,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) { @@ -73,7 +76,10 @@ public void logging() { public void periodic() { logging(); - if(photon.isAligned() && hasGamePiece) { + if(DriverStation.isDisabled()) { + leds.setState(LEDState.BOGO); + } + else if(photon.isAligned() && hasGamePiece) { leds.setState(LEDState.ALLIGNED); } else if(hasGamePiece) { @@ -214,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/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java index 46bee07..f3a7185 100644 --- a/src/main/java/frc/robot/subsystems/LEDs.java +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -1,14 +1,19 @@ package frc.robot.subsystems; +import java.util.Random; + import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; 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; + private bogo[] bogoArray; + public enum Color { RED(0,255,128), GREEN(60,255,128), @@ -25,7 +30,37 @@ private Color (int h, int s, int v) { } } + public enum bogo { + 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) { + this.h = h; + this.s = s; + this.v = v; + this.sorting = sorting; + } + } + public enum LEDState { + BOGO, ALLIGNED, HASPEICE, NOPEICE; @@ -37,14 +72,44 @@ 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); + + bogoCycle = 0; + } + 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 +126,32 @@ 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 < 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; + } + /** * setting one of the leds a certian color in HS @@ -154,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; 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,