diff --git a/mirror-autos.py b/mirror-autos.py index 8c2b23e9..3bf193e4 100644 --- a/mirror-autos.py +++ b/mirror-autos.py @@ -87,6 +87,9 @@ def main(): outpost_paths = [ "1st outpost shoot", "fast outpost shoot", + "outpost flex pickup", + "outpost flex bump", + "outpost flex center", "2nd outpost cleanup", "2nd outpost shoot", "outpost center half neutral zone" @@ -96,7 +99,9 @@ def main(): outpost_autos = [ "outpost cleanup", "outpost fast", - "outpost shoot" + "outpost shoot", + "outpost flex", + "outpost center flex" ] # Mirror paths diff --git a/src/main/deploy/pathplanner/autos/depot center flex.auto b/src/main/deploy/pathplanner/autos/depot center flex.auto new file mode 100644 index 00000000..36966f79 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/depot center flex.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "depot flex bump" + } + }, + { + "type": "named", + "data": { + "name": "variable wait command" + } + }, + { + "type": "path", + "data": { + "pathName": "depot flex center" + } + }, + { + "type": "named", + "data": { + "name": "pointwheelsinx" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/depot flex.auto b/src/main/deploy/pathplanner/autos/depot flex.auto new file mode 100644 index 00000000..8897b00e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/depot flex.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "depot flex bump" + } + }, + { + "type": "named", + "data": { + "name": "variable wait command" + } + }, + { + "type": "path", + "data": { + "pathName": "depot flex pickup" + } + }, + { + "type": "named", + "data": { + "name": "snap to hub flex command" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/outpost center flex.auto b/src/main/deploy/pathplanner/autos/outpost center flex.auto new file mode 100644 index 00000000..1bc49f8a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/outpost center flex.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "outpost flex bump" + } + }, + { + "type": "named", + "data": { + "name": "variable wait command" + } + }, + { + "type": "path", + "data": { + "pathName": "outpost flex center" + } + }, + { + "type": "named", + "data": { + "name": "pointwheelsinx" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/outpost flex.auto b/src/main/deploy/pathplanner/autos/outpost flex.auto new file mode 100644 index 00000000..08f6350d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/outpost flex.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "outpost flex bump" + } + }, + { + "type": "named", + "data": { + "name": "variable wait command" + } + }, + { + "type": "path", + "data": { + "pathName": "outpost flex pickup" + } + }, + { + "type": "named", + "data": { + "name": "snap to hub flex command" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot flex bump.path b/src/main/deploy/pathplanner/paths/depot flex bump.path new file mode 100644 index 00000000..a6ef2f54 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot flex bump.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.679, + "y": 5.4510000000000005 + }, + "prevControl": null, + "nextControl": { + "x": 4.679, + "y": 5.4510000000000005 + }, + "isLocked": false, + "linkedName": "depot neutral zone start" + }, + { + "anchor": { + "x": 5.559956958912037, + "y": 5.4510000000000005 + }, + "prevControl": { + "x": 4.559956958912037, + "y": 5.4510000000000005 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "depot flex wait" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deploy intake trigger", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot flex center.path b/src/main/deploy/pathplanner/paths/depot flex center.path new file mode 100644 index 00000000..338e3720 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot flex center.path @@ -0,0 +1,106 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.559956958912037, + "y": 5.4510000000000005 + }, + "prevControl": null, + "nextControl": { + "x": 6.559956958912037, + "y": 5.4510000000000005 + }, + "isLocked": false, + "linkedName": "depot flex wait" + }, + { + "anchor": { + "x": 6.452785083912038, + "y": 7.299567418981482 + }, + "prevControl": { + "x": 6.394654152199075, + "y": 6.613609302662038 + }, + "nextControl": { + "x": 6.473895452122505, + "y": 7.548674529185142 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.222563585069445, + "y": 6.884646412037037 + }, + "prevControl": { + "x": 8.194941550925927, + "y": 7.884550925925927 + }, + "nextControl": { + "x": 8.239274483811554, + "y": 6.279719816241851 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.222563585069445, + "y": 4.02743894675926 + }, + "prevControl": { + "x": 8.24040964988426, + "y": 4.71680881076389 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.493687913907287, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 2.342922185430464, + "rotationDegrees": -110.0 + }, + { + "waypointRelativePos": 2.91, + "rotationDegrees": -110.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake trigger", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot flex pickup.path b/src/main/deploy/pathplanner/paths/depot flex pickup.path new file mode 100644 index 00000000..e776aa1a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot flex pickup.path @@ -0,0 +1,158 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.559956958912037, + "y": 5.4510000000000005 + }, + "prevControl": null, + "nextControl": { + "x": 5.909295402082991, + "y": 5.587837798377319 + }, + "isLocked": false, + "linkedName": "depot flex wait" + }, + { + "anchor": { + "x": 7.407549551504631, + "y": 5.576766059027778 + }, + "prevControl": { + "x": 6.930470304294506, + "y": 5.918058125183217 + }, + "nextControl": { + "x": 7.759287615740742, + "y": 5.325140335648149 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.952868561921297, + "y": 4.385803674768519 + }, + "prevControl": { + "x": 7.832411026228868, + "y": 4.399262124437204 + }, + "nextControl": { + "x": 5.456423538773148, + "y": 4.362905598958334 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.559956958912037, + "y": 5.4510000000000005 + }, + "prevControl": { + "x": 5.5497217158564816, + "y": 4.663795500578704 + }, + "nextControl": { + "x": 5.563800393852754, + "y": 5.7466030708936815 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.262, + "y": 5.4510000000000005 + }, + "prevControl": { + "x": 3.5033209700115195, + "y": 5.385699238651457 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "depot close shooting point" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 1.147690850815852, + "rotationDegrees": -100.0 + }, + { + "waypointRelativePos": 1.8467548076923106, + "rotationDegrees": 167.61604660845643 + }, + { + "waypointRelativePos": 2.35, + "rotationDegrees": -180.0 + }, + { + "waypointRelativePos": 3.0033143939393963, + "rotationDegrees": -180.0 + }, + { + "waypointRelativePos": 3.8491307947019875, + "rotationDegrees": -180.0 + } + ], + "constraintZones": [ + { + "name": "come back over the bump", + "minWaypointRelativePos": 3.3041964285714287, + "maxWaypointRelativePos": 3.6382142857142843, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake trigger", + "waypointRelativePos": 0.2, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "shoot flex trigger", + "waypointRelativePos": 3.84, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -46.24 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/outpost flex bump.path b/src/main/deploy/pathplanner/paths/outpost flex bump.path new file mode 100644 index 00000000..47593302 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/outpost flex bump.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.679, + "y": 2.618 + }, + "prevControl": null, + "nextControl": { + "x": 4.679, + "y": 2.6180000000000003 + }, + "isLocked": false, + "linkedName": "outpost neutral zone start" + }, + { + "anchor": { + "x": 5.559956958912037, + "y": 2.618 + }, + "prevControl": { + "x": 4.559956958912037, + "y": 2.618 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "outpost flex wait" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deploy intake trigger", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/outpost flex center.path b/src/main/deploy/pathplanner/paths/outpost flex center.path new file mode 100644 index 00000000..94feb25d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/outpost flex center.path @@ -0,0 +1,106 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.559956958912037, + "y": 2.618 + }, + "prevControl": null, + "nextControl": { + "x": 6.559956958912037, + "y": 2.618 + }, + "isLocked": false, + "linkedName": "outpost flex wait" + }, + { + "anchor": { + "x": 6.452785083912038, + "y": 0.7694325810185183 + }, + "prevControl": { + "x": 6.394654152199075, + "y": 1.455390697337963 + }, + "nextControl": { + "x": 6.473895452122505, + "y": 0.5203254708148592 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.222563585069445, + "y": 1.1843535879629632 + }, + "prevControl": { + "x": 8.194941550925927, + "y": 0.18444907407407418 + }, + "nextControl": { + "x": 8.239274483811554, + "y": 1.7892801837581498 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.222563585069445, + "y": 4.04156105324074 + }, + "prevControl": { + "x": 8.24040964988426, + "y": 3.352191189236111 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.493687913907287, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.342922185430464, + "rotationDegrees": 110.0 + }, + { + "waypointRelativePos": 2.91, + "rotationDegrees": 110.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake trigger", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/outpost flex pickup.path b/src/main/deploy/pathplanner/paths/outpost flex pickup.path new file mode 100644 index 00000000..3fa2752f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/outpost flex pickup.path @@ -0,0 +1,158 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.559956958912037, + "y": 2.618 + }, + "prevControl": null, + "nextControl": { + "x": 5.909295402082991, + "y": 2.4811622016226815 + }, + "isLocked": false, + "linkedName": "outpost flex wait" + }, + { + "anchor": { + "x": 7.407549551504631, + "y": 2.4922339409722225 + }, + "prevControl": { + "x": 6.930470304294506, + "y": 2.1509418748167835 + }, + "nextControl": { + "x": 7.759287615740742, + "y": 2.7438596643518527 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.952868561921297, + "y": 3.6831963252314814 + }, + "prevControl": { + "x": 7.832411026228868, + "y": 3.6697378755627974 + }, + "nextControl": { + "x": 5.456423538773148, + "y": 3.7060944010416668 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.559956958912037, + "y": 2.618 + }, + "prevControl": { + "x": 5.5497217158564816, + "y": 3.405204499421296 + }, + "nextControl": { + "x": 5.563800393852754, + "y": 2.3223969291063193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.262, + "y": 2.618 + }, + "prevControl": { + "x": 3.5033209700115195, + "y": 2.6833007613485447 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "outpost close shooting point" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.147690850815852, + "rotationDegrees": 100.0 + }, + { + "waypointRelativePos": 1.8467548076923106, + "rotationDegrees": -167.61604660845643 + }, + { + "waypointRelativePos": 2.35, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.0033143939393963, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.8491307947019875, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "come back over the bump", + "minWaypointRelativePos": 3.3041964285714287, + "maxWaypointRelativePos": 3.6382142857142843, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake trigger", + "waypointRelativePos": 0.2, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "shoot flex trigger", + "waypointRelativePos": 3.84, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 46.24 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f55e41b0..50daea21 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -725,6 +725,9 @@ public static class SensorConstants { // placeholder constants public static class AutoConstants { public static final double AUTO_SHOOT_TIMEOUT = 2.53; // in seconds public static final double AUTO_SHOOT_CLEANUP_TIMEOUT = 5; + public static final double AUTO_SHOOT_FLEX_TIMEOUT = 5; + + public static final double DEFAULT_WAIT_TIME = 2; // in seconds } public static class TeleopConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 40088986..a6bda7fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -460,6 +460,8 @@ public void logToSmartDashboard() { SmartDashboard.putNumber("robotCommandFactory/angleToHubRadians", Math.toRadians(m_robotCommandFactory.getAngleToHub())); SmartDashboard.putNumber("robotCommandFactory/dashboardTestShooterVelocityRotationsPerSecond", m_robotCommandFactory.getTargetShooterVelocity()); + + SmartDashboard.putNumber("auton/waitTime", SmartDashboard.getNumber("auton/waitTime", AutoConstants.DEFAULT_WAIT_TIME)); } @@ -587,7 +589,15 @@ private void createNamedCommands() { new EventTrigger("intake trigger").onTrue(m_robotCommandFactory.autoIntakeModeCommand()); new EventTrigger("shoot trigger").onTrue(m_robotCommandFactory.autoShootWithVisionCommand().withTimeout(AutoConstants.AUTO_SHOOT_TIMEOUT)); // TODO: tune timeout new EventTrigger("shoot cleanup trigger").onTrue(m_robotCommandFactory.autoShootWithVisionCommand().withTimeout(AutoConstants.AUTO_SHOOT_CLEANUP_TIMEOUT)); + new EventTrigger("shoot flex trigger").onTrue(m_robotCommandFactory.autoShootWithVisionCommand().withTimeout(AutoConstants.AUTO_SHOOT_FLEX_TIMEOUT)); + + NamedCommands.registerCommand("variable wait command", + m_robotCommandFactory.variableWaitCommand(() -> SmartDashboard.getNumber("auton/waitTime", AutoConstants.DEFAULT_WAIT_TIME))); + NamedCommands.registerCommand("snap to hub flex command", + m_robotCommandFactory.snapToHubCommand(() -> new JoystickVals(0, 0)).withTimeout(AutoConstants.AUTO_SHOOT_FLEX_TIMEOUT)); + NamedCommands.registerCommand("pointwheelsinx", + m_drivetrainCommandFactory.pointWheelsinX()); NamedCommands.registerCommand("intake command", m_robotCommandFactory.intakeModeCommand()); // DOES NOT END NamedCommands.registerCommand("deploy intake command", diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index cef7e518..36fe7c48 100644 --- a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java @@ -2,6 +2,7 @@ import java.util.Optional; import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import edu.wpi.first.math.filter.Debouncer.DebounceType; @@ -720,6 +721,21 @@ public Command autoShootWithVisionDynamicCommand() { ).withName("autoShootWithVisionDynamic"); } + /** + * Creates a wait command with a variable duration determined by a DoubleSupplier. + * The duration is evaluated when the command is scheduled. + * + * @param durationSupplier Supplier that provides the wait duration in seconds + * @return Command that waits for the supplied duration + */ + public Command variableWaitCommand(DoubleSupplier durationSupplier) { + return Commands.defer( + () -> new WaitCommand(durationSupplier.getAsDouble()), + java.util.Set.of() + ).withName("variableWaitCommand"); + + } + public Command autoZeroPivotCommand() { return Commands.parallel( Commands.sequence(