From db4bab6e16b2c67f3ff907d707f23613a6abb74e Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Sat, 21 Mar 2026 17:07:31 -0400 Subject: [PATCH] center auto works --- .../autos/MOVE B____ left to center.auto | 69 +++++++++++ .../pathplanner/autos/left to center.auto | 34 ++++- .../pathplanner/paths/back from center.path | 12 +- .../paths/left start to center.path | 15 +-- .../paths/over bump to pile move.path | 116 ++++++++++++++++++ .../pathplanner/paths/over bump to pile.path | 18 +-- src/main/java/frc/robot/RobotContainer.java | 9 +- .../frc/robot/constants/ShooterConstants.java | 4 +- 8 files changed, 245 insertions(+), 32 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/MOVE B____ left to center.auto create mode 100644 src/main/deploy/pathplanner/paths/over bump to pile move.path diff --git a/src/main/deploy/pathplanner/autos/MOVE B____ left to center.auto b/src/main/deploy/pathplanner/autos/MOVE B____ left to center.auto new file mode 100644 index 0000000..40ebc7a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MOVE B____ left to center.auto @@ -0,0 +1,69 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake down" + } + }, + { + "type": "path", + "data": { + "pathName": "left start to center" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "over bump to pile move" + } + }, + { + "type": "named", + "data": { + "name": "spinup" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "back from center" + } + }, + { + "type": "named", + "data": { + "name": "spinup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "auto shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/left to center.auto b/src/main/deploy/pathplanner/autos/left to center.auto index 73a0044..dfbe6b5 100644 --- a/src/main/deploy/pathplanner/autos/left to center.auto +++ b/src/main/deploy/pathplanner/autos/left to center.auto @@ -17,15 +17,41 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "over bump to pile" + "commands": [ + { + "type": "path", + "data": { + "pathName": "over bump to pile" + } + }, + { + "type": "named", + "data": { + "name": "spinup" + } + } + ] } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "back from center" + "commands": [ + { + "type": "path", + "data": { + "pathName": "back from center" + } + }, + { + "type": "named", + "data": { + "name": "spinup" + } + } + ] } }, { diff --git a/src/main/deploy/pathplanner/paths/back from center.path b/src/main/deploy/pathplanner/paths/back from center.path index 5a44fbd..593df8b 100644 --- a/src/main/deploy/pathplanner/paths/back from center.path +++ b/src/main/deploy/pathplanner/paths/back from center.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 6.050842639593909, + "x": 6.428375634517765, "y": 5.715482233502538 }, "prevControl": null, "nextControl": { - "x": 5.130030456852792, + "x": 5.507563451776648, "y": 5.7523147208121825 }, "isLocked": false, @@ -42,8 +42,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.5, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -57,7 +57,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -123.34070734647689 + "rotation": -129.95754893082906 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left start to center.path b/src/main/deploy/pathplanner/paths/left start to center.path index 6aedd5b..8612644 100644 --- a/src/main/deploy/pathplanner/paths/left start to center.path +++ b/src/main/deploy/pathplanner/paths/left start to center.path @@ -48,11 +48,11 @@ }, { "anchor": { - "x": 6.050842639593909, + "x": 6.428375634517765, "y": 5.715482233502538 }, "prevControl": { - "x": 5.525979695431473, + "x": 5.903512690355329, "y": 5.807563451776649 }, "nextControl": null, @@ -68,14 +68,7 @@ ], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake Start", - "waypointRelativePos": 2.1, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 2.0, @@ -86,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -123.34070734647689 + "rotation": -129.95754893082906 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/over bump to pile move.path b/src/main/deploy/pathplanner/paths/over bump to pile move.path new file mode 100644 index 0000000..0491180 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/over bump to pile move.path @@ -0,0 +1,116 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.428375634517765, + "y": 5.715482233502538 + }, + "prevControl": null, + "nextControl": { + "x": 6.897989847715735, + "y": 6.406091370558377 + }, + "isLocked": false, + "linkedName": "over bump" + }, + { + "anchor": { + "x": 7.616223350253808, + "y": 7.207197969543147 + }, + "prevControl": { + "x": 6.616223350253808, + "y": 7.207197969543147 + }, + "nextControl": { + "x": 8.616223350253813, + "y": 7.207197969543147 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.417329949238578, + "y": 4.831502538071066 + }, + "prevControl": { + "x": 8.475340101522843, + "y": 6.042370558375635 + }, + "nextControl": { + "x": 8.359319796954313, + "y": 3.620634517766498 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.428375634517765, + "y": 5.715482233502538 + }, + "prevControl": { + "x": 7.375431472081217, + "y": 5.183713197969544 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "over bump" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0045454545454569, + "rotationDegrees": -115.0 + }, + { + "waypointRelativePos": 2.348863636363685, + "rotationDegrees": -115.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1243680485338854, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake Start", + "waypointRelativePos": 0.7886754297269942, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -129.95754893082906 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -129.95754893082906 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/over bump to pile.path b/src/main/deploy/pathplanner/paths/over bump to pile.path index 1760a40..ae04d09 100644 --- a/src/main/deploy/pathplanner/paths/over bump to pile.path +++ b/src/main/deploy/pathplanner/paths/over bump to pile.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 6.050842639593909, + "x": 6.428375634517765, "y": 5.715482233502538 }, "prevControl": null, "nextControl": { - "x": 6.520456852791878, + "x": 6.897989847715735, "y": 6.406091370558377 }, "isLocked": false, @@ -48,22 +48,26 @@ }, { "anchor": { - "x": 6.050842639593909, + "x": 6.428375634517765, "y": 5.715482233502538 }, "prevControl": { - "x": 6.99789847715736, + "x": 7.375431472081217, "y": 5.183713197969544 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "over bump" } ], "rotationTargets": [ { "waypointRelativePos": 1.0045454545454569, "rotationDegrees": -115.0 + }, + { + "waypointRelativePos": 2.348863636363685, + "rotationDegrees": -115.0 } ], "constraintZones": [ @@ -100,13 +104,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -120.34324888423971 + "rotation": -129.95754893082906 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -123.34070734647689 + "rotation": -129.95754893082906 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 770104c..c716e17 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -391,7 +391,7 @@ public class RobotContainer { NamedCommands.registerCommand("spinup", shooter.maintainSpeed(ShooterSpeeds.kHubSpeed) - .withTimeout(3)); + .withTimeout(2)); NamedCommands.registerCommand("shoot close", spindexer.spinToShooter() @@ -405,6 +405,10 @@ public class RobotContainer { .onTrue( intakeRoller.runIn()); + new EventTrigger("windup trigger") + .onTrue( + shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)); + NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop()); NamedCommands.registerCommand("jimmy", @@ -420,7 +424,8 @@ public class RobotContainer { ).withTimeout(3).andThen(spindexer.instantaneousStop())); - NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter)); + NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter) + .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), spindexer.spinToShooter(), intakePivot.jimmy(0.5))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index c549d41..efccf44 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -43,14 +43,14 @@ public class ShooterConstants { public static final boolean kLeftShooterMotorInverted = true; public static final boolean kRightShooterMotorInverted = false; - public static final double kLeftP = 0.1;//0.01;//0.001; + public static final double kLeftP = 0.5;//0.01;//0.001; public static final double kLeftI = 0; public static final double kLeftD = 0;//0.1;//1.8; public static final double kLeftS = 0; public static final double kLeftV = 0.00129; public static final double kLeftA = 0; - public static final double kRightP = 0.1;//0.001;//0.001; + public static final double kRightP = 0.5;//0.001;//0.001; public static final double kRightI = 0; public static final double kRightD = 0;//0.1; public static final double kRightS = 0;