From 7e1236668be4c4aa7ba50d7c7849d96d547e71e1 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Thu, 15 Jan 2026 16:34:53 -0800 Subject: [PATCH 01/10] get game data and figure out if we can sore based on our field position and if its our turn --- src/main/java/frc/robot/Superstructure.java | 36 +++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index c414d0e..077d224 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -14,6 +15,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; +import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -49,6 +51,7 @@ public Trigger getTrigger() { private SuperState prevState = SuperState.IDLE; private Timer stateTimer = new Timer(); + public Optional alliance = DriverStation.getAlliance(); private final SwerveSubsystem swerve; private final RoutingSubsystem routing; @@ -110,6 +113,7 @@ private void addTriggers() { driver .rightTrigger() .and(DriverStation::isTeleop) + .and(() -> canScore()) .or(Autos.autoScoreReq); // Maybe should include if its our turn? intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); @@ -280,4 +284,36 @@ public static SuperState getState() { public boolean stateIsIdle() { return getState() == SuperState.IDLE; } + + public boolean turnToScore() { + String gameData = DriverStation.getGameSpecificMessage(); + boolean isBlueTurn = false; + boolean isRedTurn = false; + + if (gameData.length() > 0) { + switch (gameData.charAt(0)) { + case 'B': + isBlueTurn = true; + break; + case 'R': + isRedTurn = true; + break; + default: + break; + } + } else { + return false; + } + + return (isBlueTurn && alliance.get() == Alliance.Blue || isRedTurn && alliance.get() == Alliance.Red) ? true : false; + } + + public boolean inScoringArea() { + return (alliance.get() == Alliance.Blue && (swerve.getPose().getX() <= 4.6914191246032715) + || alliance.get() == Alliance.Red && (swerve.getPose().getX() >= 11.889562606811523)) ? true : false; + } + + public boolean canScore() { + return turnToScore() && inScoringArea(); + } } From 4d5013abeebbc7e4e779cc218e7fa5bad695ada7 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Fri, 16 Jan 2026 16:17:37 -0800 Subject: [PATCH 02/10] cleaning code --- src/main/java/frc/robot/Superstructure.java | 24 +++++++++++++-------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 077d224..1ae7594 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -15,7 +15,6 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; -import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -51,7 +50,7 @@ public Trigger getTrigger() { private SuperState prevState = SuperState.IDLE; private Timer stateTimer = new Timer(); - public Optional alliance = DriverStation.getAlliance(); + public Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); private final SwerveSubsystem swerve; private final RoutingSubsystem routing; @@ -118,7 +117,14 @@ private void addTriggers() { intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); - feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); + // or should it be like the same button/general req for feeding vs scoring and choose based on + // if it can score or operator can override?? + feedReq = + driver + .rightBumper() + .and(DriverStation::isTeleop) + .and(() -> !inScoringArea()) + .or(Autos.autoFeedReq); flowReq = operator.rightTrigger(); @@ -285,7 +291,7 @@ public boolean stateIsIdle() { return getState() == SuperState.IDLE; } - public boolean turnToScore() { + public boolean isOurShift() { String gameData = DriverStation.getGameSpecificMessage(); boolean isBlueTurn = false; boolean isRedTurn = false; @@ -305,15 +311,15 @@ public boolean turnToScore() { return false; } - return (isBlueTurn && alliance.get() == Alliance.Blue || isRedTurn && alliance.get() == Alliance.Red) ? true : false; + return (isBlueTurn && alliance == Alliance.Blue || isRedTurn && alliance == Alliance.Red); } public boolean inScoringArea() { - return (alliance.get() == Alliance.Blue && (swerve.getPose().getX() <= 4.6914191246032715) - || alliance.get() == Alliance.Red && (swerve.getPose().getX() >= 11.889562606811523)) ? true : false; - } + return (alliance == Alliance.Blue && (swerve.getPose().getX() <= 4.6914191246032715) + || alliance == Alliance.Red && (swerve.getPose().getX() >= 11.889562606811523)); + } public boolean canScore() { - return turnToScore() && inScoringArea(); + return isOurShift() && inScoringArea(); } } From 54df6bfa975801b122acb98db2a9c8f4db5477ce Mon Sep 17 00:00:00 2001 From: vivi-o Date: Fri, 16 Jan 2026 17:02:46 -0800 Subject: [PATCH 03/10] working on incorperating timer to see if we can score --- src/main/java/frc/robot/Superstructure.java | 30 +++++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 1ae7594..f5e9e72 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -293,25 +293,45 @@ public boolean stateIsIdle() { public boolean isOurShift() { String gameData = DriverStation.getGameSpecificMessage(); - boolean isBlueTurn = false; - boolean isRedTurn = false; + boolean blueStarts = false; + boolean redStarts = false; + double timeLeftinMatch = Timer.getMatchTime(); + boolean isShiftOne; if (gameData.length() > 0) { switch (gameData.charAt(0)) { case 'B': - isBlueTurn = true; + blueStarts = true; break; case 'R': - isRedTurn = true; + redStarts = true; break; default: break; } } else { + // not sure return false; } - return (isBlueTurn && alliance == Alliance.Blue || isRedTurn && alliance == Alliance.Red); + // TODO shorten and fix this ugly ahh code + isShiftOne = (blueStarts && alliance == Alliance.Blue || redStarts && alliance == Alliance.Red); + + // Obviously fix naming :( and thers prob a prettier way to do ts + boolean shiftOneandthree = + ((105.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 130.00) + || (55.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 80.00)); + + boolean shifttwoandfour = + ((80.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 105.00) + || (30.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 55.00)); + + // make this like a loop or list for efficency + if (isShiftOne) { + return !shifttwoandfour; + } else { + return !shiftOneandthree; + } } public boolean inScoringArea() { From 08f17e42db948549230337b0d18f6addbcdafdfc Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 17 Jan 2026 12:14:07 -0800 Subject: [PATCH 04/10] refactor figuring out if we can score --- src/main/java/frc/robot/Superstructure.java | 54 ++++++++++----------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f5e9e72..f9dbcc5 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -291,46 +291,46 @@ public boolean stateIsIdle() { return getState() == SuperState.IDLE; } - public boolean isOurShift() { + private Alliance getStartingAlliance() { String gameData = DriverStation.getGameSpecificMessage(); - boolean blueStarts = false; - boolean redStarts = false; - double timeLeftinMatch = Timer.getMatchTime(); - boolean isShiftOne; - + // gives first inactive alliance if (gameData.length() > 0) { switch (gameData.charAt(0)) { case 'B': - blueStarts = true; - break; + return Alliance.Red; case 'R': - redStarts = true; - break; + return Alliance.Blue; default: - break; + return Alliance.Blue; } } else { // not sure - return false; + return Alliance.Blue; } + } - // TODO shorten and fix this ugly ahh code - isShiftOne = (blueStarts && alliance == Alliance.Blue || redStarts && alliance == Alliance.Red); - - // Obviously fix naming :( and thers prob a prettier way to do ts - boolean shiftOneandthree = - ((105.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 130.00) - || (55.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 80.00)); - - boolean shifttwoandfour = - ((80.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 105.00) - || (30.00 <= Timer.getMatchTime() && Timer.getMatchTime() <= 55.00)); + private int getCurrentShift() { + double timeLeftinMatch = Timer.getMatchTime(); + // may be a nicer way to do this + if (105.00 <= timeLeftinMatch && timeLeftinMatch <= 130.00) { + return 1; + } else if (80.00 <= timeLeftinMatch && timeLeftinMatch <= 105.00) { + return 2; + } else if ((55.00 <= timeLeftinMatch && timeLeftinMatch <= 80.00)) { + return 3; + } else if ((30.00 <= timeLeftinMatch && timeLeftinMatch <= 55.00)) { + return 4; + } else { + return 0; + } + } - // make this like a loop or list for efficency - if (isShiftOne) { - return !shifttwoandfour; + public boolean isOurShift() { + // only cant score when its the others turn, otherwise everyone can + if (getStartingAlliance() == alliance) { + return !(getCurrentShift() == 2 || getCurrentShift() == 4); } else { - return !shiftOneandthree; + return !(getCurrentShift() == 1 || getCurrentShift() == 3); } } From 0c9e9d20cea12e4ad32ec0be0a1bafa4734a504b Mon Sep 17 00:00:00 2001 From: vivi-o Date: Mon, 19 Jan 2026 10:50:47 -0800 Subject: [PATCH 05/10] remove preventing scoring and feeding based on if we can score because the operator will choose --- src/main/java/frc/robot/Superstructure.java | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f9dbcc5..2310805 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -50,7 +50,6 @@ public Trigger getTrigger() { private SuperState prevState = SuperState.IDLE; private Timer stateTimer = new Timer(); - public Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); private final SwerveSubsystem swerve; private final RoutingSubsystem routing; @@ -112,19 +111,13 @@ private void addTriggers() { driver .rightTrigger() .and(DriverStation::isTeleop) - .and(() -> canScore()) .or(Autos.autoScoreReq); // Maybe should include if its our turn? intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); // or should it be like the same button/general req for feeding vs scoring and choose based on // if it can score or operator can override?? - feedReq = - driver - .rightBumper() - .and(DriverStation::isTeleop) - .and(() -> !inScoringArea()) - .or(Autos.autoFeedReq); + feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); flowReq = operator.rightTrigger(); @@ -327,7 +320,7 @@ private int getCurrentShift() { public boolean isOurShift() { // only cant score when its the others turn, otherwise everyone can - if (getStartingAlliance() == alliance) { + if (getStartingAlliance() == DriverStation.getAlliance().orElse(Alliance.Blue)) { return !(getCurrentShift() == 2 || getCurrentShift() == 4); } else { return !(getCurrentShift() == 1 || getCurrentShift() == 3); @@ -335,8 +328,10 @@ public boolean isOurShift() { } public boolean inScoringArea() { - return (alliance == Alliance.Blue && (swerve.getPose().getX() <= 4.6914191246032715) - || alliance == Alliance.Red && (swerve.getPose().getX() >= 11.889562606811523)); + return (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + && (swerve.getPose().getX() <= 4.6914191246032715) + || DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red + && (swerve.getPose().getX() >= 11.889562606811523)); } public boolean canScore() { From 78f1a97038acfd88fa69498f9cac8431cc6645f7 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Mon, 19 Jan 2026 11:51:37 -0800 Subject: [PATCH 06/10] merge main --- src/main/java/frc/robot/Superstructure.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 2310805..3ba31ef 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -111,6 +111,7 @@ private void addTriggers() { driver .rightTrigger() .and(DriverStation::isTeleop) + .and(() -> canScore()) .or(Autos.autoScoreReq); // Maybe should include if its our turn? intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); From bfcada95db89c3a5821c364d378e74881138cb3e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 11:57:47 -0800 Subject: [PATCH 07/10] change flowreq to include feed --- src/main/java/frc/robot/Superstructure.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 20ce916..e51a0b6 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -125,7 +125,8 @@ private void addTriggers() { // if it can score or operator can override?? feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); - flowReq = driver.leftTrigger().and(driver.rightTrigger()); + //flow = intake + (score or feed) + flowReq = driver.leftTrigger().and(driver.rightTrigger().or(driver.rightBumper())); antiJamReq = driver.a().or(operator.a()); From 4f2b674c4a644fe8cde8463be59e2e760efa035a Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 12:04:38 -0800 Subject: [PATCH 08/10] fmt --- src/main/java/frc/robot/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index e51a0b6..c5bed44 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -125,7 +125,7 @@ private void addTriggers() { // if it can score or operator can override?? feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); - //flow = intake + (score or feed) + // flow = intake + (score or feed) flowReq = driver.leftTrigger().and(driver.rightTrigger().or(driver.rightBumper())); antiJamReq = driver.a().or(operator.a()); From 7001b3fa7285797ab65e21115228bf4608dca852 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Thu, 22 Jan 2026 09:46:44 -0800 Subject: [PATCH 09/10] auto log scoring status so drive team can see if scoring is active on dashboard --- src/main/java/frc/robot/Autos.java | 245 +++++++++++++++++--- src/main/java/frc/robot/Superstructure.java | 6 + 2 files changed, 218 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index ebad639..d107fc6 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +// import frc.robot.Autos.PathEndType; import frc.robot.Robot.RobotType; import frc.robot.subsystems.swerve.SwerveSubsystem; import org.littletonrobotics.junction.AutoLogOutput; @@ -23,43 +24,87 @@ public class Autos { // Declare triggers // mehhhhhhh - private static boolean autoPreScore; - private static boolean autoScore; - private static boolean autoIntake; private static boolean autoFeed; + private static boolean autoIntake; + private static boolean autoScore; + private static boolean autoClimb; // private static boolean autoIntakeAlgae; - @AutoLogOutput(key = "Superstructure/Auto Pre Score Request") - public static Trigger autoPreScoreReq = - new Trigger(() -> autoPreScore).and(DriverStation::isAutonomous); - - @AutoLogOutput(key = "Superstructure/Auto Score Request") - public static Trigger autoScoreReq = - new Trigger(() -> autoScore).and(DriverStation::isAutonomous); + @AutoLogOutput(key = "Superstructure/Auto Feed Request") + public static Trigger autoFeedReq = new Trigger(() -> autoFeed).and(DriverStation::isAutonomous); @AutoLogOutput(key = "Superstructure/Auto Intake Request") public static Trigger autoIntakeReq = new Trigger(() -> autoIntake).and(DriverStation::isAutonomous); - @AutoLogOutput(key = "Superstructure/Auto Feed Request") - public static Trigger autoFeedReq = new Trigger(() -> autoFeed).and(DriverStation::isAutonomous); + @AutoLogOutput(key = "Superstructure/Auto Score Request") + public static Trigger autoScoreReq = + new Trigger(() -> autoScore).and(DriverStation::isAutonomous); - public enum PathEndType { - PLACEHOLDER; + @AutoLogOutput(key = "Superstructure/Auto Climb Request") + public static Trigger autoClimbReq = + new Trigger(() -> autoClimb).and(DriverStation::isAutonomous); + + public enum Action { + FEED, + INTAKE, + SCORE, + CLIMB; } public enum Path { - PLACEHOLDER("placeholder", "placeholder", PathEndType.PLACEHOLDER); + // R for right + // L for left + // M for middle + // P for park (starting pose but often used for scoring pose ig) + // D for depot + // O for outpost + // C for climb + // S was going to be for scoreing pos but i think we will just score + // F for feeding poses + // I for intake??? + + // may have to rethink naming to some extent and add more poses + + DtoFL("D", "FL", Action.FEED), + FLMtoPL("FLM", "PL", Action.SCORE), + FLtoFLM("FL", "FLM", Action.FEED), + FLtoPL("FL", "PL", Action.SCORE), + FRMtoPR("FRM", "PR", Action.SCORE), + FRtoFRM("FR", "FRM", Action.FEED), + FRtoPR("FR", "PR", Action.SCORE), + OtoFR("O", "FR", Action.FEED), + PLtoCL("PL", "CL", Action.CLIMB), + PLtoCM("PL", "CM", Action.CLIMB), + PLtoD("PL", "D", Action.INTAKE), + PLtoFL("PL", "FL", Action.FEED), + PRtoCM("PR", "CM", Action.CLIMB), + PRtoCR("PR", "CR", Action.CLIMB), + PRtoFR("PR", "FR", Action.FEED), + PRtoO("PR", "O", Action.INTAKE), + // idk seperate intake and feed so action is included makes it easier for me but they use the + // same + // trajectories so i dont have to make new paths + DtoIL("D", "FL", Action.INTAKE), + ILMtoPL("FLM", "PL", Action.SCORE), + ILtoILM("FL", "FLM", Action.INTAKE), + ILtoPL("FL", "PL", Action.SCORE), + IRMtoPR("FRM", "PR", Action.SCORE), + IRtoIRM("FR", "FRM", Action.INTAKE), + IRtoPR("FR", "PR", Action.SCORE), + OtoIR("O", "FR", Action.INTAKE), + PLtoIL("PL", "FL", Action.INTAKE), + PRtoIR("PR", "FR", Action.INTAKE); private final String start; private final String end; - private final PathEndType type; + private final Action action; - private Path(String start, String end, PathEndType type) { + private Path(String start, String end, Action action) { this.start = start; this.end = end; - this.type = type; + this.action = action; } public AutoTrajectory getTrajectory(AutoRoutine routine) { @@ -88,7 +133,6 @@ public Autos(SwerveSubsystem swerve) { }); } - // TODO write leave auto public Command leaveAuto() { final AutoRoutine routine = factory.newRoutine("Leave Auto"); Path[] paths = {}; @@ -104,29 +148,164 @@ public Command leaveAuto() { } public Command runPath(Path path, AutoRoutine routine) { - PathEndType type = path.type; - switch (type) { + Action action = path.action; + switch (action) { + case INTAKE: + return intakePath(path, routine); + case FEED: + return feedPath(path, routine); + case SCORE: + return scorePath(path, routine); + case CLIMB: + return climbPath(path, routine); default: // this should never happen return Commands.none(); } } - public Command setAutoScoreReqTrue() { - return Commands.runOnce( - () -> { - autoScore = true; - }); + // TODO aligning to climb pos correctly + public Command climbPath(Path path, AutoRoutine routine) { + // path align and climb + return Commands.sequence( + path.getTrajectory(routine) + .cmd() + .until( + routine.observe( + path.getTrajectory(routine) + .atTime( + path.getTrajectory(routine).getRawTrajectory().getTotalTime() + - (0.3)))), + setAutoClimbReqTrue()); + } + + public Command feedPath(Path path, AutoRoutine routine) { + return Commands.sequence( + setAutoFeedReqTrue(), + path.getTrajectory(routine).cmd(), + path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()), + setAutoFeedReqFalse()); + } + + public Command scorePath(Path path, AutoRoutine routine) { + // path align and score + return Commands.sequence( + path.getTrajectory(routine) + .cmd() + .until( + routine.observe( + path.getTrajectory(routine) + .atTime( + path.getTrajectory(routine).getRawTrajectory().getTotalTime() + - (0.3)))), + setAutoScoreReqTrue(), + waitUntilEmpty(), + setAutoScoreReqFalse()); + } + + // feeding and intake could prob be improved + public Command intakePath(Path path, AutoRoutine routine) { + return Commands.sequence( + setAutoIntakeReqTrue(), + path.getTrajectory(routine).cmd(), + path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()), + setAutoIntakeReqFalse()); } - public Command setAutoPreScoreReqTrue() { - return Commands.runOnce(() -> autoPreScore = true); + public Command shootPreload() { + return Commands.sequence(setAutoScoreReqTrue(), waitUntilEmpty(), setAutoScoreReqFalse()); + } + + public Command setAutoIntakeReqTrue() { + return Commands.runOnce(() -> autoIntake = true); + } + + public Command setAutoIntakeReqFalse() { + return Commands.runOnce(() -> autoIntake = false); + } + + public Command setAutoScoreReqTrue() { + return Commands.runOnce(() -> autoScore = true); } public Command setAutoScoreReqFalse() { - return Commands.runOnce( - () -> { - autoScore = false; - autoPreScore = false; - }); + return Commands.runOnce(() -> autoScore = false); + } + + public Command setAutoFeedReqTrue() { + return Commands.runOnce(() -> autoFeed = true); + } + + public Command setAutoFeedReqFalse() { + return Commands.runOnce(() -> autoFeed = false); + } + + public Command setAutoClimbReqTrue() { + return Commands.runOnce(() -> autoClimb = true); + } + + public Command setAutoClimbReqFalse() { + return Commands.runOnce(() -> autoClimb = false); + } + + // TODO: score at the start of each auto + // specific paths: + // no idea what to name them + public Command getDepotScoreClimbAuto() { + final AutoRoutine routine = factory.newRoutine("Depot Score Clim Auto"); + Path[] paths = {Path.PLtoD, Path.DtoIL, Path.ILtoILM, Path.ILMtoPL, Path.PLtoCL}; + // Will always need to reset odo at the start of a routine + Command autoCommand = + paths[0] + .getTrajectory(routine) + .resetOdometry() + .andThen(shootPreload()); // shoot preload then do the paths + + for (Path p : paths) { + autoCommand = autoCommand.andThen(runPath(p, routine)); + } + + return routine.cmd(); + } + + public Command getOutpostScoreClimbAuto() { + final AutoRoutine routine = factory.newRoutine("Outpost Score Climb Auto"); + Path[] paths = {Path.PRtoO, Path.OtoIR, Path.IRtoIRM, Path.IRMtoPR, Path.PRtoCR}; + Command autoCommand = paths[0].getTrajectory(routine).resetOdometry().andThen(shootPreload()); + + for (Path p : paths) { + autoCommand = autoCommand.andThen(runPath(p, routine)); + } + + return routine.cmd(); + } + + public Command getDepotFeedClimbAuto() { + final AutoRoutine routine = factory.newRoutine("Depot Feed Climb Auto"); + Path[] paths = {Path.PLtoD, Path.DtoFL, Path.FLtoFLM, Path.FLMtoPL, Path.PLtoCL}; + Command autoCommand = paths[0].getTrajectory(routine).resetOdometry().andThen(shootPreload()); + + for (Path p : paths) { + autoCommand = autoCommand.andThen(runPath(p, routine)); + } + + return routine.cmd(); + } + + public Command getOutpostFeedClimbAuto() { + final AutoRoutine routine = factory.newRoutine("Outpost Feed Climb Auto"); + Path[] paths = {Path.PLtoD, Path.DtoFL, Path.FLtoFLM, Path.FLMtoPL, Path.PLtoCL}; + Command autoCommand = paths[0].getTrajectory(routine).resetOdometry().andThen(shootPreload()); + + for (Path p : paths) { + autoCommand = autoCommand.andThen(runPath(p, routine)); + } + + return routine.cmd(); + } + + public Command waitUntilEmpty() { + // TODO wait till robot empty / done scoring + // return null; + return Commands.waitSeconds(0.5); } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index c5bed44..ebb62c1 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -47,6 +47,12 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/State") private static SuperState state = SuperState.IDLE; + @AutoLogOutput(key = "Scoring/Scoring Active") + public boolean isScoringActive = + isOurShift(); // assuming we want the dashboard to show if the time allows us to score not if + + // its litterly possible + private SuperState prevState = SuperState.IDLE; private Timer stateTimer = new Timer(); From c8decc3f3882cb8553f4614f7d3767a2d228c540 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 21:36:36 -0800 Subject: [PATCH 10/10] fmt --- src/main/java/frc/robot/Superstructure.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9a739e3..a24cc5d 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -127,11 +127,7 @@ private void addTriggers() { intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); - feedReq = - driver - .rightBumper() - .and(DriverStation::isTeleop) - .or(Autos.autoFeedReq); + feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); // flowReq = driver.leftTrigger().and(driver.rightTrigger());