diff --git a/networktables.json.bck b/networktables.json.bck index ab07ff1..479c505 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -38,5 +38,205 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Drive/Autopilot/MaxVelocity", + "type": "double", + "value": 4.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/MaxAccel", + "type": "double", + "value": 3.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/MaxRotation", + "type": "double", + "value": 0.15915494309189535, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/MaxAngularAccel", + "type": "double", + "value": 0.3183098861837907, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/LinearFFMinError", + "type": "double", + "value": 100.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/RotationFFMinError", + "type": "double", + "value": 57.29577951308232, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/LinearFFMaxError", + "type": "double", + "value": 500.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/RotationFFMaxError", + "type": "double", + "value": 286.4788975654116, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Autopilot/MinAppliedVelocity", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Launch Speed", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Hood Launch Position", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Launch Percent (for without PID)", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Hood/kP", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Hood/kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Hood/Target Rotations", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Tuning/FlywheelRPS", + "type": "double", + "value": 16.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Tuning/FlywheelStepRPS", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Tuning/HoodRotations", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Tuning/HoodStepRotations", + "type": "double", + "value": 0.2, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Indexer/Index Speed", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Indexer/Index Percent (for without PID)", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Indexer/Reverse Index Percent (for without PID)", + "type": "double", + "value": -0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Indexer/Accelerator Percent (for without PID)", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Indexer/Pulsing Run Time", + "type": "double", + "value": 2.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Indexer/Pulsing Pause Time", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } } ] diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/deploy/pathplanner/autos/Basic Comp Auto - CENTER.auto b/src/main/deploy/pathplanner/autos/Basic Comp Auto - CENTER.auto new file mode 100644 index 0000000..126d008 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Basic Comp Auto - CENTER.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to Shoot - CENTER" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to HP - CENTER" + } + }, + { + "type": "named", + "data": { + "name": "HP Reload" + } + }, + { + "type": "path", + "data": { + "pathName": "HP to Shoot - CENTER" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Climb - CENTER" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Basic Comp Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Basic Comp Auto.auto b/src/main/deploy/pathplanner/autos/Basic Comp Auto - LEFT.auto similarity index 63% rename from src/main/deploy/pathplanner/autos/Basic Comp Auto.auto rename to src/main/deploy/pathplanner/autos/Basic Comp Auto - LEFT.auto index e9f47c7..46dbb9b 100644 --- a/src/main/deploy/pathplanner/autos/Basic Comp Auto.auto +++ b/src/main/deploy/pathplanner/autos/Basic Comp Auto - LEFT.auto @@ -7,49 +7,49 @@ { "type": "path", "data": { - "pathName": "Start to Shoot" + "pathName": "Start to Shoot - LEFT" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 1.0 + "name": "Shoot" } }, { "type": "path", "data": { - "pathName": "Shoot to Overbump" + "pathName": "Shoot to Overbump - LEFT" } }, { "type": "path", "data": { - "pathName": "Collect" + "pathName": "Collect - LEFT" } }, { "type": "path", "data": { - "pathName": "Overbump to Shoot" + "pathName": "Overbump to Shoot - LEFT" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 1.0 + "name": "Shoot" } }, { "type": "path", "data": { - "pathName": "Shoot to Climber" + "pathName": "Shoot to Climb - LEFT" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "Basic Comp Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Basic Comp Auto - RIGHT.auto b/src/main/deploy/pathplanner/autos/Basic Comp Auto - RIGHT.auto new file mode 100644 index 0000000..b8f0f60 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Basic Comp Auto - RIGHT.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to Shoot - RIGHT" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Overbump - RIGHT" + } + }, + { + "type": "path", + "data": { + "pathName": "Collect - RIGHT" + } + }, + { + "type": "path", + "data": { + "pathName": "Overbump to Shoot - RIGHT" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Climb - RIGHT" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Basic Comp Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Full Simple Auto.auto b/src/main/deploy/pathplanner/autos/Full Simple Auto.auto index 2265148..9b7c52b 100644 --- a/src/main/deploy/pathplanner/autos/Full Simple Auto.auto +++ b/src/main/deploy/pathplanner/autos/Full Simple Auto.auto @@ -26,6 +26,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Simple Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Immediate HP Auto (Around Tower) - RIGHT.auto b/src/main/deploy/pathplanner/autos/Immediate HP Auto (Around Tower) - RIGHT.auto new file mode 100644 index 0000000..e5851b0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Immediate HP Auto (Around Tower) - RIGHT.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to HP - HP AUTO" + } + }, + { + "type": "named", + "data": { + "name": "HP Reload" + } + }, + { + "type": "path", + "data": { + "pathName": "HP to Right Tower - HP AUTO" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Tower through Depot - HP AUTO" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot to Shoot - HP AUTO" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Climb - HP AUTO" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Other Comp Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Immediate HP Auto (Through Tower) - RIGHT.auto b/src/main/deploy/pathplanner/autos/Immediate HP Auto (Through Tower) - RIGHT.auto new file mode 100644 index 0000000..2b9fb03 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Immediate HP Auto (Through Tower) - RIGHT.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to HP - HP AUTO" + } + }, + { + "type": "named", + "data": { + "name": "HP Reload" + } + }, + { + "type": "path", + "data": { + "pathName": "HP to Right Tower (through tower) - HP AUTO" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Tower through Depot (through tower) - HP AUTO" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot to Shoot - HP AUTO" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Climb - HP AUTO" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Other Comp Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - LEFT.auto b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - LEFT.auto new file mode 100644 index 0000000..e830b64 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - LEFT.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to shoot - SHOOT LEFT" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Overbump - SHOOT LEFT" + } + }, + { + "type": "path", + "data": { + "pathName": "Overbump through Field - SHOOT LEFT" + } + }, + { + "type": "path", + "data": { + "pathName": "Field to Shoot - SHOOT LEFT" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Climb - SHOOT LEFT" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Other Comp Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - RIGHT.auto b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - RIGHT.auto new file mode 100644 index 0000000..5115f58 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - RIGHT.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to shoot - SHOOT RIGHT" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Overbump - SHOOT RIGHT" + } + }, + { + "type": "path", + "data": { + "pathName": "Overbump through Field - SHOOT RIGHT" + } + }, + { + "type": "path", + "data": { + "pathName": "Field to Shoot - SHOOT RIGHT" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Climb - SHOOT RIGHT" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Other Comp Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..be4cea9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Plus Auto.auto b/src/main/deploy/pathplanner/autos/Plus Auto.auto index f8b62f3..6dceeb3 100644 --- a/src/main/deploy/pathplanner/autos/Plus Auto.auto +++ b/src/main/deploy/pathplanner/autos/Plus Auto.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Seed" + } + }, { "type": "path", "data": { @@ -31,7 +37,7 @@ ] } }, - "resetOdom": false, - "folder": "Test Autos", + "resetOdom": true, + "folder": "Test Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto b/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto index 5a9e346..a4f7a58 100644 --- a/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto +++ b/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Seed" + } + }, { "type": "path", "data": { @@ -19,7 +25,7 @@ ] } }, - "resetOdom": false, - "folder": "Test Autos", + "resetOdom": true, + "folder": "Test Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto b/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto index 8c69b62..b794f0f 100644 --- a/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto +++ b/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto @@ -20,6 +20,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Simple Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto b/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto index 45c093f..fafa49a 100644 --- a/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto +++ b/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto @@ -20,6 +20,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Simple Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Square Auto.auto b/src/main/deploy/pathplanner/autos/Square Auto.auto index 91d6e26..ca9c0e0 100644 --- a/src/main/deploy/pathplanner/autos/Square Auto.auto +++ b/src/main/deploy/pathplanner/autos/Square Auto.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Seed" + } + }, { "type": "path", "data": { @@ -31,7 +37,7 @@ ] } }, - "resetOdom": false, - "folder": "Test Autos", + "resetOdom": true, + "folder": "Test Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1.path b/src/main/deploy/pathplanner/paths/1.path new file mode 100644 index 0000000..f1f1290 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.238, + "y": 3.179851198598982 + }, + "prevControl": null, + "nextControl": { + "x": 2.457431495949073, + "y": 2.980132737950834 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0, + "y": 1.0 + }, + "prevControl": { + "x": 2.270351128472223, + "y": 0.9975724103009258 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.60398696427043 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.46896508702454 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2.path b/src/main/deploy/pathplanner/paths/2.path new file mode 100644 index 0000000..9558e91 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.8069829193959241, + "y": 0.6799783365174665 + }, + "prevControl": null, + "nextControl": { + "x": 1.8646866471069872, + "y": 1.6965427697416304 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6173113400655024, + "y": 2.653168497543669 + }, + "prevControl": { + "x": 1.8331215884279475, + "y": 1.784456195414848 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.16894996806721 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.55517050349204 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Collect - LEFT.path b/src/main/deploy/pathplanner/paths/Collect - LEFT.path new file mode 100644 index 0000000..b93421e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Collect - LEFT.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.5, + "y": 5.6 + }, + "prevControl": null, + "nextControl": { + "x": 6.23250357830652, + "y": 6.717714710948497 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.66188302425107, + "y": 7.146747503566334 + }, + "prevControl": { + "x": 7.417959543415542, + "y": 7.8788594451551655 + }, + "nextControl": { + "x": 7.8860485021398, + "y": 6.473937232524964 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.8, + "y": 2.0 + }, + "prevControl": { + "x": 7.629810024551348, + "y": 0.9438438751506559 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9650053022269222, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.56790381583522 + }, + "reversed": false, + "folder": "Basic Comp Paths - LEFT", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Collect - RIGHT.path b/src/main/deploy/pathplanner/paths/Collect - RIGHT.path new file mode 100644 index 0000000..f7d17fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Collect - RIGHT.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.5, + "y": 2.4 + }, + "prevControl": null, + "nextControl": { + "x": 6.827975062694294, + "y": 2.4358068711646417 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.674821683315434, + "y": 1.0267617689018789 + }, + "prevControl": { + "x": 6.465836505806936, + "y": 0.8522019141369845 + }, + "nextControl": { + "x": 7.9222558379197965, + "y": 1.0624876577673015 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.8, + "y": 6.0 + }, + "prevControl": { + "x": 7.791269614841827, + "y": 6.383366619115859 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Basic Comp Paths - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot to Shoot - HP AUTO.path b/src/main/deploy/pathplanner/paths/Depot to Shoot - HP AUTO.path new file mode 100644 index 0000000..aaad55a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot to Shoot - HP AUTO.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.4655813953488368, + "y": 7.0123613595706615 + }, + "prevControl": null, + "nextControl": { + "x": 1.4079636390795849, + "y": 6.518619887262773 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.370014152386781, + "y": 5.909174093482252 + }, + "prevControl": { + "x": 0.9151841935150555, + "y": 6.31878071137575 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.95192098180585 + }, + "reversed": false, + "folder": "HP AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": 90.55439661960996 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Field to Shoot - SHOOT LEFT.path b/src/main/deploy/pathplanner/paths/Field to Shoot - SHOOT LEFT.path new file mode 100644 index 0000000..2a84859 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Field to Shoot - SHOOT LEFT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.797, + "y": 2.118 + }, + "prevControl": null, + "nextControl": { + "x": 5.955621796588128, + "y": 2.5165972115973068 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.338031383873929, + "y": 2.118 + }, + "prevControl": { + "x": 4.598582370716036, + "y": 2.709704186429008 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 60.31783745930524 + }, + "reversed": false, + "folder": "Immediate Shoot - LEFT", + "idealStartingState": { + "velocity": 0, + "rotation": 90.18948660335153 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Field to Shoot - SHOOT RIGHT.path b/src/main/deploy/pathplanner/paths/Field to Shoot - SHOOT RIGHT.path new file mode 100644 index 0000000..8a051d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Field to Shoot - SHOOT RIGHT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.797, + "y": 5.9 + }, + "prevControl": null, + "nextControl": { + "x": 5.976364367847712, + "y": 5.21865096830986 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.338031383873929, + "y": 5.9 + }, + "prevControl": { + "x": 5.088626141615317, + "y": 5.540636966329226 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.282116013370086 + }, + "reversed": false, + "folder": "Immediate Shoot - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": 90.18948660335153 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Forward Simple.path b/src/main/deploy/pathplanner/paths/Forward Simple.path index a2af603..61446e6 100644 --- a/src/main/deploy/pathplanner/paths/Forward Simple.path +++ b/src/main/deploy/pathplanner/paths/Forward Simple.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "Simple Paths", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/HP to Right Tower (through tower) - HP AUTO.path b/src/main/deploy/pathplanner/paths/HP to Right Tower (through tower) - HP AUTO.path new file mode 100644 index 0000000..f8d3449 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP to Right Tower (through tower) - HP AUTO.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.49567711520807833, + "y": 0.6595525263922888 + }, + "prevControl": null, + "nextControl": { + "x": 1.1317202321750588, + "y": 1.7739181074007808 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.49567711520807833, + "y": 2.7126296958855094 + }, + "prevControl": { + "x": 0.5477158238978252, + "y": 1.1469379792458696 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.22748075341538 + }, + "reversed": false, + "folder": "HP AUTO - Through tower", + "idealStartingState": { + "velocity": 0, + "rotation": -178.9377529135697 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP to Right Tower - HP AUTO.path b/src/main/deploy/pathplanner/paths/HP to Right Tower - HP AUTO.path new file mode 100644 index 0000000..1fe7520 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP to Right Tower - HP AUTO.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.49567711520807833, + "y": 0.6595525263922888 + }, + "prevControl": null, + "nextControl": { + "x": 1.1317202321750588, + "y": 1.7739181074007808 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6500357781753126, + "y": 2.9560107334525934 + }, + "prevControl": { + "x": 1.7020744868650595, + "y": 1.3903190168129536 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.22748075341538 + }, + "reversed": false, + "folder": "HP AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": -178.9377529135697 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Collect.path b/src/main/deploy/pathplanner/paths/HP to Shoot - CENTER.path similarity index 75% rename from src/main/deploy/pathplanner/paths/Collect.path rename to src/main/deploy/pathplanner/paths/HP to Shoot - CENTER.path index d7a08de..560f56d 100644 --- a/src/main/deploy/pathplanner/paths/Collect.path +++ b/src/main/deploy/pathplanner/paths/HP to Shoot - CENTER.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.5, - "y": 5.6 + "x": 0.5, + "y": 0.65 }, "prevControl": null, "nextControl": { - "x": 7.764965909090909, - "y": 4.838931818181818 + "x": 1.3181489060587515, + "y": 1.0150202723378208 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.8, - "y": 2.6 + "x": 2.25, + "y": 4.0 }, "prevControl": { - "x": 7.897767937834849, - "y": 2.8300900483104847 + "x": 1.9884508395807834, + "y": 2.6549768111230105 }, "nextControl": null, "isLocked": false, @@ -42,10 +42,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": -88.56790381583522 + "rotation": 0.0 }, "reversed": false, - "folder": "Basic Comp Paths", + "folder": "Center AUTO", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Overbump through Field - SHOOT LEFT.path b/src/main/deploy/pathplanner/paths/Overbump through Field - SHOOT LEFT.path new file mode 100644 index 0000000..9ee3d06 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Overbump through Field - SHOOT LEFT.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.957, + "y": 5.619 + }, + "prevControl": null, + "nextControl": { + "x": 6.660616202572754, + "y": 6.892922515682691 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.796968138007269, + "y": 7.223064756732263 + }, + "prevControl": { + "x": 7.796998847236914, + "y": 8.223064756260726 + }, + "nextControl": { + "x": 7.7968814068261185, + "y": 4.398793939336301 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.796968138007269, + "y": 2.1179373374391175 + }, + "prevControl": { + "x": 7.849613964965654, + "y": 4.9567356276777925 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0376970409292028, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.61501030464382 + }, + "reversed": false, + "folder": "Immediate Shoot - LEFT", + "idealStartingState": { + "velocity": 0, + "rotation": 0.33183666738490913 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Overbump through Field - SHOOT RIGHT.path b/src/main/deploy/pathplanner/paths/Overbump through Field - SHOOT RIGHT.path new file mode 100644 index 0000000..c437f5e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Overbump through Field - SHOOT RIGHT.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.957, + "y": 2.4 + }, + "prevControl": null, + "nextControl": { + "x": 7.485993411641727, + "y": 0.06737400968309881 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.796968138007269, + "y": 1.8 + }, + "prevControl": { + "x": 7.801380627984402, + "y": 0.7066200206669451 + }, + "nextControl": { + "x": 7.789890308653041, + "y": 3.553829913077088 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.796968138007269, + "y": 5.9 + }, + "prevControl": { + "x": 7.7802843722533925, + "y": 4.075170279494341 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7627730918141591, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.72389091084005 + }, + "reversed": false, + "folder": "Immediate Shoot - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": 0.45951737426344114 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Overbump to Shoot - LEFT.path b/src/main/deploy/pathplanner/paths/Overbump to Shoot - LEFT.path new file mode 100644 index 0000000..e09fa5a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Overbump to Shoot - LEFT.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 6.985983737990004, + "y": 2.050324291527099 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 2.4 + }, + "prevControl": { + "x": 2.1995281659399466, + "y": 2.4581032736970116 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.34040296924708524, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.6712619300105975, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 40.0 + }, + "reversed": false, + "folder": "Basic Comp Paths - LEFT", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Overbump to Shoot - RIGHT.path b/src/main/deploy/pathplanner/paths/Overbump to Shoot - RIGHT.path new file mode 100644 index 0000000..cfa1ab7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Overbump to Shoot - RIGHT.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 8.38271200464836, + "y": 6.129951196039749 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 5.6 + }, + "prevControl": { + "x": 1.7934616317779772, + "y": 5.556654737521497 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4125132555673362, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.6988335100742215, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -40.0 + }, + "reversed": false, + "folder": "Basic Comp Paths - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Plus 1.path b/src/main/deploy/pathplanner/paths/Plus 1.path index a156789..c27bf7e 100644 --- a/src/main/deploy/pathplanner/paths/Plus 1.path +++ b/src/main/deploy/pathplanner/paths/Plus 1.path @@ -45,7 +45,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": "Plus Sign Test", + "folder": "Plus - Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Plus 2.path b/src/main/deploy/pathplanner/paths/Plus 2.path index d016497..731c622 100644 --- a/src/main/deploy/pathplanner/paths/Plus 2.path +++ b/src/main/deploy/pathplanner/paths/Plus 2.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Plus Sign Test", + "folder": "Plus - Test", "idealStartingState": { "velocity": 0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/Plus 3.path b/src/main/deploy/pathplanner/paths/Plus 3.path index 2859d80..dbb7c87 100644 --- a/src/main/deploy/pathplanner/paths/Plus 3.path +++ b/src/main/deploy/pathplanner/paths/Plus 3.path @@ -45,7 +45,7 @@ "rotation": 90.0 }, "reversed": false, - "folder": "Plus Sign Test", + "folder": "Plus - Test", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Plus 4.path b/src/main/deploy/pathplanner/paths/Plus 4.path index ee32f3c..6db6d8e 100644 --- a/src/main/deploy/pathplanner/paths/Plus 4.path +++ b/src/main/deploy/pathplanner/paths/Plus 4.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Plus Sign Test", + "folder": "Plus - Test", "idealStartingState": { "velocity": 0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/Right Tower through Depot (through tower) - HP AUTO.path b/src/main/deploy/pathplanner/paths/Right Tower through Depot (through tower) - HP AUTO.path new file mode 100644 index 0000000..9af9344 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Tower through Depot (through tower) - HP AUTO.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.44935599284436445, + "y": 2.7288550983899813 + }, + "prevControl": null, + "nextControl": { + "x": 0.4655813953488366, + "y": 5.195116279069768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.44935599284436445, + "y": 7.0123613595706615 + }, + "prevControl": { + "x": 0.48180679785330893, + "y": 5.779230769230769 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "HP AUTO - Through tower", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Tower through Depot - HP AUTO.path b/src/main/deploy/pathplanner/paths/Right Tower through Depot - HP AUTO.path new file mode 100644 index 0000000..669f1f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Tower through Depot - HP AUTO.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6338103756708402, + "y": 2.939785330948122 + }, + "prevControl": null, + "nextControl": { + "x": 2.4126296958855096, + "y": 5.649427549194993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.44935599284436445, + "y": 5.0004114490161 + }, + "prevControl": { + "x": 0.434241796979505, + "y": 4.496604920187455 + }, + "nextControl": { + "x": 0.49803220035778134, + "y": 6.622951699463327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.44935599284436445, + "y": 7.0123613595706615 + }, + "prevControl": { + "x": 0.48180679785330893, + "y": 5.779230769230769 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "HP AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rotate And Move Backward.path b/src/main/deploy/pathplanner/paths/Rotate And Move Backward.path index 303e568..d51251e 100644 --- a/src/main/deploy/pathplanner/paths/Rotate And Move Backward.path +++ b/src/main/deploy/pathplanner/paths/Rotate And Move Backward.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Rotation + Translation Test", + "folder": "Test Paths", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Rotate And Move Forward.path b/src/main/deploy/pathplanner/paths/Rotate And Move Forward.path index 6454947..02b714d 100644 --- a/src/main/deploy/pathplanner/paths/Rotate And Move Forward.path +++ b/src/main/deploy/pathplanner/paths/Rotate And Move Forward.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Rotation + Translation Test", + "folder": "Test Paths", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Rotation Simple.path b/src/main/deploy/pathplanner/paths/Rotation Simple.path index 73e0707..08d11ce 100644 --- a/src/main/deploy/pathplanner/paths/Rotation Simple.path +++ b/src/main/deploy/pathplanner/paths/Rotation Simple.path @@ -45,7 +45,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": null, + "folder": "Simple Paths", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb - CENTER.path b/src/main/deploy/pathplanner/paths/Shoot to Climb - CENTER.path new file mode 100644 index 0000000..a0aafad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb - CENTER.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.25, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.7767340783506171, + "y": 4.094137704264151 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5837631483323134, + "y": 4.127368707925337 + }, + "prevControl": { + "x": 1.8849457944546124, + "y": 4.070277167380038 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "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": "Center AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb - HP AUTO.path b/src/main/deploy/pathplanner/paths/Shoot to Climb - HP AUTO.path new file mode 100644 index 0000000..4396b26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb - HP AUTO.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.37, + "y": 5.909 + }, + "prevControl": null, + "nextControl": { + "x": 1.7628196718176254, + "y": 4.857124866126071 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6261313303243572, + "y": 4.202000889305385 + }, + "prevControl": { + "x": 1.593079301535745, + "y": 4.44980638438763 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.38915598286002084 + }, + "reversed": false, + "folder": "HP AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": -30.086375167430813 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climber.path b/src/main/deploy/pathplanner/paths/Shoot to Climb - LEFT.path similarity index 96% rename from src/main/deploy/pathplanner/paths/Shoot to Climber.path rename to src/main/deploy/pathplanner/paths/Shoot to Climb - LEFT.path index 40101a5..6bb14c8 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Climber.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb - LEFT.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Basic Comp Paths", + "folder": "Basic Comp Paths - LEFT", "idealStartingState": { "velocity": 0, "rotation": 40.0 diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb - RIGHT.path b/src/main/deploy/pathplanner/paths/Shoot to Climb - RIGHT.path new file mode 100644 index 0000000..b756051 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb - RIGHT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 5.6 + }, + "prevControl": null, + "nextControl": { + "x": 2.7791476627370493, + "y": 5.598677970740438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5548359486447931, + "y": 4.235549215406562 + }, + "prevControl": { + "x": 1.3049767748435428, + "y": 4.22715914583852 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Basic Comp Paths - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": -40.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb - SHOOT LEFT.path b/src/main/deploy/pathplanner/paths/Shoot to Climb - SHOOT LEFT.path new file mode 100644 index 0000000..5230360 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb - SHOOT LEFT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.338, + "y": 2.118 + }, + "prevControl": null, + "nextControl": { + "x": 2.165942938817355, + "y": 3.3106742707251007 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.594908105110159, + "y": 4.108851600749693 + }, + "prevControl": { + "x": 2.3705101758536844, + "y": 3.2351672538446543 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.1726074424252688 + }, + "reversed": false, + "folder": "Immediate Shoot - LEFT", + "idealStartingState": { + "velocity": 0, + "rotation": 56.071323149334354 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb - SHOOT RIGHT.path b/src/main/deploy/pathplanner/paths/Shoot to Climb - SHOOT RIGHT.path new file mode 100644 index 0000000..1db1ce4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb - SHOOT RIGHT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.338, + "y": 5.9 + }, + "prevControl": null, + "nextControl": { + "x": 2.3704845181159406, + "y": 5.0860485428938755 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.594908105110159, + "y": 4.108851600749693 + }, + "prevControl": { + "x": 2.8930631021179387, + "y": 5.415867786084497 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.1726074424252688 + }, + "reversed": false, + "folder": "Immediate Shoot - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": -55.31887958752116 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to HP - CENTER.path b/src/main/deploy/pathplanner/paths/Shoot to HP - CENTER.path new file mode 100644 index 0000000..ef6067f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to HP - CENTER.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.25, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.082727633491432, + "y": 1.8730084818696446 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5, + "y": 0.65 + }, + "prevControl": { + "x": 1.0479057055025294, + "y": 1.6765242538024667 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.25641974735566 + }, + "reversed": false, + "folder": "Center AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Overbump.path b/src/main/deploy/pathplanner/paths/Shoot to Overbump - LEFT.path similarity index 86% rename from src/main/deploy/pathplanner/paths/Shoot to Overbump.path rename to src/main/deploy/pathplanner/paths/Shoot to Overbump - LEFT.path index c15fb32..96f9169 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Overbump.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Overbump - LEFT.path @@ -28,7 +28,12 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.32979851537645677, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -45,7 +50,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Basic Comp Paths", + "folder": "Basic Comp Paths - LEFT", "idealStartingState": { "velocity": 0, "rotation": -40.0 diff --git a/src/main/deploy/pathplanner/paths/Shoot to Overbump - RIGHT.path b/src/main/deploy/pathplanner/paths/Shoot to Overbump - RIGHT.path new file mode 100644 index 0000000..ba2476d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Overbump - RIGHT.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 2.4 + }, + "prevControl": null, + "nextControl": { + "x": 3.439650812800243, + "y": 2.426156135912835 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.5, + "y": 2.4 + }, + "prevControl": { + "x": 5.62327005800708, + "y": 2.403713255567281 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2895015906680828, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "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": "Basic Comp Paths - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": 40.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Overbump - SHOOT LEFT.path b/src/main/deploy/pathplanner/paths/Shoot to Overbump - SHOOT LEFT.path new file mode 100644 index 0000000..3d08072 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Overbump - SHOOT LEFT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.496, + "y": 5.619 + }, + "prevControl": null, + "nextControl": { + "x": 4.368679921324825, + "y": 5.7210289667693655 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.956902990206867, + "y": 5.619 + }, + "prevControl": { + "x": 4.956902990206867, + "y": 5.619 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "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": "Immediate Shoot - LEFT", + "idealStartingState": { + "velocity": 0, + "rotation": -37.07164394607802 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Overbump - SHOOT RIGHT.path b/src/main/deploy/pathplanner/paths/Shoot to Overbump - SHOOT RIGHT.path new file mode 100644 index 0000000..da6675b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to Overbump - SHOOT RIGHT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.496, + "y": 2.4 + }, + "prevControl": null, + "nextControl": { + "x": 4.368679921324825, + "y": 2.5020289667693656 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.956902990206867, + "y": 2.4 + }, + "prevControl": { + "x": 4.956902990206867, + "y": 2.4 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "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": "Immediate Shoot - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": 37.290481679227305 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to HP - HP AUTO.path b/src/main/deploy/pathplanner/paths/Start to HP - HP AUTO.path new file mode 100644 index 0000000..24e1a4d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start to HP - HP AUTO.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.556247035648715, + "y": 0.64 + }, + "prevControl": null, + "nextControl": { + "x": 2.058850332555817, + "y": 0.6373907327540984 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5, + "y": 0.64 + }, + "prevControl": { + "x": 2.353873425242809, + "y": 0.6383105690369354 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.51071558389555 + }, + "reversed": false, + "folder": "HP AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": 179.04387247602065 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to Shoot - CENTER.path b/src/main/deploy/pathplanner/paths/Start to Shoot - CENTER.path new file mode 100644 index 0000000..7507391 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start to Shoot - CENTER.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.645493421052632, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.1504040316792814, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.25, + "y": 4.0 + }, + "prevControl": { + "x": 2.85728498740718, + "y": 4.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -0.21901661014910967 + }, + "reversed": false, + "folder": "Center AUTO", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to Shoot.path b/src/main/deploy/pathplanner/paths/Start to Shoot - LEFT.path similarity index 90% rename from src/main/deploy/pathplanner/paths/Start to Shoot.path rename to src/main/deploy/pathplanner/paths/Start to Shoot - LEFT.path index 676f55a..64d1e7b 100644 --- a/src/main/deploy/pathplanner/paths/Start to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Start to Shoot - LEFT.path @@ -20,8 +20,8 @@ "y": 5.6 }, "prevControl": { - "x": 2.397239513949471, - "y": 5.827904108139057 + "x": 2.3972407268555767, + "y": 5.827904655026679 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -40.0 }, "reversed": false, - "folder": "Basic Comp Paths", + "folder": "Basic Comp Paths - LEFT", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Overbump to Shoot.path b/src/main/deploy/pathplanner/paths/Start to Shoot - RIGHT.path similarity index 78% rename from src/main/deploy/pathplanner/paths/Overbump to Shoot.path rename to src/main/deploy/pathplanner/paths/Start to Shoot - RIGHT.path index cc11753..04be42c 100644 --- a/src/main/deploy/pathplanner/paths/Overbump to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Start to Shoot - RIGHT.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.8, - "y": 2.4 + "x": 3.6, + "y": 3.0 }, "prevControl": null, "nextControl": { - "x": 7.770205608742315, - "y": 2.151781760844244 + "x": 3.9370214328880317, + "y": 3.0058081435684487 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.4 }, "prevControl": { - "x": 2.255053798152365, - "y": 2.4500135801599687 + "x": 2.2511707277397344, + "y": 2.375834047371835 }, "nextControl": null, "isLocked": false, @@ -42,10 +42,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": 37.509098664816776 + "rotation": 40.0 }, "reversed": false, - "folder": "Basic Comp Paths", + "folder": "Basic Comp Paths - RIGHT", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Start to shoot - SHOOT LEFT.path b/src/main/deploy/pathplanner/paths/Start to shoot - SHOOT LEFT.path new file mode 100644 index 0000000..1021b40 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start to shoot - SHOOT LEFT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.555107009242959, + "y": 5.435595428036972 + }, + "prevControl": null, + "nextControl": { + "x": 2.702018749389301, + "y": 5.756747649189761 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.496, + "y": 5.619 + }, + "prevControl": { + "x": 3.1088135586253745, + "y": 5.739373641587598 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.880220390363995 + }, + "reversed": false, + "folder": "Immediate Shoot - LEFT", + "idealStartingState": { + "velocity": 0, + "rotation": -178.86444563841715 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to shoot - SHOOT RIGHT.path b/src/main/deploy/pathplanner/paths/Start to shoot - SHOOT RIGHT.path new file mode 100644 index 0000000..53b50a3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start to shoot - SHOOT RIGHT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.555107009242959, + "y": 2.6 + }, + "prevControl": null, + "nextControl": { + "x": 3.005892825042831, + "y": 2.3685744913088005 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.496, + "y": 2.4 + }, + "prevControl": { + "x": 3.293124860800254, + "y": 2.821171808141837 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 37.500508916961145 + }, + "reversed": false, + "folder": "Immediate Shoot - RIGHT", + "idealStartingState": { + "velocity": 0, + "rotation": -178.86444563841715 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 6110946..2d30c15 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,15 +1,29 @@ { - "robotWidth": 0.858, - "robotLength": 0.858, + "robotWidth": 0.9, + "robotLength": 0.9, "holonomicMode": true, "pathFolders": [ - "Basic Comp Paths", + "Basic Comp Paths - LEFT", + "Basic Comp Paths - RIGHT", + "Center AUTO", + "Comp Autos", + "HP AUTO", + "HP AUTO - Through tower", + "Left Middle AUTO", + "Immediate Shoot - LEFT", + "Immediate Shoot - RIGHT", + "Plus - Test", "Plus Sign Test", "Rotation + Translation Test", - "Square Test" + "Simple Paths", + "Square Test", + "Test Paths" ], "autoFolders": [ - "Test Autos" + "Basic Comp Autons", + "Other Comp Autons", + "Simple Autons", + "Test Autons" ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, @@ -20,19 +34,19 @@ "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, - "driveGearing": 6.746, + "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, - "flModuleX": 0.276, - "flModuleY": 0.276, - "frModuleX": 0.276, - "frModuleY": -0.276, - "blModuleX": -0.276, - "blModuleY": 0.276, - "brModuleX": -0.276, - "brModuleY": -0.276, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..b23d39e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,16 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.statemachines.AllianceState; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -16,4 +26,70 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + + public static class RobotConstants { + public static final Distance robotCenterToEdge = Meters.of(0.47); + public static final Distance robotCenterToIntakeExtended = Meters.of(0.65); + } + + public static class FieldConstants { + + public static final AprilTagFieldLayout layout = + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + /* ***** SHOOTING TARGETS ***** */ + + public static final Pose3d BLUE_HUB = + new Pose3d(Meters.of(4.623), Meters.of(4.06), Meters.of(1.435), Rotation3d.kZero); + public static final Pose3d BLUE_LEFT_PASS = + new Pose3d(Meters.of(7), Meters.of(1), Meters.of(0), Rotation3d.kZero); + public static final Pose3d BLUE_RIGHT_PASS = + new Pose3d(Meters.of(1.0), Meters.of(1.0), Meters.of(0.0), Rotation3d.kZero); + + public static final Pose3d RED_HUB = + new Pose3d(Meters.of(11.918), Meters.of(4.06), Meters.of(1.435), Rotation3d.kZero); + + public static final Pose3d RED_LEFT_PASS = + new Pose3d(Meters.of(14), Meters.of(1), Meters.of(0), Rotation3d.kZero); + public static final Pose3d RED_RIGHT_PASS = + new Pose3d(Meters.of(14), Meters.of(7), Meters.of(0), Rotation3d.kZero); + + /* ***** DRIVING POSES ***** */ + public static final Pose3d BLUE_OUTPOST_LOADING = + new Pose3d( + RobotConstants.robotCenterToIntakeExtended, + RobotConstants.robotCenterToEdge, + Meters.of(0.0), + new Rotation3d(Degrees.of(0.0), Degrees.of(0.0), Degrees.of(180.0))); + + public static final Pose3d RED_OUTPOST_LOADING = + new Pose3d( + layout.getOrigin().getMeasureX().minus(RobotConstants.robotCenterToIntakeExtended), + layout.getOrigin().getMeasureY().minus(RobotConstants.robotCenterToEdge), + Meters.of(0), + Rotation3d.kZero); + + /* ***** Helpers ***** */ + public static final Pose3d getHubTarget() { + return AllianceState.getInstance().getAlliance().equals(Alliance.Blue) ? BLUE_HUB : RED_HUB; + } + + public static final Pose3d getLeftPassTarget() { + return AllianceState.getInstance().getAlliance().equals(Alliance.Blue) + ? BLUE_LEFT_PASS + : RED_LEFT_PASS; + } + + public static final Pose3d getRightPassTarget() { + return AllianceState.getInstance().getAlliance().equals(Alliance.Blue) + ? BLUE_RIGHT_PASS + : RED_RIGHT_PASS; + } + + public static final Pose3d getOutpostLoading() { + return AllianceState.getInstance().getAlliance().equals(Alliance.Blue) + ? BLUE_OUTPOST_LOADING + : RED_OUTPOST_LOADING; + } + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index efd43e2..e2bf6c6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.statemachines.ShiftState; /** * The methods in this class are called automatically corresponding to each mode, as described in @@ -23,6 +24,7 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private final RobotContainer m_robotContainer; + ShiftState shiftState = ShiftState.getInstance(); /** * This function is run when the robot is first started up and should be used for any @@ -84,6 +86,7 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + shiftState.periodic(); // Update shift state and FMS connection status } /** This function is called once each time the robot enters Disabled mode. */ @@ -117,6 +120,9 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + + m_robotContainer.configureSubsystemDefaultCommands(); + m_robotContainer.configureTeleopBindings(); } /** This function is called periodically during operator control. */ @@ -127,6 +133,10 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + + SignalLogger.stop(); + m_robotContainer.removeSubsystemDefaultCommands(); + m_robotContainer.configureTestBindings(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c61fc15..fbaf992 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,34 +4,36 @@ package frc.robot; +import static edu.wpi.first.units.Units.RadiansPerSecond; + import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import frc.robot.commands.DriveBySpeed; -import frc.robot.commands.WheelSlipTest; +import frc.robot.statemachines.DriveState; +import frc.robot.statemachines.LaunchState; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DrivePreferences; import frc.robot.subsystems.drive.DrivetrainSubsystem; import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.LaunchRequest; +import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.vision.VisionSubsystem; @Logged public class RobotContainer { - - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); - private final CommandXboxController joystick = new CommandXboxController(0); + private final CommandXboxController driverJoystick = new CommandXboxController(0); + private final CommandXboxController operatorJoystick = new CommandXboxController(1); public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); @@ -41,9 +43,23 @@ public class RobotContainer { @Logged(name = "Indexer") public final IndexerSubsystem indexer = new IndexerSubsystem(); + @Logged(name = "Shooter") + public final ShooterSubsystem shooter = new ShooterSubsystem(); + @Logged(name = "Climber") public final ClimberSubsystem climber = new ClimberSubsystem(); + @Logged(name = "Vision") + public final VisionSubsystem vision = new VisionSubsystem(); + + private final DriveState driveState = DriveState.getInstance(); + private final LaunchState launchState = LaunchState.getInstance(); + + private final Command driveAndLaunchCommand = + drivetrain + .applyRequest(() -> getDriveAndLaunchRequest()) + .alongWith(shooter.spinFlywheelRanged()); + private final SendableChooser autoChooser; public RobotContainer() { @@ -51,12 +67,13 @@ public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser("Auto Chooser"); SmartDashboard.putData("Auto Mode", autoChooser); - configureSubsystemDefaultCommands(); - configureBindings(); + // Idle while the robot is disabled. This ensures the configured + // neutral mode is applied to the drive motors while disabled. + RobotModeTriggers.disabled() + .whileTrue(drivetrain.applyRequest(() -> new SwerveRequest.Idle()).ignoringDisable(true)); } - private void configureSubsystemDefaultCommands() { - + public void configureSubsystemDefaultCommands() { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest( @@ -64,50 +81,146 @@ private void configureSubsystemDefaultCommands() { DriveConstants.DEFAULT_DRIVE_REQUEST .withVelocityX( -1 - * Math.copySign(Math.pow(joystick.getLeftY(), 2), joystick.getLeftY()) + * Math.copySign( + Math.pow(driverJoystick.getLeftY(), 2), driverJoystick.getLeftY()) * DriveConstants .MAX_DRIVE_SPEED) // Drive forward with negative Y (forward) .withVelocityY( -1 - * Math.copySign(Math.pow(joystick.getLeftX(), 2), joystick.getLeftX()) + * Math.copySign( + Math.pow(driverJoystick.getLeftX(), 2), driverJoystick.getLeftX()) * DriveConstants.MAX_DRIVE_SPEED) // Drive left with negative X (left) .withRotationalRate( -1 - * Math.copySign(Math.pow(joystick.getRightX(), 2), joystick.getRightX()) + * Math.copySign( + Math.pow(driverJoystick.getRightX(), 2), driverJoystick.getRightX()) * DriveConstants .MAX_ANGULAR_SPEED) // Drive counterclockwise with negative X (left) .withDeadband(DriveConstants.MAX_DRIVE_SPEED * 0.1) .withRotationalDeadband(DriveConstants.MAX_ANGULAR_SPEED * 0.1))); } - private void configureBindings() { + public void removeSubsystemDefaultCommands() { + drivetrain.removeDefaultCommand(); + } + + public void configureTestBindings() { // Idle while the robot is disabled. This ensures the configured // neutral mode is applied to the drive motors while disabled. + final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled() .whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true)); - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick - .b() - .whileTrue( - drivetrain.applyRequest( - () -> - point.withModuleDirection( - new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())))); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. + // driverJoystick.rightBumper().onTrue(Commands.runOnce(SignalLogger::start)); + // driverJoystick.leftBumper().onTrue(Commands.runOnce(SignalLogger::stop)); - // joystick.x().onTrue(drivetrain.sysIdSteer()); - // joystick.y().onTrue(drivetrain.sysIdTranslation()); - joystick.x().onTrue(new WheelSlipTest(drivetrain)); - joystick - .y() - .whileTrue(new DriveBySpeed(drivetrain, DrivePreferences.onemeter_speed)); // Testing only + // operatorJoystick.y().whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + // operatorJoystick.a().whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + // operatorJoystick.b().whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // operatorJoystick.x().whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); // Reset the field-centric heading on left bumper press. - joystick.start().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); + driverJoystick.start().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); + + SmartDashboard.putData(shooter.startShooterTuningCommand()); + SmartDashboard.putData(shooter.stopShooterTuningCommand()); + SmartDashboard.putData(shooter.increaseFlywheelCommand()); + SmartDashboard.putData(shooter.decreaseFlywheelCommand()); + SmartDashboard.putData(shooter.increaseHoodCommand()); + SmartDashboard.putData(shooter.decreaseHoodCommand()); + + driverJoystick + .a() + .whileTrue(indexer.startFullIndexingNoPID()) + .onFalse(indexer.stopFullIndexingNoPID()); + } + + public void configureTeleopBindings() { + + // driverJoystick + // .a() + // .whileTrue(intake.setExtendNoPID()) + // .onFalse( + // intake + // .stopExtensionNoPID() + // .andThen(intake.startRollerReverseNoPID()) + // .alongWith(indexer.startIndexerReverseNoPID())); + + // driverJoystick + // .b() + // .onTrue(intake.stopRollerNoPID().alongWith(indexer.stopIndexerNoPID())) + // .whileTrue(intake.setRetractNoPID()) + // .onFalse(intake.stopExtensionNoPID()); + + driverJoystick + .rightBumper() + .whileTrue(intake.setExtendNoPID()) + .onFalse(intake.stopExtensionNoPID().andThen(intake.startRollerNoPID())); + + driverJoystick + .leftBumper() + .whileTrue(intake.setRetractNoPID()) + .onFalse(intake.stopExtensionNoPID().andThen(intake.stopRollerNoPID())); + + // operatorJoystick + // .leftTrigger() + // .whileTrue(shooter.spinFlywheelCommand()) + // .onFalse(shooter.stopFlywheelCommand()); + + operatorJoystick + .rightTrigger() + .whileTrue(indexer.pulsingIndexCommand()) + .onFalse(indexer.stopFullIndexingNoPID()); + + // operatorJoystick + // .leftTrigger() + driverJoystick + .a() + .whileTrue(driveAndLaunchCommand) + .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood())); + + // driverJoystick.leftTrigger().whileTrue(driveAndLaunchCommand); + + /* + driverJoystick + .rightTrigger() + .whileTrue(indexer.startFullIndexingNoPID()) + .onFalse(indexer.stopFullIndexingNoPID()); + */ + + // operatorJoystick.leftTrigger().whileTrue(driveAndLaunchCommand.repeatedly()); + // operatorJoystick + // .rightBumper() + // .whileTrue(indexer.startFullIndexingNoPID()) + // .onFalse(indexer.stopFullIndexingNoPID()); + + // Reset the field-centric heading on start button press. + driverJoystick.start().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); + + operatorJoystick + .a() + .onTrue( + Commands.runOnce( + () -> + LaunchState.getInstance() + .setTargetPose3d(Constants.FieldConstants.getHubTarget()))); + + operatorJoystick + .x() + .onTrue( + Commands.runOnce( + () -> + LaunchState.getInstance() + .setTargetPose3d(Constants.FieldConstants.getLeftPassTarget()))); + + operatorJoystick + .b() + .onTrue( + Commands.runOnce( + () -> + LaunchState.getInstance() + .setTargetPose3d(Constants.FieldConstants.getRightPassTarget()))); drivetrain.registerTelemetry(logger::telemeterize); } @@ -115,4 +228,30 @@ private void configureBindings() { public Command getAutonomousCommand() { return autoChooser.getSelected(); } + + private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { + LaunchRequest launchRequest = launchState.getLaunchRequest(); + double rotationalRate = + launchRequest.getTargetRobotAngularVelocity().in(RadiansPerSecond) + + DrivePreferences.autoAim_kP.getValue() + * launchRequest + .getTargetRobotAngle() + .minus(driveState.getCurrentDriveStats().Pose.getRotation()) + .getRadians() + + DrivePreferences.autoAim_kD.getValue() + * (launchRequest.getTargetRobotAngularVelocity().in(RadiansPerSecond) + - driveState.getFieldVelocity().omegaRadiansPerSecond); + return DriveConstants.DEFAULT_DRIVE_REQUEST + .withVelocityX( + -1 + * Math.copySign(Math.pow(driverJoystick.getLeftY(), 2), driverJoystick.getLeftY()) + * DriveConstants.MAX_DRIVE_SPEED) // Drive forward with negative Y (forward) + .withVelocityY( + -1 + * Math.copySign(Math.pow(driverJoystick.getLeftX(), 2), driverJoystick.getLeftX()) + * DriveConstants.MAX_DRIVE_SPEED) // Drive left with negative X (left) + .withRotationalRate(rotationalRate) + .withDeadband(DriveConstants.MAX_DRIVE_SPEED * 0.1) + .withRotationalDeadband(DriveConstants.MAX_ANGULAR_SPEED * 0.1); + } } diff --git a/src/main/java/frc/robot/commands/DriveAndLaunch.java b/src/main/java/frc/robot/commands/DriveAndLaunch.java new file mode 100644 index 0000000..e7a721f --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveAndLaunch.java @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.statemachines.LaunchState; +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.DrivetrainSubsystem; +import frc.robot.subsystems.shooter.LaunchRequest; +import java.util.function.DoubleSupplier; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class DriveAndLaunch extends Command { + + private DrivetrainSubsystem drivetrain; + private DoubleSupplier xSupplier; + private DoubleSupplier ySupplier; + + /** Creates a new DriveAndLanuch. */ + public DriveAndLaunch( + DrivetrainSubsystem drivetrain, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + this.drivetrain = drivetrain; + this.xSupplier = xSupplier; + this.ySupplier = ySupplier; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.drivetrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + LaunchRequest launchRequest = LaunchState.getInstance().getLaunchRequest(); + + AngularVelocity turnVelocity = launchRequest.getTargetRobotAngularVelocity(); + + drivetrain.applyRequest( + () -> + DriveConstants.DEFAULT_DRIVE_REQUEST + .withVelocityX( + -1 + * Math.copySign( + Math.pow(xSupplier.getAsDouble(), 2), xSupplier.getAsDouble()) + * DriveConstants.MAX_DRIVE_SPEED) // Drive forward with negative Y (forward) + .withVelocityY( + -1 + * Math.copySign( + Math.pow(ySupplier.getAsDouble(), 2), ySupplier.getAsDouble()) + * DriveConstants.MAX_DRIVE_SPEED) // Drive left with negative X (left) + .withRotationalRate( + turnVelocity.in( + RadiansPerSecond)) // Drive counterclockwise with negative X (left) + .withDeadband(DriveConstants.MAX_DRIVE_SPEED * 0.1)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/statemachines/DriveState.java b/src/main/java/frc/robot/statemachines/DriveState.java index 9155ca4..1c39de9 100644 --- a/src/main/java/frc/robot/statemachines/DriveState.java +++ b/src/main/java/frc/robot/statemachines/DriveState.java @@ -1,6 +1,7 @@ package frc.robot.statemachines; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.subsystems.vision.CameraConstants; @@ -15,12 +16,17 @@ public class DriveState { private HashMap> concurrentQueueMap; - private SwerveDriveState currentDriveStats = null; + private SwerveDriveState previousDriveStats = new SwerveDriveState(); + private SwerveDriveState currentDriveStats = new SwerveDriveState(); private DriveState() { concurrentQueueMap = new HashMap>(); concurrentQueueMap.put( CameraConstants.photonCameraName_Front, new ConcurrentLinkedQueue()); + concurrentQueueMap.put( + CameraConstants.photonCameraName_Left, new ConcurrentLinkedQueue()); + concurrentQueueMap.put( + CameraConstants.photonCameraName_Right, new ConcurrentLinkedQueue()); } public static synchronized DriveState getInstance() { @@ -56,6 +62,7 @@ public ArrayList grabVisionEstimateList(String cameraName) { } public void adjustCurrentDriveStats(SwerveDriveState newStats) { + previousDriveStats = currentDriveStats; currentDriveStats = newStats; } @@ -66,4 +73,13 @@ public boolean hasDriveStats() { public SwerveDriveState getCurrentDriveStats() { return currentDriveStats; } + + public SwerveDriveState getPreviousDriveStats() { + return previousDriveStats; + } + + public ChassisSpeeds getFieldVelocity() { + return ChassisSpeeds.fromRobotRelativeSpeeds( + getCurrentDriveStats().Speeds, getCurrentDriveStats().Pose.getRotation()); + } } diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java new file mode 100644 index 0000000..f4ee015 --- /dev/null +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -0,0 +1,166 @@ +package frc.robot.statemachines; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.statemachines.LaunchState.LaunchType; +import frc.robot.subsystems.shooter.LaunchRequest; +import frc.robot.subsystems.shooter.MappedLaunchRequestBuilder; +import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; + +public class LaunchCalculator { + + private static LaunchCalculator single_instance = null; + + private LaunchCalculator() {} + + protected static synchronized LaunchCalculator getInstance() { + if (single_instance == null) single_instance = new LaunchCalculator(); + return single_instance; + } + + private double loopPeriodSecs = 0.02; + + private final LinearFilter driveAngleFilter = + LinearFilter.movingAverage((int) (0.8 / loopPeriodSecs)); + + private static final double phaseDelay; + + private static final InterpolatingDoubleTreeMap hubTimeOfFlightMap = + new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap passingTimeOfFlightMap = + new InterpolatingDoubleTreeMap(); + + // TODO: All of this is made up. Need real numbers. + static { + phaseDelay = 0.03; + + hubTimeOfFlightMap.put(5.68, 1.16); + hubTimeOfFlightMap.put(4.55, 1.12); + hubTimeOfFlightMap.put(3.15, 1.11); + hubTimeOfFlightMap.put(1.88, 1.09); + hubTimeOfFlightMap.put(1.38, 0.90); + + passingTimeOfFlightMap.put(6.68, 1.16); + passingTimeOfFlightMap.put(5.55, 1.12); + passingTimeOfFlightMap.put(4.15, 1.11); + passingTimeOfFlightMap.put(2.88, 1.09); + passingTimeOfFlightMap.put(2.38, 0.90); + } + + protected LaunchRequest refreshRequest(Pose3d target, LaunchType builderType) { + + boolean passing = target.getZ() < 0.1; + + // current pose and movement + Pose2d estimatedRobotPose = DriveState.getInstance().getCurrentDriveStats().Pose; + ChassisSpeeds robotSpeeds = DriveState.getInstance().getCurrentDriveStats().Speeds; + + // predicted pose + estimatedRobotPose.exp( + new Twist2d( + robotSpeeds.vxMetersPerSecond * phaseDelay, + robotSpeeds.vyMetersPerSecond * phaseDelay, + robotSpeeds.omegaRadiansPerSecond * phaseDelay)); + + // TODO: for now assume they're the same. calculate offsets later + Pose2d launcherPose = estimatedRobotPose; + double launcherToTargetDistance = + launcherPose.getTranslation().getDistance(target.toPose2d().getTranslation()); + + // Calculate field relative velocity + double xSpeed = DriveState.getInstance().getFieldVelocity().vxMetersPerSecond; + double ySpeed = DriveState.getInstance().getFieldVelocity().vyMetersPerSecond; + + // Account for imparted velocity by robot (launcher) to offset + double timeOfFlight = + passing + ? passingTimeOfFlightMap.get(launcherToTargetDistance) + : hubTimeOfFlightMap.get(launcherToTargetDistance); + Pose2d lookaheadPose = launcherPose; + double lookaheadLauncherToTargetDistance = launcherToTargetDistance; + + for (int i = 0; i < 20; i++) { + timeOfFlight = + passing + ? passingTimeOfFlightMap.get(lookaheadLauncherToTargetDistance) + : hubTimeOfFlightMap.get(lookaheadLauncherToTargetDistance); + double offsetX = xSpeed * timeOfFlight; + double offsetY = ySpeed * timeOfFlight; + lookaheadPose = + new Pose2d( + launcherPose.getTranslation().plus(new Translation2d(offsetX, offsetY)), + launcherPose.getRotation()); + lookaheadLauncherToTargetDistance = + target.getTranslation().toTranslation2d().getDistance(lookaheadPose.getTranslation()); + } + + SmartDashboard.putNumber("Launch Request/Look Ahead Pose/X", lookaheadPose.getX()); + SmartDashboard.putNumber("Launch Request/Look Ahead Pose/Y", lookaheadPose.getY()); + SmartDashboard.putNumber( + "Launch Request/Look Ahead Target Distance", lookaheadLauncherToTargetDistance); + + // calcuate rotation angle + Rotation2d targetRobotAngle = + target.getTranslation().toTranslation2d().minus(lookaheadPose.getTranslation()).getAngle(); + + // Rotation2d targetRobotAngle = getDriveAngle(lookaheadPose, + // target.getTranslation().toTranslation2d()); + + AngularVelocity targetRobotAngularVelocity = + // RadiansPerSecond.of( + // driveAngleFilter.calculate( + // targetRobotAngle + // + // .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()) + // .getRadians() + // / loopPeriodSecs)); + RadiansPerSecond.of( + targetRobotAngle + .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()) + .getRadians()); + + if (builderType == LaunchType.MAPPED) + return new MappedLaunchRequestBuilder() + .createLaunchRequest( + passing, + lookaheadLauncherToTargetDistance, + targetRobotAngularVelocity, + targetRobotAngle, + Meters.of(launcherToTargetDistance)); + else + return new ParabolicLaunchRequestBuilder() + .createLaunchRequest( + passing, + lookaheadLauncherToTargetDistance, + targetRobotAngularVelocity, + targetRobotAngle, + Meters.of(launcherToTargetDistance)); + } + + /* + private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { + Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); + Rotation2d hubAngle = + new Rotation2d( + Math.asin( + MathUtil.clamp( + robotPose.getTranslation().getY() + / target.getDistance(robotPose.getTranslation()), + -1.0, + 1.0))); + Rotation2d driveAngle = fieldToHubAngle.plus(hubAngle).plus(robotPose.getRotation()); + return driveAngle; + } + */ +} diff --git a/src/main/java/frc/robot/statemachines/LaunchState.java b/src/main/java/frc/robot/statemachines/LaunchState.java new file mode 100644 index 0000000..2437c90 --- /dev/null +++ b/src/main/java/frc/robot/statemachines/LaunchState.java @@ -0,0 +1,50 @@ +package frc.robot.statemachines; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Pose3d; +import frc.robot.Constants; +import frc.robot.subsystems.shooter.LaunchRequest; + +@Logged +public class LaunchState { + private static LaunchState single_instance = null; + + private LaunchState() {} + + public static synchronized LaunchState getInstance() { + if (single_instance == null) single_instance = new LaunchState(); + return single_instance; + } + + AllianceState allianceState = AllianceState.getInstance(); + private LaunchCalculator launchCalculator = LaunchCalculator.getInstance(); + + @Logged(name = "Current Launch Request") + private LaunchRequest currentLaunchRequest = null; + + @Logged(name = "3D Target Pose") + private Pose3d targetPose3d = Constants.FieldConstants.getHubTarget(); + + private LaunchType builderType = LaunchType.MAPPED; + + public LaunchRequest getLaunchRequest() { + return currentLaunchRequest; + } + + public void refreshRequest() { + currentLaunchRequest = launchCalculator.refreshRequest(targetPose3d, builderType); + } + + public void setTargetPose3d(Pose3d target) { + this.targetPose3d = target; + } + + public void setBuilderType(LaunchType builderType) { + this.builderType = builderType; + } + + public enum LaunchType { + PARABOLIC, + MAPPED + } +} diff --git a/src/main/java/frc/robot/statemachines/ShiftState.java b/src/main/java/frc/robot/statemachines/ShiftState.java new file mode 100644 index 0000000..7ee9dfc --- /dev/null +++ b/src/main/java/frc/robot/statemachines/ShiftState.java @@ -0,0 +1,117 @@ +package frc.robot.statemachines; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +@Logged +public class ShiftState { + + private static ShiftState single_instance = null; + + public static enum ActiveState { + RED_ACTIVE, + BLUE_ACTIVE, + BOTH_ACTIVE, + UNKNOWN + } + + public static enum Shift { + AUTONOMOUS, + TRANSITION, + SHIFT_1, + SHIFT_2, + SHIFT_3, + SHIFT_4, + ENDGAME, + NONE + } + + @Logged(name = "Shift State") + private ActiveState shiftState = ActiveState.UNKNOWN; + + @Logged(name = "Current Shift") + private Shift currentShift = Shift.NONE; + + private boolean isFMSConnected = DriverStation.isFMSAttached(); + + @Logged(name = "Red Won Auton") + private boolean redWonAuton = true; // Set to true if red wins auton, false if blue wins auton + + private ShiftState() {} + + public static synchronized ShiftState getInstance() { + if (single_instance == null) single_instance = new ShiftState(); + + return single_instance; + } + + public ActiveState getShiftState() { + return shiftState; + } + + private void updateInternalShiftState() { + switch (getShiftFromMatchTime()) { + case AUTONOMOUS, TRANSITION, ENDGAME: // auto, transition, endgame + shiftState = ActiveState.BOTH_ACTIVE; + break; + case SHIFT_1, SHIFT_3: // shift 1 and shift 3 + shiftState = redWonAuton ? ActiveState.BLUE_ACTIVE : ActiveState.RED_ACTIVE; + break; + case SHIFT_2, SHIFT_4: // shift 2 and shift 4 + shiftState = redWonAuton ? ActiveState.RED_ACTIVE : ActiveState.BLUE_ACTIVE; + break; + default: + shiftState = ActiveState.UNKNOWN; + } + } + + public boolean isOurHubActive() { + ActiveState shiftState = getShiftState(); // update shiftState before checking + Alliance alliance = AllianceState.getInstance().getAlliance(); + + if (shiftState == ActiveState.BOTH_ACTIVE) return true; + if (shiftState == ActiveState.UNKNOWN) return false; + + return (shiftState == ActiveState.RED_ACTIVE && alliance == Alliance.Red) + || (shiftState == ActiveState.BLUE_ACTIVE && alliance == Alliance.Blue); + } + + public Shift getShiftFromMatchTime() { + Shift shiftStateInt = Shift.NONE; + double matchTime = DriverStation.getMatchTime(); + if (DriverStation.isAutonomousEnabled()) { // Auton + shiftStateInt = Shift.AUTONOMOUS; + } else if (DriverStation.isTeleopEnabled()) { + if (matchTime >= 130) { // Transition shift + shiftStateInt = Shift.TRANSITION; + } else if (matchTime >= 105) { // Shift 1 + shiftStateInt = Shift.SHIFT_1; + } else if (matchTime >= 80) { // Shift 2 + shiftStateInt = Shift.SHIFT_2; + } else if (matchTime >= 55) { // Shift 3 + shiftStateInt = Shift.SHIFT_3; + } else if (matchTime >= 30) { // Shift 4 + shiftStateInt = Shift.SHIFT_4; + } else { // Endgame + shiftStateInt = Shift.ENDGAME; + } + } + return shiftStateInt; + } + + public void periodic() { + // Update FMS connection status + isFMSConnected = DriverStation.isFMSAttached(); // Update periodically + currentShift = getShiftFromMatchTime(); // Update current shift based on match time + + if (isFMSConnected) { + String gameData = DriverStation.getGameSpecificMessage(); + if (gameData.length() > 0) { + redWonAuton = (gameData.charAt(0) == 'R'); // true if red won auton, false if blue won auton + } + } + // Update the internal shiftState based on current match time and redWonAuton + updateInternalShiftState(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 31034be..9b89b31 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -26,10 +26,16 @@ public class DriveConstants { .withSteerRequestType(SteerRequestType.Position) // Closed Loop Steer (Default) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); // Prevents "flips" + // request with only drive deadbands since we're auto steer.= + public static final SwerveRequest.FieldCentric AUTO_AIM_DRIVE_REQUEST = + new FieldCentric() + .withDeadband(MAX_DRIVE_SPEED * DEADBAND_FACTOR) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.Position); + public static final double TRANSLATION_ALIGN_TOLERANCE = 0.01; // meters public static final double ROTATION_ALIGN_TOLERANCE = 1; // degrees public static final double TRANSLATION_ALIGN_KP = 0.0001; public static final double ROTATION_ALIGN_KP = 6.28; - } diff --git a/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java b/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java index 2af7b14..170e73b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java +++ b/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java @@ -4,15 +4,17 @@ public final class DrivePreferences { - protected static DoublePreference translation_kP = + public static DoublePreference translation_kP = new DoublePreference("Drive/Translation/kP", DriveConstants.TRANSLATION_ALIGN_KP); - protected static DoublePreference translation_kD = - new DoublePreference("Drive/Translation/kD", 0); + public static DoublePreference translation_kD = new DoublePreference("Drive/Translation/kD", 0); - protected static DoublePreference rotation_kP = + public static DoublePreference rotation_kP = new DoublePreference("Drive/Rotation/kP", DriveConstants.ROTATION_ALIGN_KP); - protected static DoublePreference rotation_kD = new DoublePreference("Drive/Rotation/kD", 0); + public static DoublePreference rotation_kD = new DoublePreference("Drive/Rotation/kD", 0); public static DoublePreference onemeter_speed = new DoublePreference("Drive/Speed for 1m (command)"); + + public static DoublePreference autoAim_kP = new DoublePreference("Drive/AutoAim/kP", 1); + public static DoublePreference autoAim_kD = new DoublePreference("Drive/AutoAim/kD", 0); } diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java index 6e07d96..039e9e5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java @@ -46,6 +46,7 @@ public DrivetrainSubsystem() { @Override public void periodic() { super.periodic(); + for (VisionMeasurement estimate : driveState.grabVisionEstimateList(CameraConstants.photonCameraName_Front)) { addVisionMeasurement( @@ -53,6 +54,23 @@ public void periodic() { estimate.getTimestamp(), estimate.getTrust()); } + + for (VisionMeasurement estimate : + driveState.grabVisionEstimateList(CameraConstants.photonCameraName_Left)) { + addVisionMeasurement( + estimate.getEstimatedPose().estimatedPose.toPose2d(), + estimate.getTimestamp(), + estimate.getTrust()); + } + + for (VisionMeasurement estimate : + driveState.grabVisionEstimateList(CameraConstants.photonCameraName_Right)) { + addVisionMeasurement( + estimate.getEstimatedPose().estimatedPose.toPose2d(), + estimate.getTimestamp(), + estimate.getTrust()); + } + driveState.adjustCurrentDriveStats(this.getStateCopy()); SmartDashboard.putNumber( diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 6ad3636..196908b 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -1,13 +1,18 @@ package frc.robot.subsystems.indexer; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; final class IndexerConstants { private IndexerConstants() {} // TODO Replace with real id - protected static final int INDEXER_MOTOR_ID = 6; + protected static final int INDEXER_MOTOR_LEADER_ID = 3; + protected static final int INDEXER_MOTOR_FOLLOWER_ID = 9; + protected static final int ACCELERATOR_MOTOR_ID = 8; // TODO: Tune motor @@ -24,4 +29,39 @@ protected static Slot0Configs createIndexerMotorSlot0Configs() { slot.kD = INDEXER_KD; return slot; } + + protected static final double ACCELERATOR_KS = 0; + protected static final double ACCELERATOR_KV = 0; + protected static final double ACCELERATOR_KP = 0; + protected static final double ACCELERATOR_KD = 0; + + protected static Slot0Configs createAcceleratorMotorSlot0Configs() { + Slot0Configs slot = new Slot0Configs(); + slot.kS = ACCELERATOR_KS; + slot.kV = ACCELERATOR_KV; + slot.kP = ACCELERATOR_KP; + slot.kD = ACCELERATOR_KD; + return slot; + } + + public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } + + public static MotorOutputConfigs createFollowerMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } + + public static MotorOutputConfigs createAcceleratorMotorOutputsConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java index 9e67f54..90984ca 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java @@ -8,4 +8,19 @@ private IndexerPreferences() {} protected static DoublePreference indexSpeed = new DoublePreference("Indexer/Index Speed", 0.1); // in rotations per second + + protected static DoublePreference indexerPercent = + new DoublePreference("Indexer/Index Percent (for without PID)", 0.1); // in percent + + protected static DoublePreference indexerReversePercent = + new DoublePreference("Indexer/Reverse Index Percent (for without PID)", -0.1); // in percent + + protected static DoublePreference acceleratorPercent = + new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent + + protected static DoublePreference indexerRunTime = + new DoublePreference("Indexer/Pulsing Run Time", 2.0); // in seconds + + protected static DoublePreference indexerPauseTime = + new DoublePreference("Indexer/Pulsing Pause Time", 0.1); // in seconds } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index d1c42f9..d14e035 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -1,35 +1,137 @@ package frc.robot.subsystems.indexer; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @Logged public class IndexerSubsystem extends SubsystemBase { - private final TalonFX indexerMotor; + private final TalonFX indexerMotorLeader; + private final TalonFX indexerMotorFollower; + private final TalonFX acceleratorMotor; @Logged(name = "Indexer Velocity Target", importance = Importance.CRITICAL) private AngularVelocity indexerVelocityTarget; // RotationsPerSecond + @Logged(name = "Accelerator Velocity Target", importance = Importance.CRITICAL) + private AngularVelocity acceleratorVelocityTarget; + private VelocityVoltage indexerControl; + private VelocityVoltage acceleratorControl; + + final SysIdRoutine m_sysIdRoutineIndexer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdIndexer_State", state.toString())), + new SysIdRoutine.Mechanism(output -> setIndexerVoltage(output.magnitude()), null, this)); + + final SysIdRoutine m_sysIdRoutineAccelerator = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdAccelerator_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setAcceleratorVoltage(output.magnitude()), null, this)); public IndexerSubsystem() { - indexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR_ID); - indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); + indexerMotorLeader = new TalonFX(IndexerConstants.INDEXER_MOTOR_LEADER_ID); + indexerMotorFollower = new TalonFX(IndexerConstants.INDEXER_MOTOR_FOLLOWER_ID); + acceleratorMotor = new TalonFX(IndexerConstants.ACCELERATOR_MOTOR_ID); + + indexerMotorLeader.getConfigurator().apply(IndexerConstants.createLeaderMotorOutputConfigs()); + indexerMotorFollower + .getConfigurator() + .apply(IndexerConstants.createFollowerMotorOutputConfigs()); + + indexerMotorLeader.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); + indexerMotorFollower.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); + + acceleratorMotor.getConfigurator().apply(IndexerConstants.createAcceleratorMotorSlot0Configs()); + acceleratorMotor + .getConfigurator() + .apply(IndexerConstants.createAcceleratorMotorOutputsConfigs()); + + indexerMotorFollower.setControl( + new Follower(indexerMotorLeader.getDeviceID(), MotorAlignmentValue.Opposed)); + indexerVelocityTarget = RotationsPerSecond.of(0); + acceleratorVelocityTarget = RotationsPerSecond.of(0); + indexerControl = new VelocityVoltage(0); + acceleratorControl = new VelocityVoltage(0); } @Override public void periodic() { - indexerMotor.setControl( - indexerControl.withVelocity(indexerVelocityTarget.in(RotationsPerSecond))); + // TODO: removed for testing only. PUT IT BACK! + // indexerMotor.setControl( + // indexerControl.withVelocity(indexerVelocityTarget.in(RotationsPerSecond))); + } + + private void setIndexerVoltage(double magnitude) { + indexerMotorLeader.setVoltage(magnitude); + } + + private void setAcceleratorVoltage(double magnitude) { + acceleratorMotor.setVoltage(magnitude); + } + + public Command startIndexerNoPID() { + return run(() -> indexerMotorLeader.set(IndexerPreferences.indexerPercent.getValue())) + .withName("Set Indexer Percent"); + } + + public Command startIndexerReverseNoPID() { + return run(() -> indexerMotorLeader.set(IndexerPreferences.indexerReversePercent.getValue())) + .withName("Set Indexer Reverse Percent"); + } + + public Command startAcceleratorNoPID() { + return run(() -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue())) + .withName("Set Acceleration Percent"); + } + + public Command stopIndexerNoPID() { + return runOnce(() -> indexerMotorLeader.set(0)).withName("Stop Indexer Percent"); + } + + public Command stopAcceleratorNoPID() { + return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent"); + } + + public Command startFullIndexingNoPID() { + return run(() -> { + indexerMotorLeader.set(IndexerPreferences.indexerPercent.getValue()); + acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); + }) + .withName("Start Full Indexing No PID"); + } + + public Command stopFullIndexingNoPID() { + return run(() -> { + indexerMotorLeader.set(0); + acceleratorMotor.set(0); + }) + .withName("Stop Full Indexing No PID"); } public Command indexCommand() { @@ -44,4 +146,25 @@ public Command stopCommand() { return runOnce(() -> indexerVelocityTarget = RotationsPerSecond.of(0)) .withName("Stop Indexing"); } + + public Command pulsingIndexCommand() { + Timer timer = new Timer(); + return runEnd( + () -> { + double cycleTime = + IndexerPreferences.indexerRunTime.getValue() + + IndexerPreferences.indexerPauseTime.getValue(); + double elapsed = timer.get() % cycleTime; + boolean shouldRun = elapsed < IndexerPreferences.indexerRunTime.getValue(); + + indexerMotorLeader.set(shouldRun ? IndexerPreferences.indexerPercent.getValue() : 0); + acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); + }, + () -> { + indexerMotorLeader.set(0); + acceleratorMotor.set(0); + }) + .beforeStarting(() -> timer.restart()) + .withName("Pulsing Index"); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 58189de..89cb56a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,7 +1,9 @@ package frc.robot.subsystems.intake; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.signals.NeutralModeValue; public class IntakeConstants { @@ -9,7 +11,7 @@ private IntakeConstants() {} // TODO: Change to actual ports public static final int ROLLER_MOTOR_ID = 2; - public static final int EXTENSION_MOTOR_ID = 3; + public static final int EXTENSION_MOTOR_ID = 1; // TODO: Tune Roller and Extension Motors @@ -53,6 +55,13 @@ public static SoftwareLimitSwitchConfigs createExtensionSoftwareLimitSwitchConfi return configs; } + public static MotorOutputConfigs createExtensionMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + // newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } + public static final double SAFE_HOMING_EFFORT = -0.2; public static final double SAFE_STATOR_LIMIT = 0.8; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePreferences.java b/src/main/java/frc/robot/subsystems/intake/IntakePreferences.java index 34fc899..22e28f9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakePreferences.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakePreferences.java @@ -11,4 +11,17 @@ private IntakePreferences() {} new DoublePreference("Intake/Roller Intake Speed", 0.1); // in rotations per second public static DoublePreference intakeCollectPosition = new DoublePreference("Intake/Stowed Intake Position", 3); // in rotations + + public static DoublePreference extendPercent = + new DoublePreference("Intake/Extend Percent (for without PID)", 0.1); // in percent + public static DoublePreference retractPercent = + new DoublePreference("Intake/Retract Percent (for without PID)", -0.1); // in percent + + public static DoublePreference rollerIntakePercent = + new DoublePreference("Intake/Roller Intake Percent (for without PID)", 0.1); // in percent + public static DoublePreference rollerOutakePercent = + new DoublePreference("Intake/Roller Outake Percent (for without PID)", -0.1); // in percent + + public static DoublePreference dislodgePosition = + new DoublePreference("Intake/Extension Dislodge Position", 1.5); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 5c2ddbf..9cd0c98 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -2,7 +2,9 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -13,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @Logged public class IntakeSubsystem extends SubsystemBase { @@ -29,6 +32,16 @@ public class IntakeSubsystem extends SubsystemBase { private PositionTorqueCurrentFOC extensionControl; + final SysIdRoutine m_sysIdRoutineRoller = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRoller_State", state.toString())), + new SysIdRoutine.Mechanism(output -> setRollerVoltage(output.magnitude()), null, this)); + public IntakeSubsystem() { rollerMotor = new TalonFX(IntakeConstants.ROLLER_MOTOR_ID); extensionMotor = new TalonFX(IntakeConstants.EXTENSION_MOTOR_ID); @@ -41,6 +54,7 @@ public IntakeSubsystem() { extensionMotor .getConfigurator() .apply(IntakeConstants.createExtensionSoftwareLimitSwitchConfigs()); + extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorOutputConfigs()); extensionMotor.setPosition(0); extensionTarget = Rotations.of(0); extensionControl = new PositionTorqueCurrentFOC(0); @@ -49,7 +63,10 @@ public IntakeSubsystem() { @Override public void periodic() { rollerMotor.setControl(rollerControl.withVelocity(rollerVelocityTarget.in(RotationsPerSecond))); - extensionMotor.setControl(extensionControl.withPosition(extensionTarget.in(Rotations))); + extensionMotor.setControl( + extensionControl + .withPosition(extensionTarget.in(Rotations)) + .withOverrideCoastDurNeutral(true)); } public Command spinRollerCommand() { @@ -65,6 +82,24 @@ public Command stopRollerCommand() { .withName("Stop Intake Roller"); } + private void setRollerVoltage(double magnitude) { + rollerMotor.setVoltage(magnitude); + } + + public Command startRollerNoPID() { + return run(() -> rollerMotor.set(IntakePreferences.rollerIntakePercent.getValue())) + .withName("Set Roller Percent"); + } + + public Command startRollerReverseNoPID() { + return run(() -> rollerMotor.set(IntakePreferences.rollerOutakePercent.getValue())) + .withName("Set Roller Reverse Percent"); + } + + public Command stopRollerNoPID() { + return runOnce(() -> rollerMotor.set(0)).withName("Stop Roller No PID"); + } + @Logged(name = "Extension Setpoint", importance = Importance.CRITICAL) public boolean atExtensionSetpoint() { return Math.abs(extensionMotor.getPosition().getValueAsDouble() - extensionTarget.in(Rotations)) @@ -76,6 +111,20 @@ public Command setIntakeExtensionCommand(Angle position) { .andThen(Commands.waitUntil(() -> atExtensionSetpoint())); } + public Command setExtendNoPID() { + return run(() -> extensionMotor.set(IntakePreferences.extendPercent.getValue())) + .withName("Extend Intake Percent"); + } + + public Command setRetractNoPID() { + return run(() -> extensionMotor.set(IntakePreferences.retractPercent.getValue())) + .withName("Retract Intake Percent"); + } + + public Command stopExtensionNoPID() { + return runOnce(() -> extensionMotor.set(0)).withName("Stop Intake Percent"); + } + public Command collectCommand() { return setIntakeExtensionCommand( Rotations.of(IntakePreferences.intakeCollectPosition.getValue())) @@ -84,11 +133,23 @@ public Command collectCommand() { } public Command stowCommand() { - return stopRollerCommand() - .andThen(setIntakeExtensionCommand(Rotations.of(0))) + return setIntakeExtensionCommand(Rotations.of(0)) + .andThen(stopRollerCommand()) + .andThen(setIntakeExtensionCommand(Rotations.of(0)).repeatedly()) .withName("Stow Intake"); } + public Command dislodgeCommand() { + return spinRollerCommand() + .andThen( + setIntakeExtensionCommand(Rotations.of(IntakePreferences.dislodgePosition.getValue())) + .andThen( + setIntakeExtensionCommand( + Rotations.of(IntakePreferences.intakeCollectPosition.getValue())))) + .repeatedly() + .withName("Dislodge Intake"); + } + public Command homeIntakeCommand() { return runEnd( () -> extensionMotor.set(IntakeConstants.SAFE_HOMING_EFFORT), diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java new file mode 100644 index 0000000..4ba8975 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; + +@Logged +public class LaunchRequest { + @Logged(name = "Launch Hood Angle", importance = Logged.Importance.CRITICAL) + private Angle launchHoodTarget; + + @Logged(name = "Launch Flywheel Velocity", importance = Logged.Importance.CRITICAL) + private AngularVelocity launchVelocity; + + @Logged(name = "Target Robot Angular Velocity", importance = Logged.Importance.CRITICAL) + private AngularVelocity targetRobotAngularVelocity; + + @Logged(name = "Target Robot Angle", importance = Logged.Importance.CRITICAL) + private Rotation2d targetRobotAngle; + + @Logged(name = "Robot distance to Target", importance = Logged.Importance.CRITICAL) + private Distance targetDistance; + + private double timestamp; + + public LaunchRequest( + Angle launchHoodTarget, + AngularVelocity launchVelocity, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle, + Distance targetDistance, + double timestamp) { + this.launchHoodTarget = launchHoodTarget; + this.launchVelocity = launchVelocity; + this.targetRobotAngularVelocity = targetRobotAngularVelocity; + this.targetRobotAngle = targetRobotAngle; + this.targetDistance = targetDistance; + this.timestamp = timestamp; + } + + public Angle getHoodTarget() { + return launchHoodTarget; + } + + public AngularVelocity getFlywheelVelocity() { + return launchVelocity; + } + + public AngularVelocity getTargetRobotAngularVelocity() { + return targetRobotAngularVelocity; + } + + public Rotation2d getTargetRobotAngle() { + return targetRobotAngle; + } + + public Distance getTargetDistance() { + return targetDistance; + } + + public double getTimestamp() { + return timestamp; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java new file mode 100644 index 0000000..f9c4ded --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; + +public interface LaunchRequestBuilder { + public LaunchRequest createLaunchRequest( + boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle, + Distance targetDistance); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java new file mode 100644 index 0000000..15184c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -0,0 +1,94 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.Utils; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.InverseInterpolator; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; + +/** Add your docs here. */ +public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { + + private static final double minDistance; + private static final double maxDistance; + private static final double passingMinDistance; + private static final double passingMaxDistance; + + // Launching Maps + private static final InterpolatingTreeMap hoodAngleMap = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); + private static final InterpolatingDoubleTreeMap flywheelSpeedMap = + new InterpolatingDoubleTreeMap(); + + // Passing Maps + private static final InterpolatingTreeMap passingHoodAngleMap = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); + private static final InterpolatingDoubleTreeMap passingFlywheelSpeedMap = + new InterpolatingDoubleTreeMap(); + + // TODO: All of this is made up. Need real numbers. + static { + minDistance = 0.9; + maxDistance = 4.0; + passingMinDistance = 0.0; + passingMaxDistance = 100000; + + hoodAngleMap.put(0.99, Rotation2d.fromRotations(0.0)); + hoodAngleMap.put(1.62, Rotation2d.fromRotations(1.0)); + hoodAngleMap.put(1.94, Rotation2d.fromRotations(2.4)); + hoodAngleMap.put(2.53, Rotation2d.fromRotations(3.2)); + hoodAngleMap.put(3.00, Rotation2d.fromRotations(3.8)); + hoodAngleMap.put(3.51, Rotation2d.fromRotations(3.8)); + hoodAngleMap.put(6.00, Rotation2d.fromRotations(6.1)); // put in a value to max out the hood + + flywheelSpeedMap.put(0.99, 57.7); + flywheelSpeedMap.put(1.62, 60.9); + flywheelSpeedMap.put(1.94, 66.7); + flywheelSpeedMap.put(2.53, 68.8); + flywheelSpeedMap.put(3.00, 72.5); + flywheelSpeedMap.put(3.51, 78.0); + flywheelSpeedMap.put(6.00, 108.0); // put in a value to max out the hood + + passingHoodAngleMap.put(passingMinDistance, Rotation2d.fromDegrees(0.0)); + passingHoodAngleMap.put(passingMaxDistance, Rotation2d.fromDegrees(0.0)); + + passingFlywheelSpeedMap.put(passingMinDistance, 0.0); + passingFlywheelSpeedMap.put(passingMaxDistance, 0.0); + } + + public LaunchRequest createLaunchRequest( + boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle, + Distance targetDistance) { + + // calculate hood angle + double hoodAngle = + passing + ? passingHoodAngleMap.get(distance).getRotations() + : hoodAngleMap.get(distance).getRotations(); + + // calculate flywheel speed + double flywheelSpeed = + passing ? passingFlywheelSpeedMap.get(distance) : flywheelSpeedMap.get(distance); + + return new LaunchRequest( + Angle.ofBaseUnits(hoodAngle, Rotations), + AngularVelocity.ofBaseUnits(flywheelSpeed, RotationsPerSecond), + targetRobotAngularVelocity, + targetRobotAngle, + targetDistance, + Utils.getCurrentTimeSeconds()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java new file mode 100644 index 0000000..b1f2304 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -0,0 +1,85 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.Utils; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; + +/** Add your docs here. */ +public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder { + + public LaunchRequest createLaunchRequest( + boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle, + Distance targetDistance) { + double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); + double x2 = distance; + double y2 = passing ? 0 : ShooterConstants.HUB_HEIGHT.in(Meters); + + double slope; + if (passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; + else slope = ShooterConstants.OPTIMAL_HUB_ENTRY_SLOPE; + + double a, b, vertex, hitWallCheckX; + Angle theta, motorAngle; + int count = 0; + do { + // system of equations + // (y2) = a(x2*x2) + b(x2) + y1 + // slope = 2a(x2) + b + a = (slope * x2 + y1 - y2) / (x2 * x2); + b = (slope - 2 * a * x2); + theta = Radians.of(Math.atan(b)); // launch angle (Hood Angle Conversion: MATH.PI/2 - theta) + motorAngle = Radians.of(Math.PI / 2 - theta.in(Radians)); + vertex = -1 * b / (2 * a); + hitWallCheckX = x2 - ShooterConstants.FROM_HUB_CENTER_TO_WALL.in(Meters); + slope -= 0.05; + } while (count++ < 30 + && (!passing + && a * Math.pow(hitWallCheckX, 2) + b * hitWallCheckX + y1 + < ShooterConstants.HUB_HEIGHT.in(Meters)) + && (motorAngle.in(Degrees) > ShooterConstants.MIN_HOOD_ANGLE.in(Degrees))); + + if (motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)) return null; + + motorAngle = + Rotations.of( + motorAngle.in(Degrees) + * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in( + Rotations)); // converts from hood angle to motor rotations + + // system of equations + // (-b/2a) = (velocity)*cos(theta)*t + // 2g(t) = (velocity)*sin(theta) + LinearVelocity velocity = + MetersPerSecond.of( + Math.sqrt( + 2 * 9.8 * vertex / (Math.sin(theta.in(Radians)) * Math.cos(theta.in(Radians))))); + + // Calculate angular velocity using the formula: angular velocity (rotations per second) = + // linear velocity / circumference + double angularVelocityMagnitude = + velocity.in(MetersPerSecond) / (Math.PI * 2 * ShooterConstants.FLYWHEEL_RADIUS.in(Meters)); + + // Create a typed AngularVelocity object (optional, for use within the units library ecosystem) + AngularVelocity angularVelocity = RotationsPerSecond.of(angularVelocityMagnitude); + + return new LaunchRequest( + motorAngle, + angularVelocity, + targetRobotAngularVelocity, + targetRobotAngle, + targetDistance, + Utils.getCurrentTimeSeconds()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 0273c99..84f27e4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,9 +2,12 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; @@ -13,32 +16,46 @@ public class ShooterConstants { private ShooterConstants() {} - // TODO: Change to actual ports - public static final int FLYWHEEL_MOTOR_ID = 4; - public static final int HOOD_MOTOR_ID = 5; + public static final int FLYWHEEL_LEADER_MOTOR_ID = 5; + public static final int FLYWHEEL_FOLLOWER_MOTOR_ID = 6; + public static final int HOOD_MOTOR_ID = 4; + + public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } + + public static MotorOutputConfigs createFollowerMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } // TODO: Tune Flywheel and Hood Motor // Flywheel motor - public static final double FLYWHEEL_KS = 0; - public static final double FLYWHEEL_KV = 0; - public static final double FLYWHEEL_KP = 0; - public static final double FLYWHEEL_KD = 0; + public static final double FLYWHEEL_KS = 0.11239; + public static final double FLYWHEEL_KV = 0.11589; + public static final double FLYWHEEL_KA = 0.0034356; + public static final double FLYWHEEL_KP = 0.066945 * 2; public static Slot0Configs createFlywheelMotorSlot0Configs() { Slot0Configs slot = new Slot0Configs(); slot.kS = FLYWHEEL_KS; slot.kV = FLYWHEEL_KV; slot.kP = FLYWHEEL_KP; - slot.kD = FLYWHEEL_KD; + slot.kA = FLYWHEEL_KA; return slot; } public static final double ALLOWABLE_HOOD_ERROR = 0.1; - public static final double HOOD_FORWARD_LIMIT = 100; + public static final double HOOD_FORWARD_LIMIT = 6.2; public static final double HOOD_REVERSE_LIMIT = 0; public static final double HOOD_KS = 0; - public static final double HOOD_KP = 0; + public static final double HOOD_KP = 2.0; public static final double HOOD_KD = 0; public static Slot0Configs createHoodMotorSlot0Configs() { @@ -51,26 +68,32 @@ public static Slot0Configs createHoodMotorSlot0Configs() { public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() { SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); - configs.ForwardSoftLimitEnable = false; - configs.ReverseSoftLimitEnable = false; + configs.ForwardSoftLimitEnable = true; + configs.ReverseSoftLimitEnable = true; configs.ForwardSoftLimitThreshold = HOOD_FORWARD_LIMIT; configs.ReverseSoftLimitThreshold = HOOD_REVERSE_LIMIT; return configs; } + public static MotorOutputConfigs createHoodMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } + public static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); public static final Current SAFE_STATOR_LIMIT = Amp.of(0.8); // Conversion Constants - public static final Angle ROTATIONS_PER_LAUNCH_DEGREE = - Rotations.of(1); // TODO: Get Better Estimate - public static final Distance FLYWHEEL_RADIUS = Inch.of(1); // TODO: Get Better Estimate + public static final Angle ROTATIONS_PER_LAUNCH_DEGREE = Rotations.of(0.2); + public static final Distance FLYWHEEL_RADIUS = Inch.of(2); // Lemon Yeeting Constants - public static final Distance SHOOTER_HEIGHT = Inch.of(65); // TODO: Get Better Estimate - public static final Distance GOAL_HEIGHT = Inch.of(23); // TODO: Get Better Estimate - public static final Distance OFFSET_DISTANCE = Meter.of(1); // TODO: Get Better Estimate - public static final Distance MIN_VERTEX_DISTANCE = Inch.of(23.5); - public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate - public static final double OPTIMAL_ENTRY_SLOPE = -1; // TODO: Tune + public static final Distance SHOOTER_HEIGHT = Inch.of(25.5); + public static final Distance HUB_HEIGHT = Inch.of(71.5); + public static final Distance FROM_HUB_CENTER_TO_WALL = Inch.of(23.5); + public static final Angle MIN_HOOD_ANGLE = Degrees.of(20); + public static final double OPTIMAL_PASSING_ENTRY_SLOPE = -1; // TODO: Tune + public static final double OPTIMAL_HUB_ENTRY_SLOPE = -1; // TODO: Tune } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java index 0a8c399..cf472ab 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java @@ -10,4 +10,23 @@ private ShooterPreferences() {} new DoublePreference("Shooter/Launch Speed", 0.1); // in rotations per second public static DoublePreference hoodLaunchAngle = new DoublePreference("Shooter/Hood Launch Position", 3); // in rotations + + public static DoublePreference flywheelLaunchPercent = + new DoublePreference("Shooter/Launch Percent (for without PID)", 0.1); // in percent + + public static DoublePreference hoodkP = new DoublePreference("Shooter/Hood/kP", 1); + public static DoublePreference hoodkD = new DoublePreference("Shooter/Hood/kD", 0); + + public static DoublePreference hoodTargetPreference = + new DoublePreference("Shooter/Hood/Target Rotations", 0); + + // unit is *radians per second** for velocity. + public static DoublePreference tuningDefaultFlywheelRPS = + new DoublePreference("Shooter/Tuning/FlywheelRPS", 1000.0 / 60.0 / 2 * Math.PI); + public static DoublePreference tuningDefaultFlywheelStepRPS = + new DoublePreference("Shooter/Tuning/FlywheelStepRPS", 100.0 / 60.0 / 2 * Math.PI); + public static DoublePreference tuningDefaultHoodRotations = + new DoublePreference("Shooter/Tuning/HoodRotations", 0.0); + public static DoublePreference tuningDefaultHoodStepRotations = + new DoublePreference("Shooter/Tuning/HoodStepRotations", 0.2); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 67ab208..f27aaf9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -2,77 +2,92 @@ import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.intake.IntakeConstants; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.statemachines.LaunchState; @Logged public class ShooterSubsystem extends SubsystemBase { - private final TalonFX flywheelMotor; + private final TalonFX flywheelMotorLeader; + private final TalonFX flywheelMotorFollower; private final TalonFX hoodMotor; - @Logged(name = "Velocity Target", importance = Importance.CRITICAL) - private AngularVelocity velocityTarget; // Rotations Per Second + @Logged(name = "Velocity Target rads/s", importance = Importance.CRITICAL) + private AngularVelocity velocityTarget; // *rads* Per Second is the base unit. private VelocityVoltage velocityControl; - @Logged(name = "Hood Target", importance = Importance.CRITICAL) - private Angle hoodTarget; // Rotations + @Logged(name = "Hood Target (radians)", importance = Importance.CRITICAL) + private Angle hoodTarget; // radians is the base unit. - private PositionTorqueCurrentFOC hoodControl; + private PositionVoltage hoodControl; - public class LaunchRequest { - private Angle launchHoodTarget; - private AngularVelocity launchVelocity; + private final LaunchState launchState = LaunchState.getInstance(); - public LaunchRequest(Angle theta, LinearVelocity velocity) { - // hood angle in degrees (0 degrees is all the way back) is converted to motor rotations - launchHoodTarget = - Rotations.of( - theta.in(Degrees) * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in(Rotations)); - // meters per second is converted to rotations per second of the flywheel - launchVelocity = - RotationsPerSecond.of( - velocity.in(MetersPerSecond) - / (2 * Math.PI * ShooterConstants.FLYWHEEL_RADIUS.in(Meters))); - } - - public Angle getHoodTarget() { - return launchHoodTarget; - } - - public AngularVelocity getVelocityTarget() { - return launchVelocity; - } - } + final SysIdRoutine m_sysIdRoutineFlywheel = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdFlywheel_State", state.toString())), + new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); public ShooterSubsystem() { - flywheelMotor = new TalonFX(ShooterConstants.FLYWHEEL_MOTOR_ID); + flywheelMotorLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEADER_MOTOR_ID); + flywheelMotorFollower = new TalonFX(ShooterConstants.FLYWHEEL_FOLLOWER_MOTOR_ID); hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID); - flywheelMotor.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs()); + flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs()); + flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createLeaderMotorOutputConfigs()); + flywheelMotorFollower + .getConfigurator() + .apply(ShooterConstants.createFlywheelMotorSlot0Configs()); + flywheelMotorFollower + .getConfigurator() + .apply(ShooterConstants.createFollowerMotorOutputConfigs()); + // flywheelMotorFollower.setControl( + // new Follower(flywheelMotorLeader.getDeviceID(), MotorAlignmentValue.Opposed)); + velocityTarget = RotationsPerSecond.of(0); velocityControl = new VelocityVoltage(0); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorOutputConfigs()); hoodTarget = Rotations.of(0); - hoodControl = new PositionTorqueCurrentFOC(0); + hoodControl = new PositionVoltage(0); } @Override public void periodic() { - flywheelMotor.setControl(velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); - hoodMotor.setControl(hoodControl.withVelocity(hoodTarget.in(Rotations))); + launchState.refreshRequest(); + flywheelMotorLeader.setControl( + velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); + flywheelMotorFollower.setControl( + velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); + hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); + } + + @Logged(name = "Velocity Target RPM", importance = Importance.CRITICAL) + public double getFlywheelTargetRPM() { + return velocityTarget.in(RotationsPerSecond) * 60; + } + + @Logged(name = "Hood Angle Rotations", importance = Importance.CRITICAL) + public double getHoodTargetRotations() { + return hoodTarget.in(Rotations); } public Command spinFlywheelCommand() { @@ -83,15 +98,33 @@ public Command spinFlywheelCommand() { .withName("Start Spinning Flywheel"); } + public Command spinFlywheelCommand(AngularVelocity v) { + return runOnce(() -> velocityTarget = v).withName("Spinning To AV"); + } + public Command stopFlywheelCommand() { return runOnce(() -> velocityTarget = RotationsPerSecond.of(0)) .withName("Stop Spinning Flywheel"); } + public Command stowHood() { + return runOnce(() -> hoodTarget = Rotations.of(0)); + } + + public Command setHoodToPreference() { + return runOnce( + () -> hoodTarget = Rotations.of(ShooterPreferences.hoodTargetPreference.getValue())); + } + + private void setFlywheelVoltage(double magnitude) { + flywheelMotorLeader.setVoltage(magnitude); + flywheelMotorFollower.setVoltage(magnitude); + } + @Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL) public boolean atHoodSetpoint() { return Math.abs(hoodMotor.getPosition().getValueAsDouble() - hoodTarget.in(Rotations)) - < IntakeConstants.ALLOWABLE_EXTENSION_ERROR; + < ShooterConstants.ALLOWABLE_HOOD_ERROR; } public Command setHoodCommand(Angle position) { @@ -106,6 +139,36 @@ public Command launchLemonsCommand() { .withName("Start Launching Lemons"); } + public Command spinFlywheelRanged() { + return run( + () -> { + velocityTarget = launchState.getLaunchRequest().getFlywheelVelocity(); + hoodTarget = launchState.getLaunchRequest().getHoodTarget(); + }); + } + + public Command launchLemonsCommandNoPID() { + return setHoodCommand(Rotations.of(ShooterPreferences.hoodLaunchAngle.getValue())) + .andThen( + runOnce( + () -> { + flywheelMotorLeader.set(ShooterPreferences.flywheelLaunchPercent.getValue()); + flywheelMotorFollower.set(ShooterPreferences.flywheelLaunchPercent.getValue()); + })) + .withName("Start Launching Lemons (No PID)"); + } + + public Command stopLaunchLemonsNoPIDCommand() { + return setHoodCommand(Rotations.of(0)) + .andThen( + runOnce( + () -> { + flywheelMotorLeader.set(0); + flywheelMotorFollower.set(0); + })) + .withName("Stop Launching Lemons (No PID)"); + } + public Command stowCommand() { return stopFlywheelCommand().andThen(setHoodCommand(Rotations.of(0))).withName("Stow Shooter"); } @@ -121,41 +184,66 @@ public Command homeShooterCommand() { }); } - // helper method to find, given a distance from the robot to the tag, - // (1) necessary angle of the hood - // (2) necessary velocity of flywheel - // to land a lemon in the goal - public LaunchRequest createLaunchRequest(double distanceToTag) { - double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); - double x2 = distanceToTag + ShooterConstants.OFFSET_DISTANCE.in(Meters); - double y2 = ShooterConstants.GOAL_HEIGHT.in(Meters); - - double slope = ShooterConstants.OPTIMAL_ENTRY_SLOPE; - double a, b, vertex; - Angle theta, motorAngle; - do { - // system of equations - // (y2) = a(x2*x2) + b(x2) + y1 - // slope = 2a(x2) + b - a = (slope * x2 + y1 - y2) / (x2 * x2); - b = (slope - 2 * a * x2); - theta = Radians.of(Math.atan(b)); // launch angle (Hood Angle Conversion: MATH.PI/2 - theta) - motorAngle = Radians.of(Math.PI / 2 - theta.in(Radians)); - vertex = -1 * b / (2 * a); - slope -= 0.05; - } while ((vertex > x2 - ShooterConstants.MIN_VERTEX_DISTANCE.in(Meters)) - && !(motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees))); - - if (motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)) return null; - - // system of equations - // (-b/2a) = (velocity)*cos(theta)*t - // 2g(t) = (velocity)*sin(theta) - LinearVelocity velocity = - MetersPerSecond.of( - Math.sqrt( - 2 * 9.8 * vertex / (Math.sin(theta.in(Radians)) * Math.cos(theta.in(Radians))))); - - return new LaunchRequest(theta, velocity); + public Command increaseFlywheelCommand() { + return runOnce( + () -> + velocityTarget = + RadiansPerSecond.of( + velocityTarget.magnitude() + + ShooterPreferences.tuningDefaultFlywheelStepRPS.getValue())) + .withName("Increase FlyWheel Speed"); + } + + public Command decreaseFlywheelCommand() { + return runOnce( + () -> + velocityTarget = + RadiansPerSecond.of( + velocityTarget.magnitude() + - ShooterPreferences.tuningDefaultFlywheelStepRPS.getValue())) + .withName("Decrease FlyWheel Speed"); + } + + public Command increaseHoodCommand() { + return runOnce( + () -> + hoodTarget = + Rotations.of( + hoodTarget.in(Rotations) + + ShooterPreferences.tuningDefaultHoodStepRotations.getValue())) + .withName("Increase Hood Angle"); + } + + public Command decreaseHoodCommand() { + return runOnce( + () -> + hoodTarget = + Rotations.of( + hoodTarget.in(Rotations) + - ShooterPreferences.tuningDefaultHoodStepRotations.getValue())) + .withName("Decrease Hood Angle"); + } + + public Command startShooterTuningCommand() { + return spinFlywheelCommand( + RadiansPerSecond.of(ShooterPreferences.tuningDefaultFlywheelRPS.getValue())) + .andThen( + setHoodCommand(Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()))) + .withName("Start Shooter Tuning"); + } + + public Command stopShooterTuningCommand() { + return stopFlywheelCommand() + .andThen( + setHoodCommand(Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()))) + .withName("Stop Shooter Tuning"); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineFlywheel.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineFlywheel.dynamic(direction); } } diff --git a/src/main/java/frc/robot/subsystems/vision/CameraConstants.java b/src/main/java/frc/robot/subsystems/vision/CameraConstants.java index 7281955..a8d5b1a 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraConstants.java @@ -1,31 +1,58 @@ package frc.robot.subsystems.vision; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.Constants; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; public class CameraConstants { - private static final AprilTagFieldLayout layout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); + private static final AprilTagFieldLayout layout = Constants.FieldConstants.layout; + // Front Camera public static final String photonCameraName_Front = "FRONT-CAMERA"; - public static final Transform3d photonCameraTransform_Front_STATIC = - new Transform3d(new Translation3d(0.3175, 0.0706, 0.2477), new Rotation3d(0, 0, 0)); - - public static final Transform3d photonCameraTransform_Front_TILTED = + public static final Transform3d photonCameraTransform_Front = new Transform3d( - new Translation3d(0.3124, 0.0706, 0.3543), new Rotation3d(0, Math.toRadians(35), 0)); + new Translation3d(-0.1651, 0, 0.7191), new Rotation3d(0, Math.toRadians(-20), 0)); public static final PhotonCamera photonCamera_Front = new PhotonCamera(photonCameraName_Front); public static final PhotonPoseEstimator photonPoseEstimator_Front = - new PhotonPoseEstimator(layout, photonCameraTransform_Front_STATIC); + new PhotonPoseEstimator( + layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, photonCameraTransform_Front); + + // Left camera + public static final String photonCameraName_Left = "LEFT-CAMERA"; + + public static final Transform3d photonCameraTransform_Left = + new Transform3d( + new Translation3d(-0.2435, 0.3174, 0.3908), + new Rotation3d(0, Math.toRadians(-20), Math.toRadians(120))); + + public static final PhotonCamera photonCamera_Left = new PhotonCamera(photonCameraName_Left); + + public static final PhotonPoseEstimator photonPoseEstimator_Left = + new PhotonPoseEstimator( + layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, photonCameraTransform_Left); + + // Right camera + public static final String photonCameraName_Right = "RIGHT-CAMERA"; + + public static final Transform3d photonCameraTransform_Right = + new Transform3d( + new Translation3d(-0.2435, -0.3175, 0.3908), + new Rotation3d(0, Math.toRadians(-20), Math.toRadians(-120))); + + public static final PhotonCamera photonCamera_Right = new PhotonCamera(photonCameraName_Right); + + public static final PhotonPoseEstimator photonPoseEstimator_Right = + new PhotonPoseEstimator( + layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, photonCameraTransform_Right); public static final double MAXIMUM_ALLOWED_AMBIGUITY = 0.2; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 394c6af..772f1df 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -2,15 +2,18 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.statemachines.DriveState; import java.util.Optional; import org.photonvision.EstimatedRobotPose; import org.photonvision.targeting.PhotonTrackedTarget; +@Logged public class VisionSubsystem extends SubsystemBase { private DriveState driveState = DriveState.getInstance(); @@ -42,6 +45,7 @@ public Vector getTrust() { public VisionSubsystem() {} + @Override public void periodic() { if (driveState.hasDriveStats()) { // this makes sure that the different parts of the periodic use different stats @@ -57,6 +61,26 @@ public void periodic() { evaluateMeasurement(estimatedPose.get(), CameraConstants.photonCameraName_Front); } } + // Left Camera + CameraConstants.photonPoseEstimator_Left.setReferencePose(driveStats.Pose); + var leftCameraResults = CameraConstants.photonCamera_Left.getAllUnreadResults(); + for (var result : leftCameraResults) { + Optional estimatedPose = + CameraConstants.photonPoseEstimator_Left.update(result); + if (!estimatedPose.isEmpty()) { + evaluateMeasurement(estimatedPose.get(), CameraConstants.photonCameraName_Left); + } + } + // Right Camera + CameraConstants.photonPoseEstimator_Right.setReferencePose(driveStats.Pose); + var rightCameraResults = CameraConstants.photonCamera_Right.getAllUnreadResults(); + for (var result : rightCameraResults) { + Optional estimatedPose = + CameraConstants.photonPoseEstimator_Right.update(result); + if (!estimatedPose.isEmpty()) { + evaluateMeasurement(estimatedPose.get(), CameraConstants.photonCameraName_Right); + } + } } } @@ -140,9 +164,12 @@ private void evaluateMeasurement(EstimatedRobotPose pose, String cameraName) { // uploads the pose with its trust values to the drive statemachine // this pose will then be exported to the drivetrain to help navigation // note that we use a singular trust value for both the x and y trust values + driveState.addVisionEstimate( - new VisionMeasurement( - pose, Utils.getCurrentTimeSeconds(), VecBuilder.fill(xyStds, xyStds, thetaStd)), + new VisionMeasurement(pose, Utils.getCurrentTimeSeconds(), VecBuilder.fill(0, 0, 0)), cameraName); + + SmartDashboard.putNumber("Vision/xyStds", xyStds); + SmartDashboard.putNumber("Vision/thetaStds", thetaStd); } }