diff --git a/src/main/deploy/pathplanner/paths/Depot - Middle.path b/src/main/deploy/pathplanner/paths/Depot - Middle.path index 1d39af22ee..7671e3b431 100644 --- a/src/main/deploy/pathplanner/paths/Depot - Middle.path +++ b/src/main/deploy/pathplanner/paths/Depot - Middle.path @@ -16,28 +16,44 @@ }, { "anchor": { - "x": 5.999810201660737, - "y": 5.288446026097272 + "x": 6.031611982879313, + "y": 5.555292439367715 }, "prevControl": { - "x": 4.792028146900899, - "y": 5.455285652833384 + "x": 4.955057996156298, + "y": 5.1432779506218695 }, "nextControl": { - "x": 7.207592256420575, - "y": 5.121606399361161 + "x": 7.07964336661683, + "y": 5.956390870180839 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.226963226571769, - "y": 4.298600237247924 + "x": 7.5066191155492135, + "y": 5.801126961483595 }, "prevControl": { - "x": 7.871909845788851, - "y": 6.310569395017794 + "x": 7.274499942827901, + "y": 5.89397463057212 + }, + "nextControl": { + "x": 7.8947788873038505, + "y": 5.64586305278174 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.153552068471324, + "y": 4.442567760337759 + }, + "prevControl": { + "x": 8.05004279600342, + "y": 5.1024393723206405 }, "nextControl": null, "isLocked": false, @@ -56,6 +72,10 @@ { "waypointRelativePos": 1.2824156305506222, "rotationDegrees": -171.1847422522127 + }, + { + "waypointRelativePos": 2.334754797441347, + "rotationDegrees": 119.52019853618923 } ], "constraintZones": [ @@ -85,7 +105,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 133.87669728592454 + "rotation": 109.62683721086032 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/L horseshoe.path b/src/main/deploy/pathplanner/paths/L horseshoe.path index bf7a438226..0eda820523 100644 --- a/src/main/deploy/pathplanner/paths/L horseshoe.path +++ b/src/main/deploy/pathplanner/paths/L horseshoe.path @@ -100,28 +100,28 @@ "y": 2.3 }, "prevControl": { - "x": 3.930941512125534, - "y": 2.3 + "x": 3.8681505898504325, + "y": 2.54258652550161 }, "nextControl": { - "x": 2.430941512125534, - "y": 2.3 + "x": 2.50965769675926, + "y": 1.7888234230324076 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.65, - "y": 0.69 + "x": 1.904179743223965, + "y": 2.500000000000001 }, "prevControl": { - "x": 2.65, - "y": 0.6899999999999997 + "x": 2.4666588375526692, + "y": 3.0030359519675933 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Outpost alliance wait" } ], "rotationTargets": [ @@ -148,10 +148,6 @@ { "waypointRelativePos": 5.997867803837968, "rotationDegrees": -45.0 - }, - { - "waypointRelativePos": 6.776119402985068, - "rotationDegrees": 0.0 } ], "constraintZones": [ @@ -181,7 +177,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 0.0 + "rotation": 80.579832280714 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/L quarter light.path b/src/main/deploy/pathplanner/paths/L quarter light.path index bcebb4ebab..f821299b05 100644 --- a/src/main/deploy/pathplanner/paths/L quarter light.path +++ b/src/main/deploy/pathplanner/paths/L quarter light.path @@ -4,28 +4,28 @@ { "anchor": { "x": 3.395203052662038, - "y": 5.71 + "y": 5.516476462196862 }, "prevControl": null, "nextControl": { "x": 4.562700290337638, - "y": 5.695287264914479 + "y": 5.501763727111341 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.569672364672364, - "y": 5.71 + "x": 5.790906032986112, + "y": 5.412967189728959 }, "prevControl": { - "x": 5.319684140380445, - "y": 5.707573538507863 + "x": 5.540917808694193, + "y": 5.410540728236822 }, "nextControl": { - "x": 6.673958188657408, - "y": 5.720718532986112 + "x": 6.895191856971156, + "y": 5.423685722715071 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Outpost - Middle.path b/src/main/deploy/pathplanner/paths/Outpost - Middle.path index a18b2ffbee..e51a1422c8 100644 --- a/src/main/deploy/pathplanner/paths/Outpost - Middle.path +++ b/src/main/deploy/pathplanner/paths/Outpost - Middle.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 0.643, - "y": 0.736 + "x": 1.904179743223965, + "y": 2.500000000000001 }, "prevControl": null, "nextControl": { - "x": 1.4365974392361116, - "y": 2.5224803964120373 + "x": 3.2498002853067045, + "y": 2.3076890156918695 + }, + "isLocked": false, + "linkedName": "Outpost alliance wait" + }, + { + "anchor": { + "x": 6.976134094151213, + "y": 2.3076890156918695 + }, + "prevControl": { + "x": 6.113849913328376, + "y": 1.9843324478833049 + }, + "nextControl": { + "x": 7.597189728958634, + "y": 2.540584878744652 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.675634186921297, - "y": 3.147420717592593 + "x": 8.050042796005705, + "y": 3.7180028530670475 }, "prevControl": { - "x": 2.9764110243055555, - "y": 2.4621843171296294 + "x": 7.90771754636234, + "y": 2.579400855920116 }, "nextControl": null, "isLocked": false, @@ -30,8 +46,12 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5653268914473685, - "rotationDegrees": 135.0 + "waypointRelativePos": 0.26652452025586415, + "rotationDegrees": -150.0 + }, + { + "waypointRelativePos": 0.707889125799577, + "rotationDegrees": -150.0 } ], "constraintZones": [ @@ -61,13 +81,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 135.0 + "rotation": -106.18920625702695 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 80.579832280714 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R quarter light.path b/src/main/deploy/pathplanner/paths/R quarter light.path index 27be8be604..724185106f 100644 --- a/src/main/deploy/pathplanner/paths/R quarter light.path +++ b/src/main/deploy/pathplanner/paths/R quarter light.path @@ -80,16 +80,16 @@ }, { "anchor": { - "x": 0.6570286140449626, - "y": 0.6528370467490765 + "x": 1.904179743223965, + "y": 2.500000000000001 }, "prevControl": { - "x": 0.907028605405066, - "y": 0.65277132043627 + "x": 2.1541797345840688, + "y": 2.4999342736871943 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Outpost alliance wait" } ], "rotationTargets": [ @@ -112,10 +112,6 @@ { "waypointRelativePos": 3.8888980263157924, "rotationDegrees": 45.0 - }, - { - "waypointRelativePos": 4.5, - "rotationDegrees": -0.0 } ], "constraintZones": [ @@ -158,7 +154,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 80.579832280714 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/R quarter.path b/src/main/deploy/pathplanner/paths/R quarter.path index c104be027c..ad2d863932 100644 --- a/src/main/deploy/pathplanner/paths/R quarter.path +++ b/src/main/deploy/pathplanner/paths/R quarter.path @@ -80,32 +80,16 @@ }, { "anchor": { - "x": 1.4819999276578297, - "y": 0.636 + "x": 1.904179743223965, + "y": 2.500000000000001 }, "prevControl": { - "x": 1.8625482553348116, - "y": 0.636 - }, - "nextControl": { - "x": 1.190630565238076, - "y": 0.636 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.643, - "y": 0.736 - }, - "prevControl": { - "x": 1.2326816465669375, - "y": 0.7360000000000001 + "x": 2.4938613897909025, + "y": 2.500000000000001 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Outpost alliance wait" } ], "rotationTargets": [ @@ -132,17 +116,13 @@ { "waypointRelativePos": 3.8888980263157924, "rotationDegrees": 45.0 - }, - { - "waypointRelativePos": 4.983367598684212, - "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", "minWaypointRelativePos": 3.7561513704318967, - "maxWaypointRelativePos": 6.0, + "maxWaypointRelativePos": 5.0, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 2.0, @@ -176,7 +156,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 80.579832280714 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 0119a4fe4a..7cc99db261 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -35,7 +35,7 @@ public class JoysticksBindings { private static final double TIME_BEFORE_SHIFT_END_TO_RUMBLE = 5.0; private static final SmartJoystick MAIN_JOYSTICK = new SmartJoystick(JoystickPorts.MAIN, true); - private static final SmartJoystick SECOND_JOYSTICK = new SmartJoystick(JoystickPorts.SECOND, true); + private static final SmartJoystick SECOND_JOYSTICK = new SmartJoystick(JoystickPorts.SECOND); private static final SmartJoystick THIRD_JOYSTICK = new SmartJoystick(JoystickPorts.THIRD); private static final SmartJoystick FOURTH_JOYSTICK = new SmartJoystick(JoystickPorts.FOURTH); private static final SmartJoystick FIFTH_JOYSTICK = new SmartJoystick(JoystickPorts.FIFTH); @@ -132,12 +132,22 @@ private static void mainJoystickButtons(Robot robot) { usedJoystick.B.onTrue(robot.getRobotCommander().driveWith(RobotState.OUTTAKE)); usedJoystick.Y.onTrue(robot.getRobotCommander().getIntakeStateHandler().setState(IntakeState.OUTTAKE)); usedJoystick.POV_DOWN.onTrue(robot.getRobotCommander().driveWith(RobotState.CONVEYOR_OUTTAKE)); - usedJoystick.X.onTrue(robot.getRobotCommander().towerAssist()); } private static void secondJoystickButtons(Robot robot) { SmartJoystick usedJoystick = SECOND_JOYSTICK; // bindings... + // Shoot & Pass... + usedJoystick.R1.onTrue( + robot.getRobotCommander() + .driveWithChangingState(RobotState.PRE_SCORE, robot.getRobotCommander().dumbScoreSequence(), () -> getScoringState(usedJoystick)) + ); + usedJoystick.getAxisAsButton(Axis.RIGHT_TRIGGER) + .onTrue( + robot.getRobotCommander() + .driveWithChangingState(RobotState.PRE_PASS, robot.getRobotCommander().dumbPassSequence(), () -> getPassState(usedJoystick)) + ); + usedJoystick.X.onTrue(robot.getRobotCommander().setState(RobotState.RESET_SUBSYSTEMS)); } private static void thirdJoystickButtons(Robot robot) { diff --git a/src/main/java/frc/RobotManager.java b/src/main/java/frc/RobotManager.java index f5c820e5f2..ff8c4b2427 100644 --- a/src/main/java/frc/RobotManager.java +++ b/src/main/java/frc/RobotManager.java @@ -98,6 +98,7 @@ public void disabledInit() { @Override public void autonomousInit() { robot.getSwerve().setIsRunningIndependently(true); + TimeUtil.setAutonomousStartTimeSeconds(TimeUtil.getCurrentTimeSeconds()); if (autonomousCommand == null) { this.autonomousCommand = robot.getAutonomousChooser().getChosenValue(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 40350b2b88..462781cadb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -309,7 +309,7 @@ private void updateAllSubsystems() { } public boolean isTurretMoveLegal() { - return TurretCalculations.isTurretMoveLegal(ShootingCalculations.getShootingParams().targetTurretPosition(), turret.getPosition()); + return TurretCalculations.isTurretMoveLegal(turret.getPositionRequest().getSetPoint(), turret.getPosition()); } public void periodic() { diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 4eb658158b..07a0d60152 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -34,6 +34,10 @@ public class AutonomousConstants { public static final double TIME_FOR_SECOND_OUTTAKE_IN_PUSH_AUTO = 1.75; public static final double STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; public static final double SIDE_STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; + public static final double TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS = 1.0; + public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START = 4.0; + public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START = 6.0; + public static final double MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE = 1.0; public static final Double DEFAULT_STUCK_DEBOUNCE_SECONDS = 2.0; diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 10c8b6440a..06192ecfb3 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -6,8 +6,10 @@ import edu.wpi.first.wpilibj2.command.*; import frc.constants.field.AllianceSide; import frc.robot.Robot; +import frc.utils.GamePeriodUtils; import frc.utils.auto.PathHelper; import frc.utils.auto.PathPlannerAutoWrapper; +import frc.utils.time.TimeUtil; import java.util.List; import java.util.function.BooleanSupplier; @@ -502,28 +504,18 @@ private static Supplier getQuarterAuto( .until(() -> hasPathEnded) .andThen( new ParallelCommandGroup( - new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_WIGGLE_AFTER_PATH_END) - .andThen( - robot.getSwerve() - .getCommandsBuilder() - .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) - .withDeadline(new WaitCommand(AutonomousConstants.TIME_TO_WAIT_AT_DEPOT)) - ) - .andThen( - getAllianceSideToStartingLineAuto( - robot, - startingSide, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle - ) - ) - .asProxy(), - new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) - .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) + getAllianceSideToStartingLineAuto( + robot, + startingSide, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + returnToMiddle, + scoreSequence, + closeIntake, + true + ) ) ) ) @@ -532,7 +524,7 @@ private static Supplier getQuarterAuto( new Pose2d(), startingSide == AllianceSide.DEPOT ? "L quarter" : "R quarter", startingSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("L quarter") : PathHelper.PATH_PLANNER_PATHS.get("R quarter"), - getAllianceSideToStartingLinePath(startingSide, returnToMiddle) + getAllianceSideToStartingLinePath(startingSide, returnToMiddle, true) ); } @@ -612,7 +604,10 @@ private static Supplier getSideAuto( regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, - returnToMiddle + returnToMiddle, + scoreSequence, + closeIntake, + false ) ) .asProxy(), @@ -725,7 +720,10 @@ private static Supplier getStealAuto( regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, - returnToMiddle + returnToMiddle, + scoreSequence, + closeIntake, + false ) ) .asProxy(), @@ -743,7 +741,7 @@ private static Supplier getStealAuto( ? PathHelper.PATH_PLANNER_PATHS.get("Depot Hub Wait") : PathHelper.PATH_PLANNER_PATHS.get("Outpost Hub Wait"), getStealPath(firstOpponentBumpSide, returnSide, skipOutpost), - getAllianceSideToStartingLinePath(actualReturnSide, returnToMiddle) + getAllianceSideToStartingLinePath(actualReturnSide, returnToMiddle, false) ); } @@ -821,7 +819,10 @@ private static Supplier getLightStealAuto( regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, - returnToMiddle + returnToMiddle, + scoreSequence, + closeIntake, + false ) ) .asProxy(), @@ -886,25 +887,19 @@ private static Supplier getLightQuarterAuto( .until(() -> hasPathEnded) .andThen( new ParallelCommandGroup( - new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_WIGGLE_AFTER_PATH_END) - .andThen( - robot.getSwerve() - .getCommandsBuilder() - .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) - .withDeadline(new WaitCommand(AutonomousConstants.TIME_TO_WAIT_AT_DEPOT)) - ) - .andThen( - getAllianceSideToStartingLineAuto( - robot, - startingSide, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle - ) - ) - .asProxy(), + + getAllianceSideToStartingLineAuto( + robot, + startingSide, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + returnToMiddle, + scoreSequence, + closeIntake, + true + ).asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) .onlyIf(() -> !returnToMiddle.getAsBoolean()) @@ -920,7 +915,7 @@ private static Supplier getLightQuarterAuto( ? PathHelper.PATH_PLANNER_PATHS.get("L quarter light to outpost") : PathHelper.PATH_PLANNER_PATHS.get("L quarter light") : PathHelper.PATH_PLANNER_PATHS.get("R quarter light"), - getAllianceSideToStartingLinePath(startingSide, returnToMiddle) + getAllianceSideToStartingLinePath(startingSide, returnToMiddle, true) ); } @@ -975,7 +970,10 @@ private static Supplier getExtendedLQuarterAuto( regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, - returnToMiddle + returnToMiddle, + scoreSequence, + closeIntake, + false ) ) .asProxy(), @@ -1048,7 +1046,10 @@ private static Supplier getHorseshoeAuto( regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, - returnToMiddle + returnToMiddle, + scoreSequence, + closeIntake, + false ) ) .asProxy(), @@ -1065,7 +1066,7 @@ private static Supplier getHorseshoeAuto( startingSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("L horseshoe") : PathHelper.PATH_PLANNER_PATHS.get("R horseshoe"), - getAllianceSideToStartingLinePath(startingSide, returnToMiddle) + getAllianceSideToStartingLinePath(startingSide, returnToMiddle, false) ); } @@ -1188,36 +1189,74 @@ private static Command getAllianceSideToStartingLineAuto( Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, double stuckDebounceSeconds, - BooleanSupplier returnToMiddle + BooleanSupplier returnToMiddle, + Supplier scoreSequence, + Supplier closeIntake, + boolean isQuarter ) { - return PathFollowingCommandsBuilder - .followAdjustedPathThenStop( - robot.getSwerve(), - () -> robot.getPoseEstimator().getEstimatedPose(), - getAllianceSideToStartingLinePath(allianceSide, returnToMiddle), - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - robot.getSwerve().getLogPath() - ) + return new ParallelCommandGroup( + new WaitCommand(AutonomousConstants.MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE) + .andThen(new RunCommand(() -> {}).until(() -> hasStoppedThrowingBalls(robot))) + .andThen(closeIntake.get()), + scoreSequence.get() + ).until( + () -> returnToMiddle.getAsBoolean() + && TimeUtil.getCurrentTimeSeconds() - TimeUtil.getAutonomousStartTimeSeconds() + > GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS + - (allianceSide == AllianceSide.OUTPOST + ? AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START + : AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START) + ) .andThen( - robot.getSwerve() - .getCommandsBuilder() - .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) + new ParallelCommandGroup( + scoreSequence.get(), + closeIntake.get(), + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + getAllianceSideToStartingLinePath(allianceSide, returnToMiddle, isQuarter), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) + .andThen( + robot.getSwerve() + .getCommandsBuilder() + .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) + .onlyIf(() -> !returnToMiddle.getAsBoolean()) + ) + ) ); } - private static PathPlannerPath getAllianceSideToStartingLinePath(AllianceSide allianceSide, BooleanSupplier returnToMiddle) { + private static PathPlannerPath getAllianceSideToStartingLinePath( + AllianceSide allianceSide, + BooleanSupplier returnToMiddle, + boolean isQuarter + ) { if (returnToMiddle.getAsBoolean()) { return allianceSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("Depot - Middle") : PathHelper.PATH_PLANNER_PATHS.get("Outpost - Middle"); } + if (isQuarter && allianceSide == AllianceSide.OUTPOST) { + return PathHelper.PATH_PLANNER_PATHS.get("R alliance stay"); + } return allianceSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("Depot - Starting line") : PathHelper.PATH_PLANNER_PATHS.get("Outpost - Starting line"); } + + private static boolean hasStoppedThrowingBalls(Robot robot) { + double lastBallTimestamp = robot.getBallsBufferIncludingPassing().getInternalBuffer().lastEntry() != null + ? robot.getBallsBufferIncludingPassing().getInternalBuffer().lastEntry().getKey() + : 0; + + return TimeUtil.getCurrentTimeSeconds() - lastBallTimestamp > AutonomousConstants.TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS; + } + } diff --git a/src/main/java/frc/robot/statemachine/RobotCommander.java b/src/main/java/frc/robot/statemachine/RobotCommander.java index bc6418c300..58eec021e4 100644 --- a/src/main/java/frc/robot/statemachine/RobotCommander.java +++ b/src/main/java/frc/robot/statemachine/RobotCommander.java @@ -12,6 +12,11 @@ import frc.robot.subsystems.swerve.Swerve; import frc.robot.subsystems.swerve.states.SwerveState; import frc.robot.subsystems.swerve.states.aimassist.AimAssist; + + +import frc.robot.subsystems.constants.flywheel.FlywheelConstants; +import frc.robot.subsystems.constants.hood.HoodConstants; +import frc.robot.subsystems.constants.turret.TurretConstants; import org.littletonrobotics.junction.Logger; import java.util.Set; @@ -198,6 +203,15 @@ public boolean isReadyToScore() { ); } + public boolean isReadyToDumbScore() { + return ShootingChecks.isReadyToDumbScore( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_SCORING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_SCORING, + StateMachineConstants.TURRET_TOLERANCE_TO_START_SCORING + ); + } + public boolean isReadyToPass() { return ShootingChecks.isReadyToPass( robot, @@ -208,6 +222,15 @@ public boolean isReadyToPass() { ); } + public boolean isReadyToDumbPass() { + return ShootingChecks.isReadyToDumbPass( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_START_PASSING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_START_PASSING, + StateMachineConstants.TURRET_TOLERANCE_TO_START_PASSING + ); + } + public boolean canContinueScoring() { return ShootingChecks.canContinueScoring( robot, @@ -218,6 +241,15 @@ public boolean canContinueScoring() { ); } + public boolean canContinueDumbScoring() { + return ShootingChecks.canContinueDumbScoring( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_SCORING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_SCORING, + StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_SCORING + ); + } + public boolean canContinuePassing() { return ShootingChecks.canContinuePassing( robot, @@ -228,6 +260,15 @@ public boolean canContinuePassing() { ); } + public boolean canContinueDumbPassing() { + return ShootingChecks.canContinueDumbPassing( + robot, + StateMachineConstants.FLYWHEEL_VELOCITY_TOLERANCE_RPS_TO_CONTINUE_PASSING, + StateMachineConstants.HOOD_POSITION_TOLERANCE_TO_CONTINUE_PASSING, + StateMachineConstants.TURRET_TOLERANCE_TO_CONTINUE_PASSING + ); + } + private boolean calibrationIsReadyToScore() { return ShootingChecks.calibrationIsReadyToShoot( robot, @@ -271,6 +312,31 @@ public Command scoreSequence() { ); } + public Command dumbScoreSequence() { + return new ParallelCommandGroup( + shooterStateHandler.setDumbState( + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY + ), + new SequentialCommandGroup( + asSubsystemCommand(funnelStateHandler.setState(FunnelState.STOP), RobotState.PRE_SCORE).until(this::isReadyToDumbScore), + new RepeatCommand( + new SequentialCommandGroup( + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbScore), + RobotState.PRE_SCORE + ), + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbScoring()), + RobotState.SCORE + ) + ) + ) + ) + ); + } + public Command passSequence() { return new ParallelCommandGroup( shooterStateHandler.setState(ShooterState.SHOOT), @@ -287,6 +353,32 @@ public Command passSequence() { ); } + public Command dumbPassSequence() { + return new ParallelCommandGroup( + shooterStateHandler.setDumbState( + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY + ), + new RepeatCommand( + new SequentialCommandGroup( + new ParallelCommandGroup( + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.PRE_SHOOT).until(this::isReadyToDumbPass), + RobotState.PRE_PASS + ) + ), + new ParallelCommandGroup( + asSubsystemCommand( + funnelStateHandler.setState(FunnelState.SHOOT).until(() -> !canContinueDumbPassing()), + RobotState.PASS + ) + ) + ) + ) + ); + } + public Command calibrationScoreSequence() { return new ParallelCommandGroup( shooterStateHandler.setState(ShooterState.CALIBRATION), diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index f9cfc45720..97113494b5 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -6,6 +6,9 @@ import frc.constants.field.Field; import frc.robot.Robot; import frc.robot.statemachine.shooterstatehandler.ShooterConstants; +import frc.robot.subsystems.constants.flywheel.FlywheelConstants; +import frc.robot.subsystems.constants.hood.HoodConstants; +import frc.robot.subsystems.constants.turret.TurretConstants; import frc.utils.HubUtil; import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; @@ -125,6 +128,35 @@ private static boolean isReadyToShoot( return isReadyToShoot; } + private static boolean isReadyToDumbShoot( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d flywheelTargetVelocityRPS, + Rotation2d hoodPositionTolerance, + Rotation2d hoodTargetPosition, + Rotation2d turretTolerance, + Rotation2d targetTurretPosition, + String actionLogPath + ) { + String logPath = shootingChecksLogPath + "/IsReadyToDumb" + actionLogPath; + + boolean isAtTurretAtTarget = isTurretAtTargetPosition(robot.getTurret().getPosition(), targetTurretPosition, turretTolerance, logPath); + + boolean isFlywheelReadyToShoot = isFlywheelAtVelocity( + robot.getFlyWheel().getVelocity(), + flywheelTargetVelocityRPS, + flywheelVelocityToleranceRPS, + logPath + ); + + boolean isHoodAtPosition = isHoodAtPosition(robot.getHood().getPosition(), hoodTargetPosition, hoodPositionTolerance, logPath); + + boolean isReadyToDumbShoot = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition; + Logger.recordOutput(shootingChecksLogPath + "/IsReadyToDumbShoot", isReadyToDumbShoot); + + return isReadyToDumbShoot; + } + private static boolean canContinueShooting( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -168,6 +200,36 @@ private static boolean canContinueShooting( return canContinueShooting; } + private static boolean canContinueDumbShooting( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d flywheelTargetVelocityRPS, + Rotation2d hoodPositionTolerance, + Rotation2d hoodTargetPosition, + Rotation2d turretTolerance, + Rotation2d turretTargetPosition, + String actionLogPath + ) { + String logPath = shootingChecksLogPath + "/CanContinueDumb" + actionLogPath; + + boolean isAtTurretAtTarget = isTurretAtTargetPosition(robot.getTurret().getPosition(), turretTargetPosition, turretTolerance, logPath); + + boolean isFlywheelReadyToShoot = isFlywheelAtVelocity( + robot.getFlyWheel().getVelocity(), + flywheelTargetVelocityRPS, + flywheelVelocityToleranceRPS, + logPath + ); + + boolean isHoodAtPosition = isHoodAtPosition(robot.getHood().getPosition(), hoodTargetPosition, hoodPositionTolerance, logPath); + + boolean canContinueDumbShooting = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition; + + Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbShooting", canContinueDumbShooting); + + return canContinueDumbShooting; + } + public static boolean calibrationIsReadyToShoot( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -245,6 +307,28 @@ public static boolean isReadyToScore( return isReadyToScore; } + public static boolean isReadyToDumbScore( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance + ) { + boolean isReadyToDumbShoot = isReadyToDumbShoot( + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, + "Score" + ); + + Logger.recordOutput(shootingChecksLogPath + "isReadyToDumbScore", isReadyToDumbShoot); + + return isReadyToDumbShoot; + } + public static boolean isReadyToPass( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -271,6 +355,28 @@ public static boolean isReadyToPass( return isReadyToPass; } + public static boolean isReadyToDumbPass( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance + ) { + boolean isReadyToDumbShoot = isReadyToDumbShoot( + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + "Pass" + ); + + Logger.recordOutput(shootingChecksLogPath + "/IsReadyToDumbPass", isReadyToDumbShoot); + + return isReadyToDumbShoot; + } + public static boolean canContinueScoring( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -300,6 +406,28 @@ public static boolean canContinueScoring( return canContinueScoring; } + public static boolean canContinueDumbScoring( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d turretTolerance + ) { + boolean canContinueDumbShooting = canContinueDumbShooting( + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_SCORE_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_SCORE_POSITION, + turretTolerance, + TurretConstants.DUMB_MODE_TURRET_SCORE_POSITION, + "Scoring" + ); + + Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbScoring", canContinueDumbShooting); + + return canContinueDumbShooting; + } + public static boolean canContinuePassing( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -324,6 +452,28 @@ public static boolean canContinuePassing( return canContinuePassing; } + public static boolean canContinueDumbPassing( + Robot robot, + Rotation2d flywheelVelocityToleranceRPS, + Rotation2d hoodPositionTolerance, + Rotation2d headingTolerance + ) { + boolean canContinueDumbShooting = canContinueDumbShooting( + robot, + flywheelVelocityToleranceRPS, + FlywheelConstants.DUMB_MODE_FLYWHEEL_PASS_VELOCITY, + hoodPositionTolerance, + HoodConstants.DUMB_MODE_HOOD_PASS_POSITION, + headingTolerance, + TurretConstants.DUMB_MODE_TURRET_PASS_POSITION, + "Passing" + ); + + Logger.recordOutput(shootingChecksLogPath + "/CanContinueDumbPassing", canContinueDumbShooting); + + return canContinueDumbShooting; + } + public static boolean isOurHubReadyToStartShooting(double distanceFromHubMeters) { boolean isOurHubReadyToStartShooting = HubUtil .isOurHubActive(TimeUtil.getTimeSinceTeleopInitSeconds() + ShootingCalculations.getDistanceToBallFlightTime(distanceFromHubMeters)); diff --git a/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeState.java b/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeState.java index e96668bc4a..4ad22fc98b 100644 --- a/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeState.java +++ b/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeState.java @@ -4,7 +4,7 @@ public enum IntakeState { - CLOSED(Rotation2d.fromDegrees(85), 0.3), + CLOSED(Rotation2d.fromDegrees(90), 0.3), SLOW_CLOSE(Rotation2d.fromDegrees(Double.NaN), 0.3), INTAKE(Rotation2d.fromDegrees(32), 0.9), OUTTAKE(Rotation2d.fromDegrees(32), -0.9), diff --git a/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeStateHandler.java b/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeStateHandler.java index 010f1c2f31..878ec59a71 100644 --- a/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeStateHandler.java +++ b/src/main/java/frc/robot/statemachine/intakestatehandler/IntakeStateHandler.java @@ -76,7 +76,8 @@ public Command intake() { pivot.getCommandsBuilder() .setTargetPosition(IntakeState.INTAKE.getPivotPosition()) .until(() -> pivot.isAtPosition(IntakeState.INTAKE.getPivotPosition(), PivotConstants.POSITION_TOLERANCE_TO_START_REST)), - pivot.getCommandsBuilder().setCurrentWithoutLimit(PivotConstants.REST_CURRENT) + pivot.getCommandsBuilder() + .setCurrentWithoutLimit(() -> isOpenPivotHarder.getAsBoolean() ? PivotConstants.HARD_OPEN : PivotConstants.REST_CURRENT) ), rollers.getCommandsBuilder().setPower(IntakeState.INTAKE.getIntakePower()) ); diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java index 14bc64cd4a..588d2ec5af 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterStateHandler.java @@ -1,5 +1,6 @@ package frc.robot.statemachine.shooterstatehandler; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.*; import frc.robot.Robot; import frc.robot.statemachine.ShootingCalculations; @@ -107,6 +108,27 @@ private Command resetSubsystems() { ); } + public Command setDumbState(Rotation2d turretTargetPosition, Rotation2d hoodTargetPosition, Rotation2d flywheelTargetVelocityRPS) { + Command command = new ParallelCommandGroup( + turret.asSubsystemCommand( + new TurretSafeMoveToPosition( + turret, + () -> turretTargetPosition, + () -> Rotation2d.kZero, + logPath + ), + "Safe move to position" + ), + hood.getCommandsBuilder().setTargetPosition(() -> hoodTargetPosition), + flyWheel.getCommandBuilder().setVelocityAsSupplier(() -> flywheelTargetVelocityRPS) + ); + return new ParallelCommandGroup( + new InstantCommand(() -> Logger.recordOutput(logPath + "/CurrentState", ShooterState.SHOOT.name())), + new InstantCommand(() -> currentState = ShooterState.SHOOT), + command + ); + } + private Command calibration() { return new ParallelCommandGroup( turret.getCommandsBuilder().setTargetPosition(() -> ShootingCalculations.getShootingParams().targetTurretPosition()), diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index b431a8c944..0a47ca9f0d 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -40,6 +40,10 @@ public Arm( commandBuilder = new ArmCommandBuilder(this); setDefaultCommand(commandBuilder.stayInPlace()); } + + public IFeedForwardRequest getPositionRequest() { + return positionRequest; + } public ArmCommandBuilder getCommandsBuilder() { return commandBuilder; diff --git a/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java b/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java index ea28ac5be1..4f2c6f4805 100644 --- a/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/flywheel/FlywheelConstants.java @@ -15,6 +15,9 @@ public class FlywheelConstants { public static final InvertedValue IS_MASTER_INVERTED = InvertedValue.CounterClockwise_Positive; public static final InvertedValue IS_FOLLOWER_INVERTED = InvertedValue.Clockwise_Positive; + public static Rotation2d DUMB_MODE_FLYWHEEL_SCORE_VELOCITY = Rotation2d.fromRotations(60); + public static Rotation2d DUMB_MODE_FLYWHEEL_PASS_VELOCITY = Rotation2d.fromRotations(60); + public static final double kP = 0.3; public static final double kI = 0; public static final double kD = 0; diff --git a/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java index 1ff7cb16b6..6dac61725a 100644 --- a/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/hood/HoodConstants.java @@ -22,6 +22,9 @@ public class HoodConstants { public static final Slot0Configs REAL_SLOT = new Slot0Configs(); public static final Slot0Configs SIMULATION_SLOT = new Slot0Configs(); + public static final Rotation2d DUMB_MODE_HOOD_SCORE_POSITION = Rotation2d.fromDegrees(0); + public static final Rotation2d DUMB_MODE_HOOD_PASS_POSITION = Rotation2d.fromDegrees(0); + static { FEEDBACK_CONFIGS.RotorToSensorRatio = 1; FEEDBACK_CONFIGS.SensorToMechanismRatio = 30.0; diff --git a/src/main/java/frc/robot/subsystems/constants/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/constants/pivot/PivotConstants.java index dd099ffd3a..6561dc8144 100644 --- a/src/main/java/frc/robot/subsystems/constants/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/pivot/PivotConstants.java @@ -20,7 +20,7 @@ public class PivotConstants { public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs(); public static final boolean IS_CONTINUOUS_WRAP = false; - public static final int CURRENT_LIMIT = 30; + public static final int CURRENT_LIMIT = 20; public static final Slot0Configs REAL_SLOT = new Slot0Configs(); public static final Slot0Configs SIMULATION_SLOT = new Slot0Configs(); @@ -56,7 +56,7 @@ public class PivotConstants { public static final double PIVOT_RESET_VOLTAGE = 2; - public static final double CURRENT_THRESHOLD_TO_RESET_POSITION = 25; + public static final double CURRENT_THRESHOLD_TO_RESET_POSITION = 15; public static final double SLOW_CLOSE_VOLTAGE = 2; public static final Rotation2d PIVOT_POSITION_FOR_SLOW_CLOSE = Rotation2d.fromDegrees(85); @@ -68,6 +68,7 @@ public class PivotConstants { public static final Rotation2d POSITION_TOLERANCE_TO_START_REST = Rotation2d.fromDegrees(7); public static final double REST_CURRENT = 0; + public static final double HARD_OPEN = -8; public static CurrentControlArm createPivot() { ArmSimulationConstants pivotSimConstant = new ArmSimulationConstants( diff --git a/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java index 3bf7892a40..64cdf2379f 100644 --- a/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/constants/turret/TurretConstants.java @@ -29,6 +29,10 @@ public class TurretConstants { public static final Slot0Configs REAL_SLOTS_CONFIG = new Slot0Configs(); public static final Slot0Configs SIMULATION_SLOTS_CONFIG = new Slot0Configs(); + public static final Rotation2d DUMB_MODE_TURRET_SCORE_POSITION = Rotation2d.fromDegrees(0); + public static final Rotation2d DUMB_MODE_TURRET_PASS_POSITION = Rotation2d.fromDegrees(0); + + static { FEEDBACK_CONFIGS.SensorToMechanismRatio = 70; diff --git a/src/main/java/frc/robot/subsystems/swerve/states/aimassist/TowerAssistCalculations.java b/src/main/java/frc/robot/subsystems/swerve/states/aimassist/TowerAssistCalculations.java index 882f998806..d141b24dcf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/states/aimassist/TowerAssistCalculations.java +++ b/src/main/java/frc/robot/subsystems/swerve/states/aimassist/TowerAssistCalculations.java @@ -11,7 +11,7 @@ public class TowerAssistCalculations { private static final double ROBOT_CLOSE_TO_DRIVER_STATION_WALL_THRESHOLD_METERS = 1.2; private static final double INTAKE_FACING_DRIVER_STATION_POINT_PRODUCT_THRESHOLD = -0.1; - private static final Translation2d TOWER_ENTRANCE_OFFSET_FROM_TOWER_MIDDLE_METERS = new Translation2d(0.1, 0); + private static final Translation2d TOWER_ENTRANCE_OFFSET_FROM_TOWER_MIDDLE_METERS = new Translation2d(-0.01, 0); public static Translation2d getClosestTowerEntrance(Pose2d robotPose) { Translation2d blueTowerEntrance = Field.TOWER_MIDDLE.plus(TOWER_ENTRANCE_OFFSET_FROM_TOWER_MIDDLE_METERS); diff --git a/src/main/java/frc/utils/time/TimeUtil.java b/src/main/java/frc/utils/time/TimeUtil.java index fac73eab6c..8c4b0f6f8e 100644 --- a/src/main/java/frc/utils/time/TimeUtil.java +++ b/src/main/java/frc/utils/time/TimeUtil.java @@ -13,6 +13,7 @@ public class TimeUtil { private static double lastCycleStartingTimeSeconds = 0; private static double currentCycleStartingTimeSeconds = 0; + private static double autonomousStartTimeSeconds = 0; static { AlertManager.addAlert( @@ -52,4 +53,12 @@ public static double getTimeSinceTeleopInitSeconds() { return TimeUtil.getCurrentTimeSeconds() - RobotManager.getTeleopStartTimeSeconds(); } + public static double getAutonomousStartTimeSeconds() { + return autonomousStartTimeSeconds; + } + + public static void setAutonomousStartTimeSeconds(double autonomousStartTimeSeconds) { + TimeUtil.autonomousStartTimeSeconds = autonomousStartTimeSeconds; + } + }