diff --git a/.Glass/glass.json b/.Glass/glass.json index 3ff3cfe..b3a18fe 100644 --- a/.Glass/glass.json +++ b/.Glass/glass.json @@ -16,9 +16,14 @@ }, "types": { "/FMSInfo": "FMSInfo", + "/Pose": "Field2d", "/SmartDashboard/Alerts": "Alerts", "/SmartDashboard/Autonomous": "String Chooser", - "/SmartDashboard/Field": "Field2d" + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Module 0": "Mechanism2d", + "/SmartDashboard/Module 1": "Mechanism2d", + "/SmartDashboard/Module 2": "Mechanism2d", + "/SmartDashboard/Module 3": "Mechanism2d" }, "windows": { "/SmartDashboard/Field": { diff --git a/src/main/deploy/pathplanner/autos/Blue Bottom 5 L1.auto b/src/main/deploy/pathplanner/autos/Blue Bottom 5 L1.auto new file mode 100644 index 0000000..2a139ae --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Bottom 5 L1.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Bottom to 3R" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 3R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 4L" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 4L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 4L" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 4L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 4R" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 4R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 4R" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue Top 5 L1.auto b/src/main/deploy/pathplanner/autos/Blue Top 5 L1.auto new file mode 100644 index 0000000..7de3f05 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Top 5 L1.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Top to 1L" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 1L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 6R" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 6R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 6R" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 6R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 6L" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue 6L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue HP to 6L" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Bottom 5 L1.auto b/src/main/deploy/pathplanner/autos/Red Bottom 5 L1.auto new file mode 100644 index 0000000..8538e19 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Bottom 5 L1.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Bottom to 3R" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 3R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 4L" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 4L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 4L" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 4L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 4R" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 4R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 4R" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Top 5 L1.auto b/src/main/deploy/pathplanner/autos/Red Top 5 L1.auto new file mode 100644 index 0000000..4cd8d7f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Top 5 L1.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Top to 1L" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 1L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 6R" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 6R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 6R" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 6R to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 6L" + } + }, + { + "type": "path", + "data": { + "pathName": "Red 6L to HP" + } + }, + { + "type": "path", + "data": { + "pathName": "Red HP to 6L" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Square Test.auto b/src/main/deploy/pathplanner/autos/Square Test.auto new file mode 100644 index 0000000..c4d8977 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Square Test.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Square Top" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Right" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Bottom" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Turn Square.auto b/src/main/deploy/pathplanner/autos/Turn Square.auto new file mode 100644 index 0000000..4d0b977 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Turn Square.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Turn Square Top" + } + }, + { + "type": "path", + "data": { + "pathName": "Turn Square Right" + } + }, + { + "type": "path", + "data": { + "pathName": "Turn Square Bottom" + } + }, + { + "type": "path", + "data": { + "pathName": "Turn Square Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue 1L to HP.path b/src/main/deploy/pathplanner/paths/Blue 1L to HP.path new file mode 100644 index 0000000..18a2fdf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue 1L to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.042732566550925, + "y": 5.527328920717593 + }, + "prevControl": null, + "nextControl": { + "x": 4.267959989050372, + "y": 5.993702613520662 + }, + "isLocked": false, + "linkedName": "Blue 1L" + }, + { + "anchor": { + "x": 1.6040711805555545, + "y": 7.401760706018519 + }, + "prevControl": { + "x": 3.062013426843564, + "y": 6.553200071246899 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Top HP" + } + ], + "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": 125.00000000000001 + }, + "reversed": false, + "folder": "Blue To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue 3R to HP.path b/src/main/deploy/pathplanner/paths/Blue 3R to HP.path new file mode 100644 index 0000000..b4c345e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue 3R to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.022817201967592, + "y": 2.4976312934027773 + }, + "prevControl": null, + "nextControl": { + "x": 4.311379425033262, + "y": 2.0899061376404124 + }, + "isLocked": false, + "linkedName": "Blue 3R" + }, + { + "anchor": { + "x": 1.561852936921295, + "y": 0.6489380787037032 + }, + "prevControl": { + "x": 2.960572539040723, + "y": 1.424981171141642 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Bottom HP" + } + ], + "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": -125.00000000000001 + }, + "reversed": false, + "folder": "Blue To HP", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue 4L to HP.path b/src/main/deploy/pathplanner/paths/Blue 4L to HP.path new file mode 100644 index 0000000..3b2620c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue 4L to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.98611183449074, + "y": 2.489537037037037 + }, + "prevControl": null, + "nextControl": { + "x": 3.6409215301274926, + "y": 2.2341548808634064 + }, + "isLocked": false, + "linkedName": "Blue 4L" + }, + { + "anchor": { + "x": 1.561852936921295, + "y": 0.6489380787037032 + }, + "prevControl": { + "x": 2.9209640948833293, + "y": 1.6407545722339187 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Bottom HP" + } + ], + "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": -125.00000000000001 + }, + "reversed": false, + "folder": "Blue To HP", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue 4R to HP.path b/src/main/deploy/pathplanner/paths/Blue 4R to HP.path new file mode 100644 index 0000000..c33129b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue 4R to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4059458188657397, + "y": 2.822507957175925 + }, + "prevControl": null, + "nextControl": { + "x": 2.9759036500385596, + "y": 2.381445794212185 + }, + "isLocked": false, + "linkedName": "Blue 4R" + }, + { + "anchor": { + "x": 1.561852936921295, + "y": 0.6489380787037032 + }, + "prevControl": { + "x": 2.700319942669882, + "y": 1.9510076209318417 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Bottom HP" + } + ], + "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": -125.00000000000001 + }, + "reversed": false, + "folder": "Blue To HP", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue 6L to HP.path b/src/main/deploy/pathplanner/paths/Blue 6L to HP.path new file mode 100644 index 0000000..1a3e975 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue 6L to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4187568721064805, + "y": 5.2345963541666665 + }, + "prevControl": null, + "nextControl": { + "x": 2.547863890709938, + "y": 6.2655048304411185 + }, + "isLocked": false, + "linkedName": "Blue 6L" + }, + { + "anchor": { + "x": 1.6040711805555545, + "y": 7.401760706018519 + }, + "prevControl": { + "x": 2.2916505162662064, + "y": 6.603640515913268 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Top HP" + } + ], + "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": 125.00000000000001 + }, + "reversed": false, + "folder": "Blue To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue 6R to HP.path b/src/main/deploy/pathplanner/paths/Blue 6R to HP.path new file mode 100644 index 0000000..585a8da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue 6R to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9418554687499987, + "y": 5.539674117476852 + }, + "prevControl": null, + "nextControl": { + "x": 3.7036876624486004, + "y": 5.712566932832984 + }, + "isLocked": false, + "linkedName": "Blue 6R" + }, + { + "anchor": { + "x": 1.6040711805555545, + "y": 7.401760706018519 + }, + "prevControl": { + "x": 2.7363334425700843, + "y": 6.47025239618562 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Top HP" + } + ], + "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": 125.00000000000001 + }, + "reversed": false, + "folder": "Blue To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Bottom to 3R.path b/src/main/deploy/pathplanner/paths/Blue Bottom to 3R.path new file mode 100644 index 0000000..a3a27bc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Bottom to 3R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.2642974853515625, + "y": 2.8141222272600444 + }, + "prevControl": null, + "nextControl": { + "x": 5.72481876328577, + "y": 2.5493579269793827 + }, + "isLocked": false, + "linkedName": "Blue Bottom-Mid Start Pose" + }, + { + "anchor": { + "x": 5.022817201967592, + "y": 2.4976312934027773 + }, + "prevControl": { + "x": 6.1734258601779315, + "y": 2.68218709169579 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue 3R" + } + ], + "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": -59.99999999999999 + }, + "reversed": false, + "folder": "Blue To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to 4L.path b/src/main/deploy/pathplanner/paths/Blue HP to 4L.path new file mode 100644 index 0000000..1e1524a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue HP to 4L.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.561852936921295, + "y": 0.6489380787037032 + }, + "prevControl": null, + "nextControl": { + "x": 2.167279365331397, + "y": 1.0652120324572332 + }, + "isLocked": false, + "linkedName": "Blue Bottom HP" + }, + { + "anchor": { + "x": 3.98611183449074, + "y": 2.489537037037037 + }, + "prevControl": { + "x": 3.5418009075776493, + "y": 2.1447405834368856 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue 4L" + } + ], + "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": -119.99999999999999 + }, + "reversed": false, + "folder": "Blue To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to 4R.path b/src/main/deploy/pathplanner/paths/Blue HP to 4R.path new file mode 100644 index 0000000..881696b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue HP to 4R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.561852936921295, + "y": 0.6489380787037032 + }, + "prevControl": null, + "nextControl": { + "x": 2.2102823316325715, + "y": 1.3960116000138763 + }, + "isLocked": false, + "linkedName": "Blue Bottom HP" + }, + { + "anchor": { + "x": 3.4059458188657397, + "y": 2.822507957175925 + }, + "prevControl": { + "x": 2.6607443178004733, + "y": 1.9528764153172546 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue 4R" + } + ], + "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": -119.99999999999999 + }, + "reversed": false, + "folder": "Blue To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to 6L.path b/src/main/deploy/pathplanner/paths/Blue HP to 6L.path new file mode 100644 index 0000000..23c5401 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue HP to 6L.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6040711805555545, + "y": 7.401760706018519 + }, + "prevControl": null, + "nextControl": { + "x": 2.1339861125987833, + "y": 6.755096543633133 + }, + "isLocked": false, + "linkedName": "Blue Top HP" + }, + { + "anchor": { + "x": 3.4187568721064805, + "y": 5.2345963541666665 + }, + "prevControl": { + "x": 3.196769161587942, + "y": 5.50442988743402 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue 6L" + } + ], + "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": 119.99999999999999 + }, + "reversed": false, + "folder": "Blue To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to 6R.path b/src/main/deploy/pathplanner/paths/Blue HP to 6R.path new file mode 100644 index 0000000..cb84452 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue HP to 6R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6040711805555545, + "y": 7.401760706018519 + }, + "prevControl": null, + "nextControl": { + "x": 1.9990162266486933, + "y": 7.076297183774122 + }, + "isLocked": false, + "linkedName": "Blue Top HP" + }, + { + "anchor": { + "x": 3.9418554687499987, + "y": 5.539674117476852 + }, + "prevControl": { + "x": 2.937474418653263, + "y": 6.389285754735428 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue 6R" + } + ], + "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": 119.99999999999999 + }, + "reversed": false, + "folder": "Blue To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Mobility.path b/src/main/deploy/pathplanner/paths/Blue Mobility.path new file mode 100644 index 0000000..b7cae07 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Mobility.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5629490443638385, + "y": 3.993375069754464 + }, + "prevControl": null, + "nextControl": { + "x": 6.675066028686354, + "y": 3.9961657653526537 + }, + "isLocked": false, + "linkedName": "Blue Middle Start Pose" + }, + { + "anchor": { + "x": 6.093646556712962, + "y": 3.993375069754464 + }, + "prevControl": { + "x": 7.02912034697901, + "y": 3.9913335310240536 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Mobility" + } + ], + "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": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Top to 1L.path b/src/main/deploy/pathplanner/paths/Blue Top to 1L.path new file mode 100644 index 0000000..9be6c46 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Top to 1L.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.239354596819196, + "y": 5.176861572265625 + }, + "prevControl": null, + "nextControl": { + "x": 5.751823300124874, + "y": 5.47746994912852 + }, + "isLocked": false, + "linkedName": "Blue Top-Mid Start Pose" + }, + { + "anchor": { + "x": 5.042732566550925, + "y": 5.527328920717593 + }, + "prevControl": { + "x": 6.200919858260081, + "y": 5.336599529825283 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue 1L" + } + ], + "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": 59.99999999999999 + }, + "reversed": false, + "folder": "Blue To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red 1L to HP.path b/src/main/deploy/pathplanner/paths/Red 1L to HP.path new file mode 100644 index 0000000..65640b9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red 1L to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.0341142216435175, + "y": 5.522495659722222 + }, + "prevControl": null, + "nextControl": { + "x": 4.3309541405429055, + "y": 5.948139761821964 + }, + "isLocked": false, + "linkedName": "Red 1L" + }, + { + "anchor": { + "x": 1.579147858796295, + "y": 7.426567563657407 + }, + "prevControl": { + "x": 2.9949493677792995, + "y": 6.599956254373756 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Top HP" + } + ], + "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": 125.00000000000001 + }, + "reversed": false, + "folder": "Red To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red 3R to HP.path b/src/main/deploy/pathplanner/paths/Red 3R to HP.path new file mode 100644 index 0000000..b743818 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red 3R to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.051583839699074, + "y": 2.5170225694444435 + }, + "prevControl": null, + "nextControl": { + "x": 4.171887542292143, + "y": 2.005305242098908 + }, + "isLocked": false, + "linkedName": "Red 3R" + }, + { + "anchor": { + "x": 1.5927741608796284, + "y": 0.6180750868055543 + }, + "prevControl": { + "x": 3.3694664044471967, + "y": 1.5454430159003403 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Bottom HP" + } + ], + "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.0 + }, + "reversed": false, + "folder": "Red To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red 4L to HP.path b/src/main/deploy/pathplanner/paths/Red 4L to HP.path new file mode 100644 index 0000000..c0eb86d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red 4L to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9473292824074067, + "y": 2.5024645543981476 + }, + "prevControl": null, + "nextControl": { + "x": 3.5772188638338123, + "y": 2.237129119463719 + }, + "isLocked": false, + "linkedName": "Red 4L" + }, + { + "anchor": { + "x": 1.5927741608796284, + "y": 0.6180750868055543 + }, + "prevControl": { + "x": 2.9243990179135304, + "y": 1.6526395919250594 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Bottom HP" + } + ], + "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.0 + }, + "reversed": false, + "folder": "Red To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red 4R to HP.path b/src/main/deploy/pathplanner/paths/Red 4R to HP.path new file mode 100644 index 0000000..d4df2d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red 4R to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4535214120370363, + "y": 2.8059700520833326 + }, + "prevControl": null, + "nextControl": { + "x": 2.579085603958592, + "y": 1.7987618991522225 + }, + "isLocked": false, + "linkedName": "Red 4R" + }, + { + "anchor": { + "x": 1.5927741608796284, + "y": 0.6180750868055543 + }, + "prevControl": { + "x": 2.8992483293267526, + "y": 2.116122502756963 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Bottom HP" + } + ], + "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.0 + }, + "reversed": false, + "folder": "Red To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red 6L to HP.path b/src/main/deploy/pathplanner/paths/Red 6L to HP.path new file mode 100644 index 0000000..eea6add --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red 6L to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4234154369212955, + "y": 5.240768952546295 + }, + "prevControl": null, + "nextControl": { + "x": 2.5985259254079405, + "y": 6.231805087253876 + }, + "isLocked": false, + "linkedName": "Red 6L" + }, + { + "anchor": { + "x": 1.579147858796295, + "y": 7.426567563657407 + }, + "prevControl": { + "x": 2.223217786527849, + "y": 6.6540689112299045 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Top HP" + } + ], + "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": 125.00000000000001 + }, + "reversed": false, + "folder": "Red To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red 6R to HP.path b/src/main/deploy/pathplanner/paths/Red 6R to HP.path new file mode 100644 index 0000000..dc779f1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red 6R to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.967826967592592, + "y": 5.545846715856482 + }, + "prevControl": null, + "nextControl": { + "x": 3.6415120056436794, + "y": 5.780623607344038 + }, + "isLocked": false, + "linkedName": "Red 6R" + }, + { + "anchor": { + "x": 1.579147858796295, + "y": 7.426567563657407 + }, + "prevControl": { + "x": 2.644553892338951, + "y": 6.639848505527206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Top HP" + } + ], + "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": 125.00000000000001 + }, + "reversed": false, + "folder": "Red To HP", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Bottom to 3R.path b/src/main/deploy/pathplanner/paths/Red Bottom to 3R.path new file mode 100644 index 0000000..abcd797 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Bottom to 3R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.2642974853515625, + "y": 2.8141222272600444 + }, + "prevControl": null, + "nextControl": { + "x": 5.751429677684496, + "y": 2.5664798105824973 + }, + "isLocked": false, + "linkedName": "Red Bottom-Mid Start Pose" + }, + { + "anchor": { + "x": 5.051583839699074, + "y": 2.5170225694444435 + }, + "prevControl": { + "x": 6.207654457595763, + "y": 2.686702121299903 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red 3R" + } + ], + "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": 119.99999999999999 + }, + "reversed": false, + "folder": "Red To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to 4L.path b/src/main/deploy/pathplanner/paths/Red HP to 4L.path new file mode 100644 index 0000000..ab68a88 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red HP to 4L.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.5927741608796284, + "y": 0.6180750868055543 + }, + "prevControl": null, + "nextControl": { + "x": 2.2747124297565406, + "y": 1.1196552013373302 + }, + "isLocked": false, + "linkedName": "Red Bottom HP" + }, + { + "anchor": { + "x": 3.9473292824074067, + "y": 2.5024645543981476 + }, + "prevControl": { + "x": 3.5573437527249867, + "y": 2.1902826658681516 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red 4L" + } + ], + "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": 59.99999999999999 + }, + "reversed": false, + "folder": "Red To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 55.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to 4R.path b/src/main/deploy/pathplanner/paths/Red HP to 4R.path new file mode 100644 index 0000000..3911527 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red HP to 4R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.5927741608796284, + "y": 0.6180750868055543 + }, + "prevControl": null, + "nextControl": { + "x": 2.224773866503856, + "y": 1.3501259355564548 + }, + "isLocked": false, + "linkedName": "Red Bottom HP" + }, + { + "anchor": { + "x": 3.4535214120370363, + "y": 2.8059700520833326 + }, + "prevControl": { + "x": 2.7587295322511447, + "y": 2.0129860398314054 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red 4R" + } + ], + "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": 59.99999999999999 + }, + "reversed": false, + "folder": "Red To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 55.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to 6L.path b/src/main/deploy/pathplanner/paths/Red HP to 6L.path new file mode 100644 index 0000000..28f730f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red HP to 6L.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.579147858796295, + "y": 7.426567563657407 + }, + "prevControl": null, + "nextControl": { + "x": 1.9925936890022837, + "y": 6.944645275628265 + }, + "isLocked": false, + "linkedName": "Red Top HP" + }, + { + "anchor": { + "x": 3.4234154369212955, + "y": 5.240768952546295 + }, + "prevControl": { + "x": 3.2410423680395146, + "y": 5.449127820521438 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red 6L" + } + ], + "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": 119.99999999999999 + }, + "reversed": false, + "folder": "Red To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to 6R.path b/src/main/deploy/pathplanner/paths/Red HP to 6R.path new file mode 100644 index 0000000..f0f3967 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red HP to 6R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.579147858796295, + "y": 7.426567563657407 + }, + "prevControl": null, + "nextControl": { + "x": 2.1090836923069762, + "y": 7.021536909378482 + }, + "isLocked": false, + "linkedName": "Red Top HP" + }, + { + "anchor": { + "x": 3.967826967592592, + "y": 5.545846715856482 + }, + "prevControl": { + "x": 2.84765794882266, + "y": 6.485314718601215 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red 6R" + } + ], + "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": 119.99999999999999 + }, + "reversed": false, + "folder": "Red To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Mobility.path b/src/main/deploy/pathplanner/paths/Red Mobility.path new file mode 100644 index 0000000..bdda450 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Mobility.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5629490443638385, + "y": 3.993375069754464 + }, + "prevControl": null, + "nextControl": { + "x": 6.957790701729911, + "y": 3.9810311453683034 + }, + "isLocked": false, + "linkedName": "Red Middle Start Pose" + }, + { + "anchor": { + "x": 6.073090639467592, + "y": 3.993375069754464 + }, + "prevControl": { + "x": 5.823092800609803, + "y": 3.9944145727294202 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Mobility" + } + ], + "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": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.4814658058383698 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Top to 1L.path b/src/main/deploy/pathplanner/paths/Red Top to 1L.path new file mode 100644 index 0000000..90e02e9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Top to 1L.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.239354596819196, + "y": 5.176861572265625 + }, + "prevControl": null, + "nextControl": { + "x": 5.735928892279499, + "y": 5.463624474234565 + }, + "isLocked": false, + "linkedName": "Red Top-Mid Start Pose" + }, + { + "anchor": { + "x": 5.0341142216435175, + "y": 5.522495659722222 + }, + "prevControl": { + "x": 6.191101049059445, + "y": 5.325624787974006 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red 1L" + } + ], + "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": 59.99999999999999 + }, + "reversed": false, + "folder": "Red To Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Bottom.path b/src/main/deploy/pathplanner/paths/Square Bottom.path new file mode 100644 index 0000000..b0fed9a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Bottom.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 1.5 + }, + "prevControl": null, + "nextControl": { + "x": 1.7606275446166344, + "y": 1.4736151950091285 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0, + "y": 1.5 + }, + "prevControl": { + "x": 1.6072133624011076, + "y": 1.5087251504874555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Left.path b/src/main/deploy/pathplanner/paths/Square Left.path new file mode 100644 index 0000000..0244e92 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0, + "y": 1.5 + }, + "prevControl": null, + "nextControl": { + "x": 0.9975205604299986, + "y": 2.5283740306821896 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": { + "x": 0.9940944824992028, + "y": 2.2649360823539055 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Right.path b/src/main/deploy/pathplanner/paths/Square Right.path new file mode 100644 index 0000000..3b3f79c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Right.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.500218377241718, + "y": 2.2487583625029384 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 1.5 + }, + "prevControl": { + "x": 2.4925704851960533, + "y": 2.1120412554520858 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Top.path b/src/main/deploy/pathplanner/paths/Square Top.path new file mode 100644 index 0000000..52930a8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Top.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 3.0 + }, + "prevControl": { + "x": 1.5, + "y": 3.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line Test.path b/src/main/deploy/pathplanner/paths/Straight Line Test.path new file mode 100644 index 0000000..8b0b943 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.1898733956473215, + "y": 1.2977578299386163 + }, + "prevControl": null, + "nextControl": { + "x": 3.18987339564732, + "y": 1.2977578299386163 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.363424246651785, + "y": 1.2977578299386163 + }, + "prevControl": { + "x": 3.3634242466517854, + "y": 1.2977578299386163 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.75, + "maxAcceleration": 0.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Turn Square Bottom.path b/src/main/deploy/pathplanner/paths/Turn Square Bottom.path new file mode 100644 index 0000000..768a0f9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Turn Square Bottom.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.2606275446166344, + "y": 4.9736151950091285 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 5.0 + }, + "prevControl": { + "x": 2.6072133624011076, + "y": 5.0087251504874555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Turn Square Left.path b/src/main/deploy/pathplanner/paths/Turn Square Left.path new file mode 100644 index 0000000..635be6f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Turn Square Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.9975205604299986, + "y": 6.0283740306821905 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 1.994094482499203, + "y": 6.264936082353906 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Turn Square Right.path b/src/main/deploy/pathplanner/paths/Turn Square Right.path new file mode 100644 index 0000000..1e9a8c0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Turn Square Right.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 4.000218377241718, + "y": 6.248758362502938 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.9925704851960533, + "y": 5.612041255452086 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Turn Square Top.path similarity index 83% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Turn Square Top.path index 3f475e5..e205dc4 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Turn Square Top.path @@ -17,11 +17,11 @@ { "anchor": { "x": 4.0, - "y": 6.0 + "y": 7.0 }, "prevControl": { "x": 3.0, - "y": 6.0 + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "reversed": false, - "folder": null, + "folder": "Tests", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 347eeb7..f2586f6 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,8 +2,17 @@ "robotWidth": 0.7366, "robotLength": 0.7366, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "Blue To HP", + "Blue To Reef", + "Tests", + "Red To HP", + "Red To Reef" + ], + "autoFolders": [ + "Red Autons", + "Blue Autons" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, @@ -15,7 +24,7 @@ "driveWheelRadius": 0.051, "driveGearing": 6.857, "maxDriveSpeed": 5.45, - "driveMotorType": "vortex", + "driveMotorType": "NEO", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.2804033, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e3fb4be..e954f48 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,20 +1,28 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.AutoPilot; +import com.stuypulse.robot.commands.SeedFieldRelative; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.auton.L1.Bottom5PieceL1; +import com.stuypulse.robot.commands.auton.L1.Top5PieceL1; +import com.stuypulse.robot.commands.auton.tests.Mobility; +import com.stuypulse.robot.commands.auton.tests.SquareTest; +import com.stuypulse.robot.commands.auton.tests.StraightLineTest; import com.stuypulse.robot.commands.dropper.DropperDrop; +import com.stuypulse.robot.commands.dropper.DropperReverse; import com.stuypulse.robot.commands.dropper.DropperShootSequence; import com.stuypulse.robot.commands.dropper.DropperStop; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwervePIDToPose; import com.stuypulse.robot.commands.swerve.SwervePathFind; +import com.stuypulse.robot.commands.swerve.SwervePathFind; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Field.CoralBranch; import com.stuypulse.robot.subsystems.dropper.Dropper; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AprilTagVision; +import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -23,8 +31,6 @@ 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.RepeatCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; public class RobotContainer { @@ -64,18 +70,49 @@ private void configureDefaultCommands() { private void configureButtonBindings() { - // manual shoot + driver.getDPadUp().onTrue(new SeedFieldRelative()); + + // align to closest coral and then shoot automatically driver.getRightTriggerButton() + .whileTrue(new SwervePIDToPose(() -> Field.getClosestBranch().getTargetPose()) + .andThen(new DropperShootSequence())) + .onFalse(new DropperStop()); + + // align to closest coral and then shoot automatically + driver.getRightTriggerButton() + .whileTrue(new SwervePIDToPose(() -> Field.getClosestBranch().getTargetPose()) + .andThen(new DropperShootSequence())) + .onFalse(new DropperStop()); + + // manual shoot + driver.getRightBumper() .onTrue(new DropperDrop()) .onFalse(new DropperStop()); - + + driver.getLeftTriggerButton() + .onTrue(new DropperReverse()) + .onFalse(new DropperStop()); + /* // align to closest coral and then shoot automatically + driver.getRightTriggerButton() + .whileTrue(new SwervePIDToPose(() -> Field.getClosestBranch().getTargetPose()).withTolerance(0.01, 0.01, Settings.Swerve.Alignment.THETA_TOLERANCE.get()) + .andThen(new DropperShootSequence())) + .onFalse(new DropperStop()); +*/ + + // align to nearest algae driver.getRightButton() - .whileTrue(new SwervePIDToPose(() -> Field.getTargetPoseForCoralBranch(Field.getClosestBranch())) - .andThen(new DropperShootSequence())); + .whileTrue(new SwervePIDToPose(() -> Field.getClosestAlgaeBranch().getTargetPose())); + + driver.getDPadLeft() + .whileTrue(new SwervePIDToPose(new Pose2d(1, Field.WIDTH, new Rotation2d()))); driver.getTopButton() .whileTrue(new AutoPilot().repeatedly()); + + driver.getLeftButton().whileTrue(SwervePathFind.toNearestCoralStation().andThen(new SwervePIDToPose(() -> Field.getTargetPoseForCDCoralStation()))); + + driver.getLeftButton().whileTrue(SwervePathFind.toNearestCoralStation().andThen(new SwervePIDToPose(() -> Field.getTargetPoseForCDCoralStation()))); } /**************/ @@ -85,6 +122,41 @@ private void configureButtonBindings() { public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + AutonConfig TOP_5_L1_BLUE = new AutonConfig("Top 5 L1", Top5PieceL1::new, + "Blue Top to 1L", "Blue 1L to HP", "Blue HP to 6R", "Blue 6R to HP", "Blue HP to 6R", "Blue 6R to HP", "Blue HP to 6L", "Blue 6L to HP", "Blue HP to 6L"); + AutonConfig TOP_5_L1_RED = new AutonConfig("Top 5 L1", Top5PieceL1::new, + "Red Top to 1L", "Red 1L to HP", "Red HP to 6R", "Red 6R to HP", "Red HP to 6R", "Red 6R to HP", "Red HP to 6L", "Red 6L to HP", "Red HP to 6L"); + + AutonConfig BOTTOM_5_L1_BLUE = new AutonConfig("Bottom 5 L1", Bottom5PieceL1::new, + "Blue Bottom to 3R", "Blue 3R to HP", "Blue HP to 4L", "Blue 4L to HP", "Blue HP to 4L", "Blue 4L to HP", "Blue HP to 4R", "Blue 4R to HP", "Blue HP to 4R"); + AutonConfig BOTTOM_5_L1_RED = new AutonConfig("Bottom 5 L1", Bottom5PieceL1::new, + "Red Bottom to 3R", "Red 3R to HP", "Red HP to 4L", "Red 4L to HP", "Red HP to 4L", "Red 4L to HP", "Red HP to 4R", "Red 4R to HP", "Red HP to 4R"); + + TOP_5_L1_BLUE.registerDefaultBlue(autonChooser); + TOP_5_L1_RED.registerDefaultRed(autonChooser); + + BOTTOM_5_L1_BLUE.registerBlue(autonChooser); + BOTTOM_5_L1_RED.registerRed(autonChooser); + + // Tests + + AutonConfig SQUARE_TEST = new AutonConfig("Square Test", SquareTest::new, + "Square Top", "Square Right", "Square Bottom", "Square Left"); + AutonConfig TURN_SQUARE_TEST = new AutonConfig("Turn Square Test", SquareTest::new, + "Turn Square Top", "Turn Square Right", "Turn Square Bottom", "Turn Square Left"); + AutonConfig MOBILITY_BLUE = new AutonConfig("Mobility", Mobility::new, + "Blue Mobility"); + AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, + "Red Mobility"); + AutonConfig STRAIGHT_LINE_RED = new AutonConfig("Straight Line Test", StraightLineTest::new, + "Straight Line Test"); + + SQUARE_TEST.registerBlue(autonChooser); + TURN_SQUARE_TEST.registerBlue(autonChooser); + MOBILITY_BLUE.registerBlue(autonChooser); + MOBILITY_RED.registerRed(autonChooser); + STRAIGHT_LINE_RED.registerRed(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/AutoPilot.java b/src/main/java/com/stuypulse/robot/commands/AutoPilot.java index f7fd3ac..ee86557 100644 --- a/src/main/java/com/stuypulse/robot/commands/AutoPilot.java +++ b/src/main/java/com/stuypulse/robot/commands/AutoPilot.java @@ -20,11 +20,11 @@ public AutoPilot() { ArrayList commands = new ArrayList<>(); for (CoralBranch branch : CoralBranch.values()) { Command command = new SequentialCommandGroup( - SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(branch).transformBy(new Transform2d(-1.0, 0, new Rotation2d()))) + SwervePathFind.toPose(branch.getTargetPose().transformBy(new Transform2d(-1.0, 0, new Rotation2d()))) .onlyIf(() -> !branch.onDriverStationSide()), - SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(branch)), + SwervePathFind.toPose(branch.getTargetPose()), new DropperShootSequence(), - SwervePathFind.toPose(Field.getTargetPoseForCoralBranch(branch).transformBy(new Transform2d(-1.0, 0, new Rotation2d()))) + SwervePathFind.toPose(branch.getTargetPose().transformBy(new Transform2d(-1.0, 0, new Rotation2d()))) .onlyIf(() -> !branch.onDriverStationSide()), SwervePathFind.toNearestCoralStation(), new WaitCommand(Settings.Dropper.WAIT_TIME_AT_CORAL_STATION) diff --git a/src/main/java/com/stuypulse/robot/commands/SeedFieldRelative.java b/src/main/java/com/stuypulse/robot/commands/SeedFieldRelative.java new file mode 100644 index 0000000..516cdc1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/SeedFieldRelative.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands; + +import com.stuypulse.robot.subsystems.odometry.Odometry; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SeedFieldRelative extends InstantCommand{ + + private final Odometry odometry; + + public SeedFieldRelative() { + this.odometry = Odometry.getInstance(); + } + + @Override + public void initialize() { + odometry.seedFieldRelative(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/L1/Bottom5PieceL1.java b/src/main/java/com/stuypulse/robot/commands/auton/L1/Bottom5PieceL1.java new file mode 100644 index 0000000..1761c3b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/L1/Bottom5PieceL1.java @@ -0,0 +1,45 @@ +package com.stuypulse.robot.commands.auton.L1; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.dropper.DropperDrop; +import com.stuypulse.robot.commands.dropper.DropperShootSequence; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class Bottom5PieceL1 extends SequentialCommandGroup { + + public Bottom5PieceL1(PathPlannerPath... paths) { + + addCommands( + + // Start in front of Side 3, Drive to + Score on Side 3, Preload + SwerveDrive.getInstance().followPathCommand(paths[0]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 2 + SwerveDrive.getInstance().followPathCommand(paths[1]), + SwerveDrive.getInstance().followPathCommand(paths[2]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 3 + SwerveDrive.getInstance().followPathCommand(paths[3]), + SwerveDrive.getInstance().followPathCommand(paths[4]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 4 + SwerveDrive.getInstance().followPathCommand(paths[5]), + SwerveDrive.getInstance().followPathCommand(paths[6]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 5 + SwerveDrive.getInstance().followPathCommand(paths[7]), + SwerveDrive.getInstance().followPathCommand(paths[8]), + new DropperShootSequence() + + ); + + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/L1/Top5PieceL1.java b/src/main/java/com/stuypulse/robot/commands/auton/L1/Top5PieceL1.java new file mode 100644 index 0000000..5c77a7a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/L1/Top5PieceL1.java @@ -0,0 +1,45 @@ +package com.stuypulse.robot.commands.auton.L1; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.dropper.DropperDrop; +import com.stuypulse.robot.commands.dropper.DropperShootSequence; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class Top5PieceL1 extends SequentialCommandGroup { + + public Top5PieceL1(PathPlannerPath... paths) { + + addCommands( + + // Start in front of Side 3, Drive to + Score on Side 3, Preload + SwerveDrive.getInstance().followPathCommand(paths[0]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 2 + SwerveDrive.getInstance().followPathCommand(paths[1]), + SwerveDrive.getInstance().followPathCommand(paths[2]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 3 + SwerveDrive.getInstance().followPathCommand(paths[3]), + SwerveDrive.getInstance().followPathCommand(paths[4]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 4 + SwerveDrive.getInstance().followPathCommand(paths[5]), + SwerveDrive.getInstance().followPathCommand(paths[6]), + new DropperShootSequence(), + + // To HP to Side 4, Piece 5 + SwerveDrive.getInstance().followPathCommand(paths[7]), + SwerveDrive.getInstance().followPathCommand(paths[8]), + new DropperShootSequence() + + ); + + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/Mobility.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/Mobility.java new file mode 100644 index 0000000..7836101 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/Mobility.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class Mobility extends SequentialCommandGroup { + + public Mobility(PathPlannerPath... path) { + + addCommands( + + // Starts in front of Side 2, drives froward + SwerveDrive.getInstance().followPathCommand(path[0]) + + ); + + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java new file mode 100644 index 0000000..aa91747 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.vision.VisionDisable; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class SquareTest extends SequentialCommandGroup { + + public SquareTest(PathPlannerPath... paths) { + + addCommands( + new VisionDisable(), + + SwerveDrive.getInstance().followPathCommand(paths[0]), + SwerveDrive.getInstance().followPathCommand(paths[1]), + SwerveDrive.getInstance().followPathCommand(paths[2]), + SwerveDrive.getInstance().followPathCommand(paths[3]) + + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLineTest.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLineTest.java new file mode 100644 index 0000000..ce7fb92 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLineTest.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class StraightLineTest extends SequentialCommandGroup { + + public StraightLineTest(PathPlannerPath... paths) { + + addCommands( + + SwerveDrive.getInstance().followPathCommand(paths[0]) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/TurnSquareTest.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/TurnSquareTest.java new file mode 100644 index 0000000..cbb2ab3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/TurnSquareTest.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.vision.VisionDisable; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class TurnSquareTest extends SequentialCommandGroup { + + public TurnSquareTest(PathPlannerPath... paths) { + + addCommands( + new VisionDisable(), + + SwerveDrive.getInstance().followPathCommand(paths[0]), + SwerveDrive.getInstance().followPathCommand(paths[1]), + SwerveDrive.getInstance().followPathCommand(paths[2]), + SwerveDrive.getInstance().followPathCommand(paths[3]) + + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/dropper/DropperDrop.java b/src/main/java/com/stuypulse/robot/commands/dropper/DropperDrop.java index b6f47a0..db67e08 100644 --- a/src/main/java/com/stuypulse/robot/commands/dropper/DropperDrop.java +++ b/src/main/java/com/stuypulse/robot/commands/dropper/DropperDrop.java @@ -11,6 +11,7 @@ public class DropperDrop extends InstantCommand{ public DropperDrop() { this.dropper = Dropper.getInstance(); + addRequirements(dropper); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/dropper/DropperReverse.java b/src/main/java/com/stuypulse/robot/commands/dropper/DropperReverse.java index ee98117..22ea70d 100644 --- a/src/main/java/com/stuypulse/robot/commands/dropper/DropperReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/dropper/DropperReverse.java @@ -11,10 +11,11 @@ public class DropperReverse extends InstantCommand{ public DropperReverse() { this.dropper = Dropper.getInstance(); + addRequirements(dropper); } @Override public void initialize() { - dropper.setState(State.DROPPING); + dropper.setState(State.REVERSING); } } diff --git a/src/main/java/com/stuypulse/robot/commands/dropper/DropperStop.java b/src/main/java/com/stuypulse/robot/commands/dropper/DropperStop.java index b78eb0d..db23e90 100644 --- a/src/main/java/com/stuypulse/robot/commands/dropper/DropperStop.java +++ b/src/main/java/com/stuypulse/robot/commands/dropper/DropperStop.java @@ -11,6 +11,7 @@ public class DropperStop extends InstantCommand{ public DropperStop() { this.dropper = Dropper.getInstance(); + addRequirements(dropper); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 764a02a..c20a2fe 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -1,5 +1,8 @@ package com.stuypulse.robot.commands.swerve; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; @@ -10,10 +13,7 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import com.stuypulse.robot.constants.Settings.Driver.Drive; -import com.stuypulse.robot.constants.Settings.Driver.Turn; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveDrive extends Command { @@ -55,6 +55,18 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { + Vector2D targetVelocity = speed.get(); + double omega = turn.get(); + + ChassisSpeeds currentSpeed = swerve.getChassisSpeeds(); + double currentSpeedMagnitude = Math.hypot(currentSpeed.vxMetersPerSecond, currentSpeed.vyMetersPerSecond); + + // if (targetVelocity.magnitude() > 0.05 || Math.abs(omega) > 0.05 || currentSpeedMagnitude > 0.05 || Math.abs(currentSpeed.omegaRadiansPerSecond) > 0.05) { + // swerve.drive(speed.get(), turn.get()); + // } + // else { + // swerve.setXMode(); + // } swerve.drive(speed.get(), turn.get()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwervePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwervePIDToPose.java index fdbc21c..8800cea 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwervePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwervePIDToPose.java @@ -123,6 +123,10 @@ private boolean isAligned() { @Override public void execute() { targetPose2d.setPose(Robot.isBlue() ? targetPose : Field.transformToOppositeAlliance(targetPose)); + + SmartDashboard.putNumber("Alignment/Target x", targetPose.getX()); + SmartDashboard.putNumber("Alignment/Target y", targetPose.getY()); + SmartDashboard.putNumber("Alignment/Target angle", targetPose.getRotation().getDegrees()); controller.update(targetPose, odometry.getPose()); Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond) @@ -147,6 +151,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { + swerve.stop(); Field.clearFieldObject(targetPose2d); } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index fb77697..da14c6b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -1,12 +1,14 @@ package com.stuypulse.robot.constants; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; /** This interface stores information about each camera. */ public interface Cameras { public CameraInfo[] LimelightCameras = new CameraInfo[] { - new CameraInfo("limelight", new Pose3d()) + new CameraInfo("limelight", new Pose3d(Settings.LENGTH / 2 - Units.inchesToMeters(4), 0, Units.inchesToMeters(16.5), new Rotation3d())) }; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 3f9f8c0..69585de 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -14,7 +14,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import java.util.ArrayList; /** This interface stores information about the field elements. */ public interface Field { @@ -147,18 +146,44 @@ public enum CoralBranch { K, L; - public boolean isLeftPeg() { + public Pose2d getTargetPose() { + Pose3d correspondingAprilTagPose; switch (this) { case A: + case B: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_AB.getLocation() : NamedTags.RED_AB.getLocation(); + break; case C: + case D: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_CD.getLocation() : NamedTags.RED_CD.getLocation(); + break; case E: + case F: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_EF.getLocation() : NamedTags.RED_EF.getLocation(); + break; case G: + case H: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_GH.getLocation() : NamedTags.RED_GH.getLocation(); + break; case I: + case J: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_IJ.getLocation() : NamedTags.RED_IJ.getLocation(); + break; case K: - return true; + case L: default: - return false; + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_KL.getLocation() : NamedTags.RED_KL.getLocation(); } + + return correspondingAprilTagPose.toPose2d() + .transformBy(new Transform2d(Settings.LENGTH/2 + 0.15, CENTER_OF_TROUGH_TO_BRANCH * (this.isLeftPeg() ? -1 : 1), Rotation2d.fromDegrees(180))); + } + + public boolean isLeftPeg() { + return switch (this) { + case A, C, E, G, I, K -> true; + default -> false; + }; } /** @@ -166,65 +191,95 @@ public boolean isLeftPeg() { * The other half is facing the barge / the opposite alliance */ public boolean onDriverStationSide() { - switch (this) { - case A: - case B: - case C: - case D: - case L: - case K: - return true; - default: - return false; - } + return switch (this) { + case A, B, C, D, L, K -> true; + default -> false; + }; } } - public static Pose2d getTargetPoseForCoralBranch(CoralBranch position) { - Pose3d correspondingAprilTagPose; - switch (position) { - case A: - case B: - correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_AB.getLocation() : NamedTags.RED_AB.getLocation(); - break; - case C: - case D: - correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_CD.getLocation() : NamedTags.RED_CD.getLocation(); - break; - case E: - case F: - correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_EF.getLocation() : NamedTags.RED_EF.getLocation(); - break; - case G: - case H: - correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_GH.getLocation() : NamedTags.RED_GH.getLocation(); - break; - case I: - case J: - correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_IJ.getLocation() : NamedTags.RED_IJ.getLocation(); - break; - case K: - case L: - correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_KL.getLocation() : NamedTags.RED_KL.getLocation(); - break; - default: - throw new IllegalArgumentException(); + public static CoralBranch getClosestBranch() { + CoralBranch nearestBranch = CoralBranch.A; + double closestDistance = Double.MAX_VALUE; + + for (CoralBranch branch : CoralBranch.values()) { + double distance = Odometry.getInstance().getPose().minus(branch.getTargetPose()).getTranslation().getNorm(); + if (distance < closestDistance) { + closestDistance = distance; + nearestBranch = branch; + } } - - Pose2d targetPose = correspondingAprilTagPose.toPose2d() - .transformBy(new Transform2d(Settings.LENGTH/2, CENTER_OF_TROUGH_TO_BRANCH * (position.isLeftPeg() ? -1 : 1), Rotation2d.fromDegrees(180))); - - return targetPose; + + return nearestBranch; } public static void setTargetPosesForCoralBranchesToField() { for (CoralBranch branch : CoralBranch.values()) { Field2d field = Odometry.getInstance().getField(); - Pose2d branchTargetPose = getTargetPoseForCoralBranch(branch); - field.getObject("Coral Branch " + branch.toString()).setPose(Robot.isBlue() ? branchTargetPose : transformToOppositeAlliance(branchTargetPose)); + field.getObject("Coral Branch " + branch.toString()).setPose(Robot.isBlue() ? branch.getTargetPose() : transformToOppositeAlliance(branch.getTargetPose())); } } + /*** REEF ALGAE ***/ + + public enum Algae { + AB, + CD, + EF, + GH, + IJ, + KL; + + public boolean isHighAlgae() { + return switch (this) { + case AB, EF, IJ -> true; + default -> false; + }; + } + + public Pose2d getTargetPose() { + Pose3d correspondingAprilTag; + switch (this) { + case AB: + correspondingAprilTag = Robot.isBlue() ? NamedTags.BLUE_AB.getLocation() : NamedTags.RED_AB.getLocation(); + break; + case CD: + correspondingAprilTag = Robot.isBlue() ? NamedTags.BLUE_CD.getLocation() : NamedTags.RED_CD.getLocation(); + break; + case EF: + correspondingAprilTag = Robot.isBlue() ? NamedTags.BLUE_EF.getLocation() : NamedTags.RED_EF.getLocation(); + break; + case GH: + correspondingAprilTag = Robot.isBlue() ? NamedTags.BLUE_GH.getLocation() : NamedTags.RED_GH.getLocation(); + break; + case IJ: + correspondingAprilTag = Robot.isBlue() ? NamedTags.BLUE_IJ.getLocation() : NamedTags.RED_IJ.getLocation(); + break; + case KL: + default: + correspondingAprilTag = Robot.isBlue() ? NamedTags.BLUE_KL.getLocation() : NamedTags.RED_KL.getLocation(); + break; + } + return correspondingAprilTag.toPose2d() + .transformBy(new Transform2d(Settings.LENGTH / 2, 0, Rotation2d.fromDegrees(180))); + } + } + + public static Algae getClosestAlgaeBranch() { + Algae nearestAlgaeBranch = Algae.AB; + double closestDistance = Double.MAX_VALUE; + + for (Algae algaeBranch : Algae.values()) { + double distance = Odometry.getInstance().getPose().minus(algaeBranch.getTargetPose()).getTranslation().getNorm(); + if (distance < closestDistance) { + closestDistance = distance; + nearestAlgaeBranch = algaeBranch; + } + } + + return nearestAlgaeBranch; + } + /*** CORAL STATIONS ***/ public static Pose2d getTargetPoseForCDCoralStation() { return Robot.isBlue() @@ -247,22 +302,6 @@ public static boolean robotIsCloserToCDCoralStation() { return robot.minus(cdCoralStation).getTranslation().getNorm() < robot.minus(klCoralStation).getTranslation().getNorm(); } - public static CoralBranch getClosestBranch() { - CoralBranch nearestBranch = CoralBranch.A; - double closestDistance = Double.MAX_VALUE; - - for (CoralBranch branch : CoralBranch.values()) { - Pose2d target = getTargetPoseForCoralBranch(branch); - double distance = Odometry.getInstance().getPose().minus(target).getTranslation().getNorm(); - if (distance < closestDistance) { - closestDistance = distance; - nearestBranch = branch; - } - } - - return nearestBranch; - } - /**** EMPTY FIELD POSES ****/ Pose2d EMPTY_FIELD_POSE2D = new Pose2d(new Translation2d(-1, -1), new Rotation2d()); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6bfce14..234bfcd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -1,9 +1,6 @@ package com.stuypulse.robot.constants; -import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.path.PathConstraints; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.network.SmartNumber; @@ -22,14 +19,14 @@ public interface Settings { double DT = 1.0 / 50.0; - double WIDTH = Units.inchesToMeters(29); // intake side - double LENGTH = Units.inchesToMeters(29); + double WIDTH = Units.inchesToMeters(36); // intake side + double LENGTH = Units.inchesToMeters(36); public interface Dropper { - boolean inverted = false; + boolean inverted = true; - double DROP_SPEED = 0.5; - double REVERSE_SPEED = 0.5; + double DROP_SPEED = 0.3; + double REVERSE_SPEED = 0.4; double DROP_TIME = 0.75; double WAIT_TIME_AT_CORAL_STATION = 0.75; @@ -39,13 +36,13 @@ public interface Swerve { double WIDTH = Units.inchesToMeters(22.213); // intake side double LENGTH = Units.inchesToMeters(22.213); - double MAX_LINEAR_VELOCITY = 4.9; - double MAX_LINEAR_ACCEL = 15; - double MAX_ANGULAR_VELOCITY = 6.75; // (rad/s) - double MAX_ANGULAR_ACCEL = 200.0; // (rad/s^2) + double MAX_LINEAR_VELOCITY = 3.0; + double MAX_LINEAR_ACCEL = 3.0; + double MAX_ANGULAR_VELOCITY = 3.5; // (rad/s) + double MAX_ANGULAR_ACCEL = 35.0; // (rad/s^2) - double MODULE_VELOCITY_DEADBAND = 0.02; // (m/s) - double MAX_MODULE_SPEED = 5.0; // (m/s) + double MODULE_VELOCITY_DEADBAND = 0.05; // (m/s) + double MAX_MODULE_SPEED = 4.0; // (m/s) PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( @@ -56,23 +53,23 @@ public interface Swerve { public interface Alignment { public interface XY { - SmartNumber kP = new SmartNumber("Swerve/Chassis/PID/XY/kP", 3.0); + SmartNumber kP = new SmartNumber("Swerve/Chassis/PID/XY/kP", 2.0); SmartNumber kI = new SmartNumber("Swerve/Chassis/PID/XY/kI", 0.0); - SmartNumber kD = new SmartNumber("Swerve/Chassis/PID/XY/kD", 0.0); + SmartNumber kD = new SmartNumber("Swerve/Chassis/PID/XY/kD", 0.01); } public interface Theta { - SmartNumber kP = new SmartNumber("Swerve/Chassis/PID/Theta/kP", 3.0); + SmartNumber kP = new SmartNumber("Swerve/Chassis/PID/Theta/kP", 5.0); SmartNumber kI = new SmartNumber("Swerve/Chassis/PID/Theta/kI", 0.0); - SmartNumber kD = new SmartNumber("Swerve/Chassis/PID/Theta/kD", 0.0); + SmartNumber kD = new SmartNumber("Swerve/Chassis/PID/Theta/kD", 0.2); } - SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance (m)", 0.1); - SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance (m)", 0.1); + SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance (m)", 0.05); + SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance (m)", 0.05); SmartNumber THETA_TOLERANCE = new SmartNumber("Alignment/Theta Tolerance (rad)", 0.1); - double XY_DEBOUNCE = 0.1; - double THETA_DEBOUNCE = 0.1; + double XY_DEBOUNCE = 0.05; + double THETA_DEBOUNCE = 0.05; } public interface Turn { @@ -82,13 +79,13 @@ public interface Turn { } public interface Drive { - SmartNumber kP = new SmartNumber("Swerve/Modules/PID/Drive/kP", 1.3); + SmartNumber kP = new SmartNumber("Swerve/Modules/PID/Drive/kP", 0.0); SmartNumber kI = new SmartNumber("Swerve/Modules/PID/Drive/kI", 0.0); - SmartNumber kD = new SmartNumber("Swerve/Modules/PID/Drive/kD", 0.0); + SmartNumber kD = new SmartNumber("Swerve/Modules/PID/Drive/kD", 0.01); - SmartNumber kS = new SmartNumber("Swerve/Modules/PID/Drive/kS", 0.17335); - SmartNumber kV = new SmartNumber("Swerve/Modules/PID/Drive/kV", 2.7274); - SmartNumber kA = new SmartNumber("Swerve/Modules/PID/Drive/kA", 0.456); + SmartNumber kS = new SmartNumber("Swerve/Modules/PID/Drive/kS", 0.26722); + SmartNumber kV = new SmartNumber("Swerve/Modules/PID/Drive/kV", 2.2119); + SmartNumber kA = new SmartNumber("Swerve/Modules/PID/Drive/kA", 0.36249); } public interface Encoder { @@ -119,25 +116,25 @@ public interface Turn { public interface FrontLeft { boolean DRIVE_INVERTED = false; boolean TURN_INVERTED = false; - Angle ANGLE_OFFSET = Angle.fromDegrees(0.0); + Angle ANGLE_OFFSET = Angle.fromDegrees(24.785156); Translation2d XY_OFFSET = new Translation2d(LENGTH * +0.5, WIDTH * +0.5); } public interface FrontRight { boolean DRIVE_INVERTED = false; boolean TURN_INVERTED = false; - Angle ANGLE_OFFSET = Angle.fromDegrees(0.0); + Angle ANGLE_OFFSET = Angle.fromDegrees(-85.913086); Translation2d XY_OFFSET = new Translation2d(LENGTH * +0.5, WIDTH * -0.5); } public interface BackLeft { boolean DRIVE_INVERTED = false; boolean TURN_INVERTED = false; - Angle ANGLE_OFFSET = Angle.fromDegrees(0.0); + Angle ANGLE_OFFSET = Angle.fromDegrees(26.762695); Translation2d XY_OFFSET = new Translation2d(LENGTH * -0.5, WIDTH * +0.5); } public interface BackRight { boolean DRIVE_INVERTED = false; boolean TURN_INVERTED = false; - Angle ANGLE_OFFSET = Angle.fromDegrees(0.0); + Angle ANGLE_OFFSET = Angle.fromDegrees(-23.686523); Translation2d XY_OFFSET = new Translation2d(LENGTH * -0.5, WIDTH * -0.5); } } @@ -148,7 +145,7 @@ public interface Vision { public interface Driver { public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.05); SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); @@ -158,7 +155,7 @@ public interface Drive { } public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.03); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); diff --git a/src/main/java/com/stuypulse/robot/subsystems/dropper/DropperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/dropper/DropperImpl.java index 9fc20a5..1463695 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/dropper/DropperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/dropper/DropperImpl.java @@ -3,12 +3,12 @@ import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.SparkMax; -import com.stuypulse.robot.constants.Settings; +import com.revrobotics.spark.config.SparkMaxConfig; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; public class DropperImpl extends Dropper { @@ -28,6 +28,7 @@ private void setMotorBasedOnState() { break; case REVERSING: motor.set(-Settings.Dropper.REVERSE_SPEED); + break; case STOP: motor.stopMotor(); break; diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index c5e787e..c26e455 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -25,6 +25,7 @@ public static Odometry getInstance(){ public abstract Pose2d getPose(); + public final Translation2d getTranslation() { return getPose().getTranslation(); } @@ -33,5 +34,10 @@ public final Rotation2d getRotation() { return getPose().getRotation(); } + public void seedFieldRelative() { + Pose2d newPose = new Pose2d(getPose().getX(), getPose().getY(), new Rotation2d()); + reset(newPose); + } + public abstract void addVisionData(Pose2d robotPose, double timestamp); } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index e462df6..4dce836 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.kauailabs.navx.frc.AHRS; @@ -38,6 +39,7 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathPlannerPath; public class SwerveDrive extends SubsystemBase { @@ -109,6 +111,10 @@ public void configureAutoBuilder() { } } + public Command followPathCommand(PathPlannerPath path) { + return AutoBuilder.followPath(path); + } + public void initFieldObjects(Field2d field) { for (int i = 0; i < modules.length; i++) { module2ds[i] = field.getObject(modules[i].getName()+"-2d"); @@ -226,10 +232,10 @@ public SwerveDriveKinematics getKinematics() { public void setXMode() { SwerveModuleState[] state = { + new SwerveModuleState(0, Rotation2d.fromDegrees(45)), new SwerveModuleState(0, Rotation2d.fromDegrees(135)), new SwerveModuleState(0, Rotation2d.fromDegrees(225)), new SwerveModuleState(0, Rotation2d.fromDegrees(315)), - new SwerveModuleState(0, Rotation2d.fromDegrees(45)), }; setModuleStates(state); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SacrodModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SacrodModule.java index 0bae5b5..4fe0429 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SacrodModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SacrodModule.java @@ -90,6 +90,7 @@ public Translation2d getOffset() { @Override public void setTargetState(SwerveModuleState state) { state.optimize(getRotation2d()); + state.cosineScale(getRotation2d()); targetState = state; } @@ -106,7 +107,8 @@ private Rotation2d getAbsolutePosition() { } private Rotation2d getRotation2d() { - return getAbsolutePosition().minus(angleOffset.getRotation2d()); + // not sure why we have to multiply this by 2 + return new Rotation2d(getAbsolutePosition().minus(angleOffset.getRotation2d()).getRadians() * 2); } @Override @@ -124,6 +126,7 @@ public void periodic() { turnMotor.setVoltage(turnController.update( Angle.fromRotation2d(targetState.angle), Angle.fromRotation2d(getRotation2d()))); + driveMotor.setVoltage(driveController.update(targetState.speedMetersPerSecond, getSpeed())); SmartDashboard.putNumber("Swerve/" + name + "/Target Angle", targetState.angle.getDegrees()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 8f3eaf4..7a31be6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -76,8 +76,8 @@ public void periodic() { PoseEstimate poseEstimate = Robot.isBlue() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName) : LimelightHelpers.getBotPoseEstimate_wpiRed(limelightName); - - if (poseEstimate.tagCount > 0) { + + if (poseEstimate != null && poseEstimate.tagCount > 0) { Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; Odometry.getInstance().addVisionData(robotPose, timestamp); diff --git a/src/main/java/com/stuypulse/robot/util/PathUtil.java b/src/main/java/com/stuypulse/robot/util/PathUtil.java new file mode 100644 index 0000000..4bfd8a9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/PathUtil.java @@ -0,0 +1,167 @@ +package com.stuypulse.robot.util; + +import java.io.IOException; +import java.nio.file.DirectoryStream; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.Collections; +import java.util.HashMap; +import java.util.List; +import java.util.function.Function; +import java.util.stream.Collectors; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.path.RotationTarget; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; + +public class PathUtil { + public static class AutonConfig { + + private final String name; + private final Function auton; + private final String[] paths; + + public AutonConfig(String name, Function auton, String... paths) { + this.name = name; + this.auton = auton; + this.paths = paths; + + for (String path : paths) { + try { + PathPlannerPath.fromPathFile(path); + } catch (Exception e) { + DriverStation.reportError("Path \"" + path + "\" not found. Did you mean \"" + PathUtil.findClosestMatch(PathUtil.getPathFileNames(), path) + "\"?", false); + + } + } + } + + public AutonConfig registerBlue(SendableChooser chooser) { + chooser.addOption("Blue " + name, auton.apply(loadPaths(paths))); + return this; + } + + public AutonConfig registerRed(SendableChooser chooser) { + chooser.addOption("Red " + name, auton.apply(loadPaths(paths))); + return this; + } + + public AutonConfig registerDefaultBlue(SendableChooser chooser) { + chooser.setDefaultOption("Blue " + name, auton.apply(loadPaths(paths))); + return this; + } + + public AutonConfig registerDefaultRed(SendableChooser chooser) { + chooser.setDefaultOption("Red " + name, auton.apply(loadPaths(paths))); + return this; + } + } + + /*** PATH LOADING ***/ + + public static PathPlannerPath[] loadPaths(String... names) { + PathPlannerPath[] output = new PathPlannerPath[names.length]; + for (int i = 0; i < names.length; i++) { + output[i] = load(names[i]); + } + return output; + } + + public static PathPlannerPath load(String name) { + try { + return PathPlannerPath.fromPathFile(name); + + } catch (Exception e) { + + DriverStation.reportError("Path not found.", false); + return null; + } + } + + /*** PATH FILENAME CORRECTION ***/ + + public static List getPathFileNames() { + // ../../../../../deploy/pathplanner/paths + + Path path = Paths.get("").toAbsolutePath().resolve("src/main/deploy/pathplanner/paths"); + ArrayList fileList = new ArrayList(); + try (DirectoryStream stream = Files.newDirectoryStream(path, "*.path")) { + for (Path file : stream) { + fileList.add(file.getFileName().toString().replaceFirst(".path","")); + } + } catch (IOException error) { + DriverStation.reportError(error.getMessage(), false); + } + Collections.sort(fileList); + return fileList; + } + + public static String findClosestMatch(List paths, String input) { + double closestValue = 10.0; + String matching = ""; + + for (String fileName : paths){ + HashMap fileChars = countChars(fileName.toCharArray()); + HashMap inputChars = countChars(input.toCharArray()); + + double proximity = compareNameProximity(fileChars, inputChars); + closestValue = Math.min(proximity, closestValue); + + if (proximity == closestValue) { + matching = fileName; + } + } + + return matching; + } + + public static HashMap countChars(char[] chars) { + HashMap letterMap = new HashMap<>(); + for (char i = 'a'; i <= 'z'; i++) letterMap.put(i, 0); + for (char i = 'a'; i <= 'z'; i++) letterMap.put(i, 0); + letterMap.put('(', 0); + letterMap.put(' ', 0); + letterMap.put(')', 0); + for (char letter : chars) { + if (letterMap.containsKey(letter)) { + letterMap.put(letter, letterMap.get(letter)); + } else { + letterMap.put(letter, 1); + } + } + return letterMap; + } + + public static double compareNameProximity(HashMap list1, HashMap list2) { + double proximity = 0.0; + int list1sum = 0, list2sum = 0; + for (char key : list1.keySet()) { + if (!list2.containsKey(key)) { + proximity += 0.1; + continue; + } + proximity += 0.05 * Math.abs(list1.get(key) - list2.get(key)); + } + for (char key : list2.keySet()) { + if (!list1.containsKey(key)) { + proximity += 0.1; + continue; + } + proximity += 0.05 * Math.abs(list1.get(key) - list2.get(key)); + } + for (int count : list1.values()) list1sum += count; + for (int count : list2.values()) list2sum += count; + proximity += 0.4 * Math.abs(list2sum - list1sum); + return proximity; + } +} \ No newline at end of file