From: iefomit Date: Mon, 30 Mar 2026 00:34:12 +0000 (-0700) Subject: Political Autos X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=af139060cad59f6b7a9e38763f236924150f030c;p=FRC2026.git Political Autos New Paths Pathplanner PID changes Spindexer changes Turret change --- diff --git a/src/main/deploy/pathplanner/autos/DirectDoubleSwipe.auto b/src/main/deploy/pathplanner/autos/DirectDoubleSwipe.auto new file mode 100644 index 0000000..76de232 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DirectDoubleSwipe.auto @@ -0,0 +1,121 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "W5 Left Trench Start" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + }, + { + "type": "path", + "data": { + "pathName": "DirectSwipe1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "Stop Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Stop Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "path", + "data": { + "pathName": "Swipe2" + } + }, + { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Start Spindexer" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DoubleSwipe.auto b/src/main/deploy/pathplanner/autos/DoubleSwipe.auto deleted file mode 100644 index 3738c9e..0000000 --- a/src/main/deploy/pathplanner/autos/DoubleSwipe.auto +++ /dev/null @@ -1,67 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Hood Down" - } - }, - { - "type": "named", - "data": { - "name": "Extend Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "W5 Left Trench Start" - } - }, - { - "type": "named", - "data": { - "name": "Spin Intake Rollers" - } - }, - { - "type": "path", - "data": { - "pathName": "W5 Full Left Path v3" - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "named", - "data": { - "name": "Stop Spindexer" - } - }, - { - "type": "named", - "data": { - "name": "Hood Down" - } - }, - { - "type": "path", - "data": { - "pathName": "Swipe2" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftConservativeDoubleSwipe.auto b/src/main/deploy/pathplanner/autos/LeftConservativeDoubleSwipe.auto new file mode 100644 index 0000000..185c470 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftConservativeDoubleSwipe.auto @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "W5 Left Trench Start" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + }, + { + "type": "path", + "data": { + "pathName": "ConservativeSwipe1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "Stop Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Stop Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Reset Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "path", + "data": { + "pathName": "Swipe2" + } + }, + { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Start Spindexer" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftLiberalDoubleSwipe.auto b/src/main/deploy/pathplanner/autos/LeftLiberalDoubleSwipe.auto new file mode 100644 index 0000000..25b15e3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftLiberalDoubleSwipe.auto @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "W5 Left Trench Start" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + }, + { + "type": "path", + "data": { + "pathName": "Swipe1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "Stop Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Stop Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Reset Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "path", + "data": { + "pathName": "Swipe2" + } + }, + { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Start Spindexer" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ConservativeSwipe1.path b/src/main/deploy/pathplanner/paths/ConservativeSwipe1.path new file mode 100644 index 0000000..65733dd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ConservativeSwipe1.path @@ -0,0 +1,164 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.5, + "y": 7.623190984573506 + }, + "prevControl": null, + "nextControl": { + "x": 6.819805487632309, + "y": 6.775820794389796 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.159230132450332, + "y": 6.641316225165562 + }, + "prevControl": { + "x": 6.869067715509156, + "y": 6.9193627325227665 + }, + "nextControl": { + "x": 7.339735203900921, + "y": 6.46834825361991 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.398505338078292, + "y": 5.566846026490066 + }, + "prevControl": { + "x": 7.64577301033512, + "y": 5.705763638397914 + }, + "nextControl": { + "x": 7.031343434260263, + "y": 5.360570558304471 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.46953642384106, + "y": 6.401738410596026 + }, + "prevControl": { + "x": 6.69147920841791, + "y": 6.100131466298098 + }, + "nextControl": { + "x": 6.078640973388454, + "y": 6.9329419033148865 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.6124792408066435, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.8389868583347635, + "y": 7.302204002163478 + }, + "nextControl": { + "x": 5.3859716232785235, + "y": 7.5138102327119665 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.5 + }, + "prevControl": { + "x": 4.396406066666464, + "y": 7.41042919989128 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9685230024212932, + "rotationDegrees": -64.90375286541574 + }, + { + "waypointRelativePos": 3.86, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7761529808773961, + "maxWaypointRelativePos": 1.8335208098987592, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 150.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stop Hood Down", + "waypointRelativePos": 4.968503937007888, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } + }, + { + "name": "Start Spindexer", + "waypointRelativePos": 4.982332155477035, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Spindexer" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.5, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DirectSwipe1.path b/src/main/deploy/pathplanner/paths/DirectSwipe1.path new file mode 100644 index 0000000..1a00a8a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DirectSwipe1.path @@ -0,0 +1,152 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.5, + "y": 7.623190984573506 + }, + "prevControl": null, + "nextControl": { + "x": 7.05559305204347, + "y": 7.1393105448289385 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.678244365361804, + "y": 6.364365361803084 + }, + "prevControl": { + "x": 7.555116359470672, + "y": 6.629895424661244 + }, + "nextControl": { + "x": 7.804590465430033, + "y": 6.091895360764787 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.699157769869515, + "y": 6.536512455516014 + }, + "prevControl": { + "x": 6.8860851430626155, + "y": 6.1598703844515175 + }, + "nextControl": { + "x": 6.2963922199408735, + "y": 7.34804930155004 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.6124792408066435, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.93853054547692, + "y": 7.303899970283091 + }, + "nextControl": { + "x": 5.324138304321591, + "y": 7.500073463670289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.5 + }, + "prevControl": { + "x": 4.5311698343686535, + "y": 7.430992876119471 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1, + "rotationDegrees": -56.57915082012971 + }, + { + "waypointRelativePos": 2.0994671403197227, + "rotationDegrees": -74.10500133422103 + }, + { + "waypointRelativePos": 2.86, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7761529808773961, + "maxWaypointRelativePos": 1.3, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 150.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stop Hood Down", + "waypointRelativePos": 3.9685039370078883, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } + }, + { + "name": "Start Spindexer", + "waypointRelativePos": 4.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Spindexer" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.5, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Swipe1.path b/src/main/deploy/pathplanner/paths/Swipe1.path new file mode 100644 index 0000000..645b5bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Swipe1.path @@ -0,0 +1,168 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.5, + "y": 7.623190984573506 + }, + "prevControl": null, + "nextControl": { + "x": 6.735631586835829, + "y": 7.2764780852994 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.710521945432978, + "y": 6.697900355871886 + }, + "prevControl": { + "x": 7.529042978857038, + "y": 6.86984623308483 + }, + "nextControl": { + "x": 7.928919995080244, + "y": 6.490974754868527 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.710521945432978, + "y": 5.7188137603795965 + }, + "prevControl": { + "x": 7.925365275955983, + "y": 5.863911730255719 + }, + "nextControl": { + "x": 7.408579979016614, + "y": 5.514892297047377 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.860545670225386, + "y": 6.364365361803084 + }, + "prevControl": { + "x": 6.917316499510639, + "y": 6.120896528187229 + }, + "nextControl": { + "x": 6.574474152212791, + "y": 7.591219006502909 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.6124792408066435, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.872250653983406, + "y": 7.333391289226129 + }, + "nextControl": { + "x": 5.300042806450162, + "y": 7.497750261334479 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.5 + }, + "prevControl": { + "x": 4.245580247742288, + "y": 7.453198911136212 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.15, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0994671403197227, + "rotationDegrees": -74.10500133422103 + }, + { + "waypointRelativePos": 3.86, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7761529808773961, + "maxWaypointRelativePos": 1.642294713160863, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 150.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stop Hood Down", + "waypointRelativePos": 4.968503937007888, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } + }, + { + "name": "Start Spindexer", + "waypointRelativePos": 5.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Spindexer" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.5, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Swipe2.path b/src/main/deploy/pathplanner/paths/Swipe2.path index 1e8b833..5efb8f7 100644 --- a/src/main/deploy/pathplanner/paths/Swipe2.path +++ b/src/main/deploy/pathplanner/paths/Swipe2.path @@ -4,60 +4,60 @@ { "anchor": { "x": 4.0, - "y": 7.623 + "y": 7.5 }, "prevControl": null, "nextControl": { - "x": 6.0, - "y": 7.623 + "x": 5.75, + "y": 7.5 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.730830367734282, - "y": 5.729572953736655 + "x": 6.0213285883748515, + "y": 6.289051008303677 }, "prevControl": { - "x": 5.730830367734282, - "y": 6.729572953736655 + "x": 6.135976270876549, + "y": 7.282457223754833 }, "nextControl": { - "x": 5.730830367734282, - "y": 4.729572953736655 + "x": 5.929276488101032, + "y": 5.4914323686422035 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.03208778173191, - "y": 3.97582443653618 + "x": 6.258030842230131, + "y": 4.449228944246738 }, "prevControl": { - "x": 5.584874186231952, - "y": 4.199431234286158 + "x": 5.653713425517404, + "y": 4.619355154982582 }, "nextControl": { - "x": 6.5915658362989324, - "y": 3.696085409252669 + "x": 6.687014708632405, + "y": 4.3284622781876605 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.699157769869515, - "y": 4.642894424673784 + "x": 6.892823250296561, + "y": 5.546666666666668 }, "prevControl": { - "x": 6.699157769869515, - "y": 4.183101989407747 + "x": 6.892823250296561, + "y": 4.907866997335997 }, "nextControl": { - "x": 6.699157769869515, - "y": 5.102686859939822 + "x": 6.892823250296561, + "y": 6.2753696930420535 }, "isLocked": false, "linkedName": null @@ -65,15 +65,15 @@ { "anchor": { "x": 6.258030842230131, - "y": 6.730177935943061 + "y": 6.977639383155397 }, "prevControl": { - "x": 6.081254146933494, - "y": 6.906954631239698 + "x": 6.423269381273585, + "y": 6.7900328128685725 }, "nextControl": { - "x": 6.434807537526767, - "y": 6.553401240646425 + "x": 6.092792303186677, + "y": 7.165245953442221 }, "isLocked": false, "linkedName": null @@ -81,11 +81,11 @@ { "anchor": { "x": 4.0, - "y": 7.623 + "y": 7.5 }, "prevControl": { - "x": 5.768466785290629, - "y": 7.738661328588376 + "x": 5.5, + "y": 7.5 }, "nextControl": null, "isLocked": false, @@ -102,8 +102,12 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 3.0284191829485203, + "waypointRelativePos": 3.0373001776198882, "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.129662522202486, + "rotationDegrees": 0.0 } ], "constraintZones": [], @@ -127,5 +131,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/W5 Full Left Path v3.path b/src/main/deploy/pathplanner/paths/W5 Full Left Path v3.path deleted file mode 100644 index d2d8298..0000000 --- a/src/main/deploy/pathplanner/paths/W5 Full Left Path v3.path +++ /dev/null @@ -1,154 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.5, - "y": 7.623190984573506 - }, - "prevControl": null, - "nextControl": { - "x": 6.819003128648605, - "y": 7.795699388727487 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.841663907284769, - "y": 6.3509188741721845 - }, - "prevControl": { - "x": 7.474301880921233, - "y": 6.65544005378413 - }, - "nextControl": { - "x": 8.536734620002141, - "y": 5.774746765569564 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.943302980132451, - "y": 5.123990066225165 - }, - "prevControl": { - "x": 8.175624745513659, - "y": 5.216329642410886 - }, - "nextControl": { - "x": 7.220724340639356, - "y": 4.836790951574134 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.868832781456953, - "y": 6.024221854304635 - }, - "prevControl": { - "x": 6.860132910129963, - "y": 5.650613883255112 - }, - "nextControl": { - "x": 6.897180594033059, - "y": 7.2415925879837 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.58020166073547, - "y": 7.5 - }, - "prevControl": { - "x": 5.774037666645563, - "y": 7.342115222985805 - }, - "nextControl": { - "x": 5.363252370286202, - "y": 7.676711185236958 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.0, - "y": 7.623190984573506 - }, - "prevControl": { - "x": 4.34969659712891, - "y": 7.508772305482001 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.15, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 3.024213075060532, - "rotationDegrees": -74.10500133422103 - }, - { - "waypointRelativePos": 3.86, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Stop Hood Down", - "waypointRelativePos": 4.968503937007888, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Stop Hood Down" - } - } - }, - { - "name": "Start Spindexer", - "waypointRelativePos": 5.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Start Spindexer" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 200.0, - "maxAngularAcceleration": 150.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0.5, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/W5 Left Trench Start.path b/src/main/deploy/pathplanner/paths/W5 Left Trench Start.path index dabc7c0..e32f097 100644 --- a/src/main/deploy/pathplanner/paths/W5 Left Trench Start.path +++ b/src/main/deploy/pathplanner/paths/W5 Left Trench Start.path @@ -29,25 +29,11 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.5, - "maxAngularVelocity": 200.0, - "maxAngularAcceleration": 300.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 0.5, "maxAcceleration": 2.0, "maxAngularVelocity": 200.0, "maxAngularAcceleration": 300.0, @@ -64,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 38af8e3..9640a05 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -21,7 +21,7 @@ "defaultMaxAngAccel": 300.0, "defaultNominalVoltage": 12.0, "robotMass": 63.37, - "robotMOI": 4.183, + "robotMOI": 7.0, "robotTrackwidth": 0.546, "driveWheelRadius": 0.05, "driveGearing": 7.03, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0b12a2a..ae35617 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -228,6 +228,8 @@ public class RobotContainer { NamedCommands.registerCommand("Start Spindexer", new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer))); NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> runSpindexer.cancel())); + NamedCommands.registerCommand("Reset Spindexer", new InstantCommand(() -> spindexer.resetSpindexer())); + NamedCommands.registerCommand("Reset Reset Angle", new InstantCommand(() -> spindexer.resetResetAngle())); } if (hood != null) { @@ -269,13 +271,15 @@ public class RobotContainer { String leftSideAuto = "Left Week V1"; String rightSideAuto = "Right Week V1"; String shootOnlyAuto = "Shoot Only Left Week V1"; - String doubleSwipe = "DoubleSwipe"; + String leftLiberalSwipe = "LeftLiberalDoubleSwipe"; + String leftConservativeSwipe = "LeftConservativeDoubleSwipe"; autoChooser.setDefaultOption("Default", new PathPlannerAuto(defaultAuto)); addAuto(leftSideAuto); addAuto(rightSideAuto); addAuto(shootOnlyAuto); - addAuto(doubleSwipe); + addAuto(leftConservativeSwipe); + addAuto(leftLiberalSwipe); // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/gpm/BrownOutControl.java b/src/main/java/frc/robot/commands/gpm/BrownOutControl.java index c5b34b7..a48076a 100644 --- a/src/main/java/frc/robot/commands/gpm/BrownOutControl.java +++ b/src/main/java/frc/robot/commands/gpm/BrownOutControl.java @@ -51,22 +51,22 @@ public class BrownOutControl extends Command { int level = 1; double batteryVoltage = RobotController.getBatteryVoltage(); if (batteryVoltage > BrownOutConstants.LEVEL_ONE_LIMIT) { // normal - return levels[0]; level = 1; + return levels[0]; } else if (batteryVoltage > BrownOutConstants.LEVEL_TWO_LIMIT) { // if 7.5 to 6.75 - return levels[1]; // lower drivetrain level = 2; + return levels[1]; // lower drivetrain } else if (batteryVoltage > BrownOutConstants.LEVEL_THREE_LIMIT) { // if 6.75 to 6.0 (browning out) - return levels[2]; level = 3; + return levels[2]; } else if (batteryVoltage > BrownOutConstants.LEVEL_FOUR_LIMIT) { // if 6.0 to 5.0 (mayday) - return levels[3]; level = 4; + return levels[3]; } else { // were are on life support at this point 5.25 to 4.75 - return levels[4]; level = 5; + return levels[4]; } - Logger.recordOutput("BrownoutProtection/Level", level); + // Logger.recordOutput("BrownoutProtection/Level", level); } public void applyLevel(BrownOutLevel level) { diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java index 83b9e34..4764aca 100644 --- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -108,10 +108,10 @@ public class DriveConstants { public static final double TRANSLATIONAL_D = 0.001; //The PIDs for PathPlanner Command - public static final double PATH_PLANNER_HEADING_P = 3.5; + public static final double PATH_PLANNER_HEADING_P = 3.5/2; public static final double PATH_PLANNER_HEADING_D = 0; - public static final double PATH_PLANNER_TRANSLATIONAL_P = 6; + public static final double PATH_PLANNER_TRANSLATIONAL_P = 6/2; public static final double PATH_PLANNER_TRANSLATIONAL_D = 0; // CAN diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index f33331b..4c7754c 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -55,7 +55,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { Logger.processInputs("Spindexer", inputs); if (resetPos == null) { - motor.setPosition(0.5 * gearRatio); + motor.setPosition(0.1 * gearRatio); resetPos = (motor.getPosition().getValueAsDouble() / gearRatio) % 1.0; resetPID.reset(); } @@ -138,7 +138,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { } private Double resetPos; - private PIDController resetPID = new PIDController(1.0, 0.0, 0); + private PIDController resetPID = new PIDController(4.0, 0.0, 0); private final double gearRatio = 27.0 / 1.0; //spindexer spins once for every 27 motor spins diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java index bd6ae29..c7bf28a 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java @@ -4,7 +4,7 @@ public class SpindexerConstants { public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls) public static final double currentLimit = 40; // A public static final double spindexerMaxPower = 0.75; - public static final double spindexerReversePower = -0.2; + public static final double spindexerReversePower = -1.00; public static final double CURRENT_SPIKE_LIMIT = 100; public static final double CURRENT_TIME_LIMIT = 1.0; //s public static final double JAM_CURRENT_THRESHOLD = 75.0; // A diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index c2e770c..315b4ad 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -75,7 +75,7 @@ public class Turret extends SubsystemBase implements TurretIO{ config.Slot0.kP = 12.0; config.Slot0.kS = 0.1; // Static friction compensation config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio - config.Slot0.kD = 0.2; // The "Braking" term to stop overshoot + config.Slot0.kD = 0.0; // The "Braking" term to stop overshoot var mm = config.MotionMagic; mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; @@ -201,7 +201,7 @@ public class Turret extends SubsystemBase implements TurretIO{ motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO); // Multiply goal velocity by kV - double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV; + double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV * TurretConstants.GEAR_RATIO; if(calibrating){ // motor.set(0.05); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 5dac346..7a8b9af 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -24,7 +24,7 @@ public class TurretConstants { public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06; - public static final double FEEDFORWARD_KV = 0.185; + public static final double FEEDFORWARD_KV = 0.06; public static final double NORMAL_CURRENT_LIMIT = 40.0; // A public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A