diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 963e026..ed8b9d3 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -5,7 +5,7 @@ "pathFolders": [], "autoFolders": [], "defaultMaxVel": 5.5, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 6.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 5.5 diff --git a/simgui.json b/simgui.json index 114432b..fffd7e7 100644 --- a/simgui.json +++ b/simgui.json @@ -4,6 +4,7 @@ "/FMSInfo": "FMSInfo", "/Shuffleboard/Climber/PID_Controller": "ProfiledPIDController", "/Shuffleboard/Commands/Climber": "Subsystem", + "/Shuffleboard/Commands/CommandScheduler": "Scheduler", "/Shuffleboard/Commands/IntakePivot": "Subsystem", "/Shuffleboard/Commands/IntakeRollers": "Subsystem", "/Shuffleboard/Commands/Shooter": "Subsystem", diff --git a/src/main/deploy/pathplanner/autos/fu.auto b/src/main/deploy/pathplanner/autos/fu.auto new file mode 100644 index 0000000..734057c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/fu.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.43, + "y": 7.67 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RushE" + } + }, + { + "type": "path", + "data": { + "pathName": "note2.1-wing" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "wing-note2.2" + } + }, + { + "type": "path", + "data": { + "pathName": "note2.2-wing" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "wing-note2.3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/su.auto b/src/main/deploy/pathplanner/autos/su.auto new file mode 100644 index 0000000..62edd5d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/su.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4311279977766789, + "y": 7.67 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot2" + } + }, + { + "type": "path", + "data": { + "pathName": "suicide" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RushE.path b/src/main/deploy/pathplanner/paths/RushE.path new file mode 100644 index 0000000..a1c3532 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RushE.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.43, + "y": 7.67 + }, + "prevControl": null, + "nextControl": { + "x": 2.4299999999999997, + "y": 7.67 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.3, + "y": 7.39 + }, + "prevControl": { + "x": 7.3876653736703775, + "y": 7.799445392696006 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom-note1.3.path b/src/main/deploy/pathplanner/paths/bottom-note1.3.path index 584cbb6..c06142a 100644 --- a/src/main/deploy/pathplanner/paths/bottom-note1.3.path +++ b/src/main/deploy/pathplanner/paths/bottom-note1.3.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/bottom-note2.4.path b/src/main/deploy/pathplanner/paths/bottom-note2.4.path index 639a77a..ee7af90 100644 --- a/src/main/deploy/pathplanner/paths/bottom-note2.4.path +++ b/src/main/deploy/pathplanner/paths/bottom-note2.4.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/bottom-note2.5.path b/src/main/deploy/pathplanner/paths/bottom-note2.5.path index 8f6eedf..c3bf970 100644 --- a/src/main/deploy/pathplanner/paths/bottom-note2.5.path +++ b/src/main/deploy/pathplanner/paths/bottom-note2.5.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/leave.path b/src/main/deploy/pathplanner/paths/leave.path index a36d51d..216bec6 100644 --- a/src/main/deploy/pathplanner/paths/leave.path +++ b/src/main/deploy/pathplanner/paths/leave.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/midBottom-note1.3.path b/src/main/deploy/pathplanner/paths/midBottom-note1.3.path index 704ba38..6557573 100644 --- a/src/main/deploy/pathplanner/paths/midBottom-note1.3.path +++ b/src/main/deploy/pathplanner/paths/midBottom-note1.3.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/midBottom-note2.3.path b/src/main/deploy/pathplanner/paths/midBottom-note2.3.path index 54e19f5..95a4456 100644 --- a/src/main/deploy/pathplanner/paths/midBottom-note2.3.path +++ b/src/main/deploy/pathplanner/paths/midBottom-note2.3.path @@ -17,11 +17,11 @@ { "anchor": { "x": 7.8, - "y": 4.05 + "y": 4.15 }, "prevControl": { "x": 6.3, - "y": 4.05 + "y": 4.15 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/midTop-note1.1.path b/src/main/deploy/pathplanner/paths/midTop-note1.1.path index 047516e..8f3a047 100644 --- a/src/main/deploy/pathplanner/paths/midTop-note1.1.path +++ b/src/main/deploy/pathplanner/paths/midTop-note1.1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.75, - "y": 7.45 + "x": 8.3, + "y": 7.39 }, "prevControl": { - "x": 4.75, - "y": 7.45 + "x": 5.300000000000001, + "y": 7.39 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/middle-note1.2.path b/src/main/deploy/pathplanner/paths/middle-note1.2.path index fead47e..1d8afb1 100644 --- a/src/main/deploy/pathplanner/paths/middle-note1.2.path +++ b/src/main/deploy/pathplanner/paths/middle-note1.2.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/middle-note1.3.path b/src/main/deploy/pathplanner/paths/middle-note1.3.path index 3cf0209..e2e9cc8 100644 --- a/src/main/deploy/pathplanner/paths/middle-note1.3.path +++ b/src/main/deploy/pathplanner/paths/middle-note1.3.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.1-leave.path b/src/main/deploy/pathplanner/paths/note1.1-leave.path index 564bdb7..5fca134 100644 --- a/src/main/deploy/pathplanner/paths/note1.1-leave.path +++ b/src/main/deploy/pathplanner/paths/note1.1-leave.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.1-note1.2.path b/src/main/deploy/pathplanner/paths/note1.1-note1.2.path index 0c102a2..432c83b 100644 --- a/src/main/deploy/pathplanner/paths/note1.1-note1.2.path +++ b/src/main/deploy/pathplanner/paths/note1.1-note1.2.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.1-note2.1.path b/src/main/deploy/pathplanner/paths/note1.1-note2.1.path index 720313d..3ba4e72 100644 --- a/src/main/deploy/pathplanner/paths/note1.1-note2.1.path +++ b/src/main/deploy/pathplanner/paths/note1.1-note2.1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.75, - "y": 7.45 + "x": 8.3, + "y": 7.39 }, "prevControl": { - "x": 7.652000140544798, - "y": 7.430099559131505 + "x": 8.202000140544799, + "y": 7.370099559131504 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.2-note1.1.path b/src/main/deploy/pathplanner/paths/note1.2-note1.1.path index 843e424..f127e95 100644 --- a/src/main/deploy/pathplanner/paths/note1.2-note1.1.path +++ b/src/main/deploy/pathplanner/paths/note1.2-note1.1.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.2-note1.3.path b/src/main/deploy/pathplanner/paths/note1.2-note1.3.path index 9413b04..87c8545 100644 --- a/src/main/deploy/pathplanner/paths/note1.2-note1.3.path +++ b/src/main/deploy/pathplanner/paths/note1.2-note1.3.path @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.2-note2.3.path b/src/main/deploy/pathplanner/paths/note1.2-note2.3.path index 101feac..1795ee2 100644 --- a/src/main/deploy/pathplanner/paths/note1.2-note2.3.path +++ b/src/main/deploy/pathplanner/paths/note1.2-note2.3.path @@ -33,11 +33,11 @@ { "anchor": { "x": 7.8, - "y": 4.05 + "y": 4.15 }, "prevControl": { "x": 6.8, - "y": 4.05 + "y": 4.15 }, "nextControl": null, "isLocked": false, @@ -67,7 +67,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.3-leave.path b/src/main/deploy/pathplanner/paths/note1.3-leave.path index 1581b1f..4c248e1 100644 --- a/src/main/deploy/pathplanner/paths/note1.3-leave.path +++ b/src/main/deploy/pathplanner/paths/note1.3-leave.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.3-midLine.path b/src/main/deploy/pathplanner/paths/note1.3-midLine.path index e899032..03a21e1 100644 --- a/src/main/deploy/pathplanner/paths/note1.3-midLine.path +++ b/src/main/deploy/pathplanner/paths/note1.3-midLine.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.3-note1.2.path b/src/main/deploy/pathplanner/paths/note1.3-note1.2.path index 0178afa..dec2b19 100644 --- a/src/main/deploy/pathplanner/paths/note1.3-note1.2.path +++ b/src/main/deploy/pathplanner/paths/note1.3-note1.2.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note1.3-note2.4.path b/src/main/deploy/pathplanner/paths/note1.3-note2.4.path index edf4c47..a5dcb4c 100644 --- a/src/main/deploy/pathplanner/paths/note1.3-note2.4.path +++ b/src/main/deploy/pathplanner/paths/note1.3-note2.4.path @@ -68,7 +68,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note2.1-wing.path b/src/main/deploy/pathplanner/paths/note2.1-wing.path index ac2d8b1..44968ca 100644 --- a/src/main/deploy/pathplanner/paths/note2.1-wing.path +++ b/src/main/deploy/pathplanner/paths/note2.1-wing.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 7.75, - "y": 7.45 + "x": 8.3, + "y": 7.39 }, "prevControl": null, "nextControl": { - "x": 7.784146393643735, - "y": 7.361219376526284 + "x": 8.334146393643735, + "y": 7.301219376526284 }, "isLocked": false, "linkedName": "note2.1" }, { "anchor": { - "x": 3.0, + "x": 4.5, "y": 6.59034094717495 }, "prevControl": { - "x": 2.9927644030104235, + "x": 4.4927644030104235, "y": 6.5968448364375325 }, "nextControl": null, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note2.2-wing.path b/src/main/deploy/pathplanner/paths/note2.2-wing.path index 7eaed13..e6bfb3a 100644 --- a/src/main/deploy/pathplanner/paths/note2.2-wing.path +++ b/src/main/deploy/pathplanner/paths/note2.2-wing.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.805601618871062, + "x": 8.3, "y": 5.7875434900537055 }, "prevControl": null, "nextControl": { - "x": 7.71519145759367, + "x": 8.209589838722609, "y": 5.830275243326839 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.0, + "x": 4.5, "y": 6.59034094717495 }, "prevControl": { - "x": 3.093301344171903, + "x": 4.593301344171903, "y": 6.554356795445111 }, "nextControl": null, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note2.3-midBottom.path b/src/main/deploy/pathplanner/paths/note2.3-midBottom.path index 6348e5a..9d15607 100644 --- a/src/main/deploy/pathplanner/paths/note2.3-midBottom.path +++ b/src/main/deploy/pathplanner/paths/note2.3-midBottom.path @@ -4,12 +4,12 @@ { "anchor": { "x": 7.8, - "y": 4.05 + "y": 4.15 }, "prevControl": null, "nextControl": { "x": 6.3, - "y": 4.05 + "y": 4.15 }, "isLocked": false, "linkedName": "note2.3" @@ -68,7 +68,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note2.3-middle.path b/src/main/deploy/pathplanner/paths/note2.3-middle.path index b841078..8b120da 100644 --- a/src/main/deploy/pathplanner/paths/note2.3-middle.path +++ b/src/main/deploy/pathplanner/paths/note2.3-middle.path @@ -4,12 +4,12 @@ { "anchor": { "x": 7.8, - "y": 4.05 + "y": 4.15 }, "prevControl": null, "nextControl": { "x": 4.8, - "y": 4.05 + "y": 4.15 }, "isLocked": false, "linkedName": "note2.3" @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "RampUp", - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.7, "command": { "type": "parallel", "data": { @@ -68,7 +68,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note2.3-wing.path b/src/main/deploy/pathplanner/paths/note2.3-wing.path index 81a3fc8..8b73cd0 100644 --- a/src/main/deploy/pathplanner/paths/note2.3-wing.path +++ b/src/main/deploy/pathplanner/paths/note2.3-wing.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.0, + "x": 4.5, "y": 6.59034094717495 }, "prevControl": { - "x": 7.5, + "x": 9.0, "y": 6.59034094717495 }, "nextControl": null, @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note2.4-bottom.path b/src/main/deploy/pathplanner/paths/note2.4-bottom.path index 5356492..73f7351 100644 --- a/src/main/deploy/pathplanner/paths/note2.4-bottom.path +++ b/src/main/deploy/pathplanner/paths/note2.4-bottom.path @@ -85,7 +85,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/note2.5-bottom.path b/src/main/deploy/pathplanner/paths/note2.5-bottom.path index 477c1c0..75df2d5 100644 --- a/src/main/deploy/pathplanner/paths/note2.5-bottom.path +++ b/src/main/deploy/pathplanner/paths/note2.5-bottom.path @@ -68,7 +68,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/suicide.path b/src/main/deploy/pathplanner/paths/suicide.path new file mode 100644 index 0000000..f60ac96 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/suicide.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4311279977766789, + "y": 7.670597937486888 + }, + "prevControl": null, + "nextControl": { + "x": 2.431127997776679, + "y": 7.670597937486888 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.3, + "y": 7.39 + }, + "prevControl": { + "x": 7.386554749429043, + "y": 7.7998792791023535 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top-note1.1.path b/src/main/deploy/pathplanner/paths/top-note1.1.path index 3d6ddc7..a047f19 100644 --- a/src/main/deploy/pathplanner/paths/top-note1.1.path +++ b/src/main/deploy/pathplanner/paths/top-note1.1.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/top-note2.1.path b/src/main/deploy/pathplanner/paths/top-note2.1.path index ff9430b..47057f3 100644 --- a/src/main/deploy/pathplanner/paths/top-note2.1.path +++ b/src/main/deploy/pathplanner/paths/top-note2.1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.75, - "y": 7.45 + "x": 8.3, + "y": 7.39 }, "prevControl": { - "x": 4.750260032654866, - "y": 7.410501540381698 + "x": 5.300260032654867, + "y": 7.350501540381697 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/wing-note1.1.path b/src/main/deploy/pathplanner/paths/wing-note1.1.path index 695b6d2..5b5475f 100644 --- a/src/main/deploy/pathplanner/paths/wing-note1.1.path +++ b/src/main/deploy/pathplanner/paths/wing-note1.1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.0, + "x": 4.5, "y": 6.59034094717495 }, "prevControl": null, "nextControl": { - "x": 2.871195284057235, + "x": 4.371195284057235, "y": 6.589408890804183 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/wing-note2.2.path b/src/main/deploy/pathplanner/paths/wing-note2.2.path index 3e538a9..2e9b00c 100644 --- a/src/main/deploy/pathplanner/paths/wing-note2.2.path +++ b/src/main/deploy/pathplanner/paths/wing-note2.2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.0, + "x": 4.5, "y": 6.59034094717495 }, "prevControl": null, "nextControl": { - "x": 3.088200159023285, + "x": 4.588200159023285, "y": 6.543216304489119 }, "isLocked": false, @@ -16,24 +16,24 @@ }, { "anchor": { - "x": 7.805601618871062, + "x": 8.3, "y": 5.7875434900537055 }, "prevControl": { - "x": 7.726290335922699, + "x": 8.220688717051637, "y": 5.848450964119576 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "note2.2" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [ { - "name": "New Event Marker", - "waypointRelativePos": 0.5, + "name": "Intake", + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/wing-note2.3.path b/src/main/deploy/pathplanner/paths/wing-note2.3.path index fba218b..badb50c 100644 --- a/src/main/deploy/pathplanner/paths/wing-note2.3.path +++ b/src/main/deploy/pathplanner/paths/wing-note2.3.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.0, + "x": 4.5, "y": 6.59034094717495 }, "prevControl": null, "nextControl": { - "x": 7.5, + "x": 6.5, "y": 6.59034094717495 }, "isLocked": false, @@ -30,28 +30,10 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.35, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 1a3962b..994e8b8 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -92,8 +92,9 @@ private void configureButtonBindings() { controller.getButton(XboxButton.kRightBumper).onTrue(rampUp(ShooterConstants.MAX_RPM, 27)).onFalse(shoot(ShooterConstants.MAX_RPM, 25)); //Ram Shot controller.getButton(XboxButton.kRightTrigger).onTrue(rampUpContinuous()).onFalse(autoShoot()); //Auto Shoot - controller.getButton(XboxButton.kY).onTrue(rampUp(5000, 15)).onFalse(feed(5000, 15,155)); //Feed Shot + controller.getButton(XboxButton.kY).onTrue(rampUp(5000, 15)).onFalse(feed(5000, 15)); //Feed Shot controller.getButton(XboxButton.kX).onTrue(intake.intakePivot.pivotTo(-55)).onFalse(ampShoot()); //Amp Shot + // controller.getButton(XboxButton.kX).onTrue(intake.intakePivot.pivotTo(-87)).onFalse(ampShootAlt()); controller.getButton(XboxButton.kA).onTrue(sequence(runOnce(()-> intake.isRetracting = false), intake.intakePivot.pivotTo(-150), climber.climbTo(Climber.Setpoint.EXTENDED))); //Extend Climber controller.getButton(XboxButton.kBack).onTrue(sequence(climber.setClimber(-0.35), waitSeconds(1), climber.setClimber(-1), waitUntil(()->climber.isClimbed()), climber.setClimber(0))); //Retract Climber diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index c841158..84087f3 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -27,6 +27,8 @@ public AutoPrograms() { private void initAutoSelector() { final String[] autoStrings = new String[] { + "fu", + "su", "bottom_2note", "bottom_3note_mid", "bottomRush_3note", diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index 9e0ff58..113c69a 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -21,6 +21,7 @@ import static frc.team3128.Constants.AutoConstants.*; import static frc.team3128.Constants.FocalAimConstants.focalPointBlue; import static frc.team3128.Constants.FocalAimConstants.focalPointRed; +import static frc.team3128.Constants.ShooterConstants.MAX_RPM; import static frc.team3128.Constants.SwerveConstants.*; import frc.team3128.Constants.ShooterConstants; @@ -53,6 +54,7 @@ public static void initTrajectories() { NamedCommands.registerCommand("Intake", intake.intakeAuto()); NamedCommands.registerCommand("Shoot", autoShoot()); NamedCommands.registerCommand("ShootFast", autoShootNoTurn()); + NamedCommands.registerCommand("Shoot2", autoShoot2()); NamedCommands.registerCommand("RampUp", rampUpAuto()); NamedCommands.registerCommand("Retract", intake.retractAuto()); NamedCommands.registerCommand("Amp", null); @@ -93,6 +95,18 @@ public static Command turnInPlace() { ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn())); } + public static Command turnInPlace2() { + DoubleSupplier setpoint = ()-> Robot.getAlliance() == Alliance.Red ? 305 - 10 : -125 + 10; + return new NAR_PIDCommand( + TURN_CONTROLLER, + ()-> swerve.getYaw(), //measurement + setpoint, //setpoint + (double output) -> { + Swerve.getInstance().drive(new Translation2d(), Units.degreesToRadians(output), true); + } + ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn())); + } + public static Command rampUpAuto() { return sequence( either(intake.retractAuto(), none(), ()-> intake.intakePivot.isEnabled()), @@ -127,6 +141,23 @@ public static Command autoShoot() { ); } + public static Command autoShoot2() { + return sequence( + runOnce(()-> turning = true), + parallel( + rampUp(MAX_RPM, 12.94), + turnInPlace2().withTimeout(0.75) + // runOnce(()-> CmdSwerveDrive.setTurnSetpoint(swerve.getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue))), + // waitUntil(()-> CmdSwerveDrive.rController.atSetpoint()) + ), + // waitSeconds(1), + runOnce(()->{turning = false;}), + intake.intakeRollers.outtakeNoRequirements(), + waitSeconds(0.1), + neutralAuto() + ); + } + public static Command neutralAuto() { return sequence( intake.intakeRollers.runNoRequirements(0), diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 25b9747..da004fb 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -3,9 +3,11 @@ import common.hardware.input.NAR_XboxController; import common.utility.Log; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.team3128.Robot; import frc.team3128.RobotContainer; import frc.team3128.Constants.IntakeConstants; import frc.team3128.Constants.ShooterConstants; @@ -49,6 +51,16 @@ public static Command autoShoot() { ); } + public static Command ampShootAlt() { + return sequence( + intake.intakePivot.pivotNoRequirements(-87), + waitUntil(()-> intake.intakePivot.atSetpoint()), + intake.intakeRollers.runManipulator(-0.2875), + waitSeconds(0.2), + intake.retract(false) + ); + } + public static Command ampShoot() { return sequence ( intake.intakePivot.pivotNoRequirements(-72.5), @@ -70,12 +82,12 @@ public static Command shoot(double rpm, double height){ ); } - public static Command feed(double rpm, double height, double angle){ + public static Command feed(double rpm, double height){ return sequence( - runOnce(()-> { - CmdSwerveDrive.setTurnSetpoint(angle); - }), - rampUp(rpm, height), + parallel( + swerve.turnInPlace(()-> Robot.getAlliance() == Alliance.Blue ? 155 : 35).asProxy().withTimeout(1), + rampUp(rpm, height) + ), intake.intakeRollers.outtakeNoRequirements(), waitSeconds(0.1), neutral(false) diff --git a/src/main/java/frc/team3128/subsystems/Climber.java b/src/main/java/frc/team3128/subsystems/Climber.java index 6685c72..76fda72 100644 --- a/src/main/java/frc/team3128/subsystems/Climber.java +++ b/src/main/java/frc/team3128/subsystems/Climber.java @@ -103,7 +103,7 @@ public Command reset(){ } public double interpolate(double dist){ - return 25 * Math.pow(dist, -0.75); + return 25 * Math.pow(dist, -0.92); // return 45.1 - 27 * dist + 6.86 * Math.pow(dist, 2) - 0.621 * Math.pow(dist, 3); } diff --git a/src/main/java/frc/team3128/subsystems/Shooter.java b/src/main/java/frc/team3128/subsystems/Shooter.java index 39c3453..bd805a9 100644 --- a/src/main/java/frc/team3128/subsystems/Shooter.java +++ b/src/main/java/frc/team3128/subsystems/Shooter.java @@ -36,6 +36,11 @@ public static synchronized Shooter getInstance(){ return instance; } + @Override + public boolean atSetpoint() { + return super.atSetpoint() && Math.abs(Math.abs(rightMotor.getVelocity()) - (getSetpoint() - 1500)) < TOLERANCE; + } + private void configMotors(){ leftMotor = new NAR_CANSpark(LEFT_MOTOR_ID, ControllerType.CAN_SPARK_FLEX); rightMotor = new NAR_CANSpark(RIGHT_MOTOR_ID, ControllerType.CAN_SPARK_FLEX); @@ -56,7 +61,7 @@ private void setPower(double power){ @Override protected void useOutput(double output, double setpoint) { leftMotor.setVolts(output); - rightMotor.setVolts(Math.max(0, (setpoint - 2000) * PIDConstants.kV)); + rightMotor.setVolts(Math.max(0, (setpoint - 1500) * PIDConstants.kV)); } @Override