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 f4e1895..a24cc5d 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; @@ -46,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(); @@ -115,17 +122,12 @@ private void addTriggers() { driver .rightTrigger() .and(DriverStation::isTeleop) - .and(() -> shouldFeed == false) + .and(() -> canScore()) .or(Autos.autoScoreReq); // Maybe should include if its our turn? intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); - feedReq = - driver - .rightBumper() - .and(DriverStation::isTeleop) - .and(() -> shouldFeed == true) - .or(Autos.autoFeedReq); + feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); // flowReq = driver.leftTrigger().and(driver.rightTrigger()); @@ -348,4 +350,58 @@ public static SuperState getState() { public boolean stateIsIdle() { return getState() == SuperState.IDLE; } + + private Alliance getStartingAlliance() { + String gameData = DriverStation.getGameSpecificMessage(); + // gives first inactive alliance + if (gameData.length() > 0) { + switch (gameData.charAt(0)) { + case 'B': + return Alliance.Red; + case 'R': + return Alliance.Blue; + default: + return Alliance.Blue; + } + } else { + // not sure + return Alliance.Blue; + } + } + + 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; + } + } + + public boolean isOurShift() { + // only cant score when its the others turn, otherwise everyone can + if (getStartingAlliance() == DriverStation.getAlliance().orElse(Alliance.Blue)) { + return !(getCurrentShift() == 2 || getCurrentShift() == 4); + } else { + return !(getCurrentShift() == 1 || getCurrentShift() == 3); + } + } + + public boolean inScoringArea() { + 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() { + return isOurShift() && inScoringArea(); + } }