Compare commits
42 Commits
a089dddae3
...
faster_ele
| Author | SHA1 | Date | |
|---|---|---|---|
| ba7e8d59ad | |||
|
626b92b769 | ||
|
|
42d47d6075 | ||
|
|
68da3c630c | ||
| c9316cebc3 | |||
| d312e125cd | |||
| 4386de4d4d | |||
| cca7d68766 | |||
| a8a597985f | |||
| 060b39669f | |||
|
|
4ada896603 | ||
|
|
dd26ff6de4 | ||
|
|
0589463c4e | ||
| 339bf642a1 | |||
| c75554dfc5 | |||
| e98b3a585e | |||
| 83db16794f | |||
| 3dcbac25cc | |||
| a391cc7910 | |||
| a19285cb0b | |||
| f6c2a82779 | |||
|
|
dead4a7289 | ||
|
|
9b7d2b45a4 | ||
| 11512e140c | |||
| c7071d409b | |||
|
|
23e2ad5a9b | ||
|
|
d693faf5c9 | ||
| c9f6928806 | |||
| fdf837ab10 | |||
| be961d286b | |||
| 073b2ab754 | |||
| 8cc14b4cc3 | |||
| c4134ef713 | |||
|
|
17143ffffb | ||
| 3e6fa986e7 | |||
| 50f402f36f | |||
| 51ef5ff2d0 | |||
| 945747778b | |||
| d85683377b | |||
| 00ecedf216 | |||
| dd50663b9e | |||
| 66a9608006 |
@@ -8,9 +8,18 @@
|
||||
"NetworkTables Settings": {
|
||||
"mode": "Client (NT4)"
|
||||
},
|
||||
"client@1": {
|
||||
"Publishers": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"client@2": {
|
||||
"open": true
|
||||
},
|
||||
"client@3": {
|
||||
"open": true
|
||||
},
|
||||
"client@4": {
|
||||
"Publishers": {
|
||||
"open": true
|
||||
|
||||
@@ -4,6 +4,12 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
@@ -21,6 +27,12 @@
|
||||
"data": {
|
||||
"pathName": "H Backup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
74
src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto
Normal file
74
src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto
Normal file
@@ -0,0 +1,74 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Start to H"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "H Backup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Pickup Algae L2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HG Algae"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HG to Barge"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Algae"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Post-Barge Backup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
81
src/main/deploy/pathplanner/autos/2.5 Coral Left.auto
Normal file
81
src/main/deploy/pathplanner/autos/2.5 Coral Left.auto
Normal file
@@ -0,0 +1,81 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Start to 30 Right"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "30 Right to HP"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Collect Coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HP to 330 Right"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "L Backup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -10,18 +10,6 @@
|
||||
"pathName": "Start to 30 Right"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "J Approach"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
@@ -56,19 +44,44 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HP to 330 Left"
|
||||
"pathName": "HP to 330 Right"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "L Backup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Collect Coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "K Approach"
|
||||
"pathName": "HP to K"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "fein"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -8,20 +8,20 @@
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.649657534252619,
|
||||
"y": 6.474186643842743
|
||||
"x": 4.843032786885245,
|
||||
"y": 6.30266393442623
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.174795081967213,
|
||||
"y": 7.321618852459016
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.031259123063103,
|
||||
"y": 6.375000701774084
|
||||
"x": 2.287270519874242,
|
||||
"y": 6.774371912194027
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,7 +33,7 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
@@ -45,10 +45,10 @@
|
||||
"rotation": -53.97262661489646
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -120.06858282186238
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -45,8 +45,8 @@
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 3.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -57,10 +57,10 @@
|
||||
"rotation": -53.98486432191523
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.99999999999999
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
73
src/main/deploy/pathplanner/paths/E to HP.path
Normal file
73
src/main/deploy/pathplanner/paths/E to HP.path
Normal file
@@ -0,0 +1,73 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.958476027397261,
|
||||
"y": 2.837970890410959
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.619606164383562,
|
||||
"y": 1.455607876712329
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "E"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1569777397260272,
|
||||
"y": 1.0198630136986298
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.7647260273972605,
|
||||
"y": 1.485659246575342
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Right HP"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "HP Pickup",
|
||||
"waypointRelativePos": 0.22857142857142831,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 54.162347045721745
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 120.25643716352937
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.7,
|
||||
"y": 4.3
|
||||
"x": 5.758260140458621,
|
||||
"y": 4.193633481772718
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.053790983606557,
|
||||
"y": 4.312704918032787
|
||||
"x": 6.112051124065178,
|
||||
"y": 4.206338399805505
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "H"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.389446721311475,
|
||||
"y": 4.3
|
||||
"x": 6.5,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.1394487099079695,
|
||||
"y": 4.300997143065429
|
||||
"x": 6.250001988596495,
|
||||
"y": 4.022982203795822
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "Behind HG Face"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 0.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -45,10 +45,10 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.289041095890411,
|
||||
"y": 2.9732020547945206
|
||||
"x": 6.5,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.950171232876712,
|
||||
"y": 1.6208904109589037
|
||||
"x": 6.224227344787433,
|
||||
"y": 4.009974315068493
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "Behind HG Face"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.4424657534246574,
|
||||
"y": 0.7944777397260271
|
||||
"x": 5.872671261177789,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.389083904109589,
|
||||
"y": 2.5374571917808213
|
||||
"x": 6.125859753477119,
|
||||
"y": 4.004837296105254
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "HG Algae"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 54.162347045721745
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 120.96375653207336
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
71
src/main/deploy/pathplanner/paths/HG to Barge.path
Normal file
71
src/main/deploy/pathplanner/paths/HG to Barge.path
Normal file
@@ -0,0 +1,71 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.872671261177789,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.829105562465259,
|
||||
"y": 3.8676166530198106
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HG Algae"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.032020547945206,
|
||||
"y": 4.761258561643835
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.262336691900294,
|
||||
"y": 4.359747737103425
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Pre-Barge"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.17234468937875755,
|
||||
"rotationDegrees": 180.0
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Processor Position",
|
||||
"waypointRelativePos": 0.188095238095238,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Processor Position"
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 300.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
66
src/main/deploy/pathplanner/paths/HP to 330 Right.path
Normal file
66
src/main/deploy/pathplanner/paths/HP to 330 Right.path
Normal file
@@ -0,0 +1,66 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.0019467213114757,
|
||||
"y": 6.434528688524591
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Left Position"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.609900518622585,
|
||||
"y": 5.005924534374863
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.2943611350609414,
|
||||
"y": 5.531823506977602
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "L"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.0,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.25,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.69923999693802
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -53.97262661489646
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
73
src/main/deploy/pathplanner/paths/HP to D.path
Normal file
73
src/main/deploy/pathplanner/paths/HP to D.path
Normal file
@@ -0,0 +1,73 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.187029109589041,
|
||||
"y": 0.9747859589041092
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.268878424657535,
|
||||
"y": 1.0949914383561645
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.011857876712329,
|
||||
"y": 2.837970890410959
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.3657534246575342,
|
||||
"y": 1.9364297945205475
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.27142857142857096,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 58.57043438516136
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 54.24611274556325
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
66
src/main/deploy/pathplanner/paths/HP to K.path
Normal file
66
src/main/deploy/pathplanner/paths/HP to K.path
Normal file
@@ -0,0 +1,66 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.9266540815180775,
|
||||
"y": 6.744320542331013
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Left Position"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.9679303278688525,
|
||||
"y": 5.211782786885246
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.975085616438356,
|
||||
"y": 6.023416095890411
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.0,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 1.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -60.49491285058726
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -53.97262661489646
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -17,11 +17,11 @@
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.974897540983607,
|
||||
"y": 5.115881147540984
|
||||
"y": 5.235758196721312
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.058811475409836,
|
||||
"y": 5.403586065573771
|
||||
"y": 5.523463114754099
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@@ -4,12 +4,12 @@
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.974897540983607,
|
||||
"y": 5.115881147540984
|
||||
"y": 5.235758196721312
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.11855393929902,
|
||||
"y": 5.383513333352196
|
||||
"y": 5.503390382532524
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "J"
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6202868852459016,
|
||||
"y": 5.859118852459017
|
||||
"x": 3.35655737704918,
|
||||
"y": 5.78719262295082
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.7970635805425386,
|
||||
"y": 5.68234215716238
|
||||
"x": 3.402629510813402,
|
||||
"y": 5.541474576217554
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Before K"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.8720286885245896,
|
||||
"y": 5.043954918032787
|
||||
"x": 3.6322745901639335,
|
||||
"y": 5.019979508196721
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.836065573770491,
|
||||
"y": 5.295696721311475
|
||||
"x": 3.596311475409835,
|
||||
"y": 5.271721311475409
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 0.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -50,5 +50,5 @@
|
||||
"velocity": 0,
|
||||
"rotation": -59.69923999693802
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.174795081967213,
|
||||
"y": 7.321618852459016
|
||||
"x": 3.609900518622585,
|
||||
"y": 5.005924534374863
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.4670039860768025,
|
||||
"y": 7.096233578486413
|
||||
"x": 3.47740078362179,
|
||||
"y": 5.217924110376135
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Left Position"
|
||||
"linkedName": "L"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6202868852459016,
|
||||
"y": 5.859118852459017
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.304747501684258,
|
||||
"y": 6.385017825061756
|
||||
"x": 1.3306352459016395,
|
||||
"y": 6.926024590163935
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Before K"
|
||||
"linkedName": "HP Left Position"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -33,22 +33,22 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 3.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.69923999693802
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -53.97262661489646
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"reversed": false,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.69923999693802
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -16,12 +16,12 @@
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 12.272,
|
||||
"y": 2.975
|
||||
"x": 3.756421232876712,
|
||||
"y": 5.227054794520548
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 10.940715672291898,
|
||||
"y": 2.975
|
||||
"x": 2.756421232876712,
|
||||
"y": 5.227054794520548
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -30,22 +30,11 @@
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [
|
||||
{
|
||||
"fieldPosition": {
|
||||
"x": 0.4,
|
||||
"y": 5.5
|
||||
},
|
||||
"rotationOffset": 0.0,
|
||||
"minWaypointRelativePos": 0.15,
|
||||
"maxWaypointRelativePos": 0.4,
|
||||
"name": "Point Towards Zone"
|
||||
}
|
||||
],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -53,10 +42,10 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 59.99999999999999
|
||||
"rotation": -59.18537788806707
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
|
||||
54
src/main/deploy/pathplanner/paths/Post-Barge Backup.path
Normal file
54
src/main/deploy/pathplanner/paths/Post-Barge Backup.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.032020547945206,
|
||||
"y": 4.761258561643835
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.71311475409836,
|
||||
"y": 4.9480532786885245
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Pre-Barge"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.933913934426228,
|
||||
"y": 5.247745901639345
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.641188524590164,
|
||||
"y": 5.043954918032786
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.602996575342465,
|
||||
"y": 2.0115582191780823
|
||||
"x": 7.572945205479453,
|
||||
"y": 0.4939640410958907
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.491095890410959,
|
||||
"y": 1.9965325342465756
|
||||
"x": 6.461044520547946,
|
||||
"y": 0.4789383561643841
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.289041095890411,
|
||||
"y": 2.9882277397260277
|
||||
"x": 4.958476027397261,
|
||||
"y": 2.837970890410959
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.130479452054795,
|
||||
"y": 2.11673801369863
|
||||
"x": 5.799914383561644,
|
||||
"y": 1.9664811643835614
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "E"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -34,14 +34,26 @@
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.7142857142857124,
|
||||
"waypointRelativePos": 0.4261904761904757,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
"command": {
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -3,38 +3,50 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.588217213114754,
|
||||
"y": 7.537397540983607
|
||||
"x": 7.150967037968244,
|
||||
"y": 7.5521014571037055
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.3534836065573765,
|
||||
"y": 6.51844262295082
|
||||
"x": 5.916233431410867,
|
||||
"y": 6.533146539070919
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.286577868852459,
|
||||
"y": 5.955020491803278
|
||||
"x": 4.974897540983607,
|
||||
"y": 5.235758196721312
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.872579581181226,
|
||||
"y": 6.826510217830675
|
||||
"x": 5.560899253312374,
|
||||
"y": 6.107247922748709
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Before J"
|
||||
"linkedName": "J"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.25,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -45,10 +57,10 @@
|
||||
"rotation": -119.71497744813712
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -16,12 +16,12 @@
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.7,
|
||||
"y": 4.3
|
||||
"x": 5.758260140458621,
|
||||
"y": 4.193633481772718
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.347336065573771,
|
||||
"y": 4.2640368852459005
|
||||
"x": 6.405596206032391,
|
||||
"y": 4.157670367018619
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -34,14 +34,19 @@
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.08214624881291864,
|
||||
"waypointRelativePos": 0.10238095238095252,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
"command": {
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -52,10 +57,10 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.600204918039607,
|
||||
"y": 6.374590163938041
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.600204918032786,
|
||||
"y": 7.573360655737705
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.209631147540984,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.120696721311476,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 4.745363537952068,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.8670081967213115,
|
||||
"y": 6.973975409836065
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.826024590163935,
|
||||
"y": 6.9979508196721305
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -2,10 +2,13 @@
|
||||
"robotWidth": 0.8763,
|
||||
"robotLength": 0.8763,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [],
|
||||
"pathFolders": [
|
||||
"Left Paths",
|
||||
"Center"
|
||||
],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 4.0,
|
||||
"defaultMaxAccel": 2.5,
|
||||
"defaultMaxVel": 3.5,
|
||||
"defaultMaxAccel": 1.75,
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 400.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
|
||||
@@ -7,11 +7,9 @@ package frc.robot;
|
||||
import frc.robot.constants.ManipulatorPivotConstants;
|
||||
import frc.robot.constants.ClimberPivotConstants;
|
||||
import frc.robot.constants.ElevatorConstants;
|
||||
import frc.robot.constants.ManipulatorConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
import frc.robot.subsystems.ManipulatorPivot;
|
||||
import frc.robot.subsystems.Vision;
|
||||
import frc.robot.subsystems.ClimberPivot;
|
||||
import frc.robot.subsystems.ClimberRollers;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
@@ -22,9 +20,9 @@ import java.util.function.IntSupplier;
|
||||
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
import com.pathplanner.lib.events.EventTrigger;
|
||||
import com.pathplanner.lib.path.EventMarker;
|
||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
@@ -44,6 +42,8 @@ public class RobotContainer {
|
||||
private Elevator elevator;
|
||||
//private ElevatorSysID elevator;
|
||||
|
||||
//private Indexer indexer;
|
||||
|
||||
private Manipulator manipulator;
|
||||
|
||||
private ManipulatorPivot manipulatorPivot;
|
||||
@@ -65,6 +65,8 @@ public class RobotContainer {
|
||||
elevator = new Elevator();
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
//indexer = new Indexer();
|
||||
|
||||
manipulator = new Manipulator();
|
||||
|
||||
manipulatorPivot = new ManipulatorPivot();
|
||||
@@ -75,6 +77,9 @@ public class RobotContainer {
|
||||
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
||||
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true));
|
||||
autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true));
|
||||
autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true));
|
||||
|
||||
closestTag = drivetrain::getClosestTag;
|
||||
|
||||
@@ -106,9 +111,9 @@ public class RobotContainer {
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
|
||||
() -> Math.pow(driver.getLeftY(), 3),
|
||||
() -> Math.pow(driver.getLeftX(), 3),
|
||||
() -> driver.getRightX(),
|
||||
() -> true
|
||||
)
|
||||
);
|
||||
@@ -116,6 +121,10 @@ public class RobotContainer {
|
||||
elevator.setDefaultCommand(
|
||||
elevator.maintainPosition()
|
||||
);
|
||||
|
||||
//indexer.setDefaultCommand(
|
||||
// indexer.runIndexer(() -> 0)
|
||||
//);
|
||||
|
||||
|
||||
manipulatorPivot.setDefaultCommand(
|
||||
@@ -124,9 +133,7 @@ public class RobotContainer {
|
||||
|
||||
|
||||
manipulator.setDefaultCommand(
|
||||
manipulator.runUntilCollected(
|
||||
() -> 0.0
|
||||
)
|
||||
manipulator.runManipulator(() -> 0.0, false)
|
||||
);
|
||||
|
||||
//Driver inputs
|
||||
@@ -141,6 +148,9 @@ public class RobotContainer {
|
||||
|
||||
driver.leftTrigger().whileTrue(
|
||||
manipulator.runUntilCollected(() -> 0.75)
|
||||
//.alongWith(indexer.runIndexer(() -> .75))
|
||||
.until(() -> manipulator.getCoralBeamBreak() == false)
|
||||
.andThen(manipulator.retractCommand(() -> .1))
|
||||
);
|
||||
|
||||
driver.start().and(driver.back()).onTrue(
|
||||
@@ -152,23 +162,36 @@ public class RobotContainer {
|
||||
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
|
||||
|
||||
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
|
||||
|
||||
driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
|
||||
|
||||
driver.start().whileTrue(drivetrain.resetToVision());
|
||||
|
||||
driver.povUp().whileTrue(
|
||||
drivetrain.resetToVision().andThen(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][0],
|
||||
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][1],
|
||||
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
|
||||
))
|
||||
);
|
||||
|
||||
driver.rightBumper().whileTrue(
|
||||
drivetrain.resetToVision().andThen(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
|
||||
() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
|
||||
)
|
||||
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
|
||||
))
|
||||
);
|
||||
|
||||
driver.leftBumper().whileTrue(
|
||||
drivetrain.resetToVision().andThen(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
|
||||
() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
|
||||
)
|
||||
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
|
||||
))
|
||||
);
|
||||
|
||||
//Operator inputs
|
||||
@@ -202,10 +225,12 @@ public class RobotContainer {
|
||||
|
||||
operator.back().onTrue(elevator.homeCommand());
|
||||
|
||||
operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
|
||||
operator.start().toggleOnTrue(
|
||||
climberPivot.runPivot(() -> -operator.getRightY())
|
||||
.alongWith(climberRollers.runRoller(() -> operator.getLeftY())));
|
||||
|
||||
operator.a().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
|
||||
safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
|
||||
);
|
||||
|
||||
operator.x().onTrue(
|
||||
@@ -225,16 +250,129 @@ public class RobotContainer {
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
);
|
||||
|
||||
operator.rightTrigger().onTrue(shootAlgae());
|
||||
|
||||
}
|
||||
private void configureNamedCommands() {
|
||||
new EventTrigger("Lift L4").whileTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
//new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
|
||||
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
||||
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
|
||||
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
|
||||
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
|
||||
NamedCommands.registerCommand(
|
||||
"Drivetrain Set X",
|
||||
drivetrain.setXCommand()
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Shoot Coral L4",
|
||||
Commands.race(
|
||||
manipulator.runManipulator(
|
||||
() -> 0.4,
|
||||
true
|
||||
).withTimeout(
|
||||
0.5
|
||||
).andThen(
|
||||
manipulator.runManipulator(
|
||||
() -> 0.0,
|
||||
false
|
||||
).withTimeout(
|
||||
0.01
|
||||
)
|
||||
),
|
||||
Commands.parallel(
|
||||
elevator.maintainPosition(),
|
||||
manipulatorPivot.maintainPosition()
|
||||
)
|
||||
)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Shoot Coral L4 Fast",
|
||||
Commands.race(
|
||||
manipulator.runManipulator(
|
||||
() -> 1,
|
||||
true
|
||||
).andThen(
|
||||
manipulator.runManipulator(
|
||||
() -> 1,
|
||||
true
|
||||
).withTimeout(
|
||||
0.125
|
||||
)
|
||||
).withTimeout(
|
||||
3
|
||||
).andThen(
|
||||
manipulator.runManipulator(
|
||||
() -> 0,
|
||||
true
|
||||
)
|
||||
),
|
||||
Commands.parallel(
|
||||
elevator.maintainPosition(),
|
||||
manipulatorPivot.maintainPosition()
|
||||
)
|
||||
)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Collect Coral",
|
||||
manipulator.runUntilCollected(
|
||||
() -> 0.30
|
||||
).andThen(
|
||||
manipulator.runManipulator(
|
||||
() -> 0,
|
||||
false
|
||||
).withTimeout(
|
||||
0.01
|
||||
)
|
||||
)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Lift L4",
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL4Position,
|
||||
ManipulatorPivotConstants.kL4Position
|
||||
).andThen(
|
||||
elevator.maintainPosition()
|
||||
.withTimeout(
|
||||
0.1
|
||||
),
|
||||
manipulatorPivot.maintainPosition()
|
||||
.withTimeout(
|
||||
0.1
|
||||
)
|
||||
)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"HP Pickup",
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kCoralIntakePosition,
|
||||
ManipulatorPivotConstants.kStartingPosition
|
||||
)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Shoot Algae",
|
||||
shootAlgae().withTimeout(2)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Processor Position",
|
||||
moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
|
||||
.raceWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Pickup Algae L2",
|
||||
moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
|
||||
.raceWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
.andThen(
|
||||
elevator.maintainPosition()
|
||||
.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
|
||||
|
||||
//Dont you need a holdPosition call?
|
||||
);
|
||||
}
|
||||
|
||||
//creates tabs and transforms them on the shuffleboard
|
||||
@@ -253,7 +391,7 @@ public class RobotContainer {
|
||||
sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
|
||||
.withSize(2, 1)
|
||||
.withPosition(0, 0)
|
||||
.withWidget(BuiltInWidgets.kGraph);
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
|
||||
.withSize(2, 1)
|
||||
@@ -286,46 +424,48 @@ public class RobotContainer {
|
||||
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
|
||||
.withWidget(BuiltInWidgets.kGraph);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
|
||||
|
||||
sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
|
||||
|
||||
sensorTab.addDouble("dt distance", drivetrain::driveDistance);
|
||||
|
||||
sensorTab.addDouble("velocity", drivetrain::getVelocity);
|
||||
|
||||
sensorTab.addDouble("heading", drivetrain::getHeading);
|
||||
|
||||
|
||||
//sensorTab.add("odometry", drivetrain::getPose);
|
||||
|
||||
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag());
|
||||
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX());
|
||||
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY());
|
||||
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist());
|
||||
apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
|
||||
apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
|
||||
apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
|
||||
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
|
||||
.withSize(1,1).withPosition(1,1);
|
||||
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
|
||||
.withSize(1,1).withPosition(2,1);
|
||||
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
|
||||
.withSize(1,1).withPosition(3,1);
|
||||
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
|
||||
.withSize(1,1).withPosition(4,1);
|
||||
apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
|
||||
.withSize(1,1).withPosition(5,1);
|
||||
apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
|
||||
.withSize(1,1).withPosition(6,1);
|
||||
|
||||
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
|
||||
apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
|
||||
.withSize(1,1).withPosition(1,2);
|
||||
apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
|
||||
.withSize(1,1).withPosition(2,2);
|
||||
apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
|
||||
.withSize(1,1).withPosition(3,2);
|
||||
apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
|
||||
.withSize(1,1).withPosition(4,2);
|
||||
apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
|
||||
.withSize(1,1).withPosition(5,2);
|
||||
apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
|
||||
.withSize(1,1).withPosition(6,2);
|
||||
|
||||
apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
|
||||
.withSize(2,1).withPosition(4,4);
|
||||
|
||||
apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected())
|
||||
.withSize(2, 1).withPosition(4, 2);
|
||||
apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected())
|
||||
.withSize(2, 1).withPosition(6, 2);
|
||||
|
||||
|
||||
}
|
||||
@@ -468,6 +608,14 @@ public class RobotContainer {
|
||||
.raceWith(elevator.maintainPosition()));
|
||||
}
|
||||
|
||||
private Command shootAlgae(){
|
||||
return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
|
||||
elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.raceWith(elevator.maintainPosition()));
|
||||
}
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private Command startingConfig() {
|
||||
return moveManipulatorUtil(0, 0, false, true)
|
||||
|
||||
@@ -11,25 +11,32 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
public class AutoConstants {
|
||||
public static final double kMaxSpeedMetersPerSecond = 4;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 2;
|
||||
public static final double kMaxSpeedMetersPerSecond = 5;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 4;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
||||
|
||||
public static final double kPXController = 6;
|
||||
public static final double kPYController = 6;
|
||||
public static final double kPThetaController = 5.5;
|
||||
public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
|
||||
|
||||
public static final double kPXYController = 3.5;
|
||||
public static final double kPThetaController = 5;
|
||||
|
||||
public static final double kAlignPXYController = 2;
|
||||
public static final double kAlignPThetaController = 5;
|
||||
|
||||
// Constraint for the motion profiled robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
|
||||
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
|
||||
|
||||
public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
|
||||
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
|
||||
|
||||
// TODO This is a constant being managed like a static rewriteable variable
|
||||
public static RobotConfig kRobotConfig;
|
||||
|
||||
public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
|
||||
new PIDConstants(kPXController, 0, 0),
|
||||
new PIDConstants(kPYController, 0, 0)
|
||||
new PIDConstants(kPXYController, 0, 0),
|
||||
new PIDConstants(kPThetaController, 0, 0)
|
||||
);
|
||||
|
||||
static {
|
||||
|
||||
@@ -1,9 +1,18 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
|
||||
public class ClimberRollersConstants {
|
||||
public static final int kRollerMotorID = 9;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static {
|
||||
motorConfig
|
||||
.smartCurrentLimit(40)
|
||||
.idleMode(IdleMode.kBrake)
|
||||
.inverted(true);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
public class DrivetrainConstants {
|
||||
// Driving Parameters - Note that these are not the maximum capable speeds of
|
||||
// the robot, rather the allowed maximum speeds
|
||||
public static final double kMaxSpeedMetersPerSecond = 5.5;
|
||||
public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75;
|
||||
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
|
||||
|
||||
// Chassis configuration
|
||||
@@ -57,7 +57,7 @@ public class DrivetrainConstants {
|
||||
|
||||
public static final boolean kGyroReversed = true;
|
||||
|
||||
public static final double kHeadingP = 0.01;
|
||||
public static final double kHeadingP = 0.1;
|
||||
|
||||
public static final double kXTranslationP = 0.5;
|
||||
public static final double kYTranslationP = 0.5;
|
||||
|
||||
@@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Second;
|
||||
import static edu.wpi.first.units.Units.Seconds;
|
||||
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
@@ -22,6 +23,7 @@ public class ElevatorConstants {
|
||||
|
||||
public static final int kCurrentLimit = 40;
|
||||
|
||||
/*
|
||||
public static final double kUpControllerP = 5.6;//7; //
|
||||
public static final double kUpControllerI = 0;
|
||||
public static final double kUpControllerD = 0.28;//0.28
|
||||
@@ -31,18 +33,25 @@ public class ElevatorConstants {
|
||||
public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
|
||||
|
||||
public static final double kMaintainP = 3;
|
||||
|
||||
*/
|
||||
|
||||
public static final double kP = 3;//7; //
|
||||
public static final double kI = 0;
|
||||
public static final double kD = 0;//.28;//0.28
|
||||
|
||||
public static final double kAllowedError = 1;
|
||||
|
||||
public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */
|
||||
public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6
|
||||
public static final double kFeedForwardV = 0.12; // calculated value 0.12
|
||||
|
||||
public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
public static final double kMaxVelocity = 100.0; // 100 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 50; // 50 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
public static final double kMaxVelocityAlgae = 120;
|
||||
public static final double kMaxAccelerationAlgae = 400;
|
||||
|
||||
public static final double kCoralIntakePosition = 0;
|
||||
public static final double kL1Position = 0;
|
||||
public static final double kL1Position = 17;
|
||||
public static final double kL2Position = 11;
|
||||
public static final double kL3Position = 27;
|
||||
public static final double kL4Position = 50.5;
|
||||
@@ -56,6 +65,8 @@ public class ElevatorConstants {
|
||||
|
||||
public static final double kVoltageLimit = 7;
|
||||
|
||||
public static final double kVoltageLimitAlgae = 9;
|
||||
|
||||
// 1, 7, 10 are the defaults for these, change as necessary
|
||||
public static final double kSysIDRampRate = .25;
|
||||
public static final double kSysIDStepVolts = 3;
|
||||
@@ -81,5 +92,9 @@ public class ElevatorConstants {
|
||||
motorConfig.encoder
|
||||
.positionConversionFactor(kEncoderPositionConversionFactor)
|
||||
.velocityConversionFactor(kEncoderVelocityConversionFactor);
|
||||
motorConfig.closedLoop
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.pid(kP, kI, kD)
|
||||
.outputRange(-1, 1);
|
||||
}
|
||||
}
|
||||
|
||||
15
src/main/java/frc/robot/constants/IndexerConstants.java
Normal file
15
src/main/java/frc/robot/constants/IndexerConstants.java
Normal file
@@ -0,0 +1,15 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
|
||||
public class IndexerConstants {
|
||||
public static final int kIndexerMotorID = 16;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static{
|
||||
motorConfig.smartCurrentLimit(30)
|
||||
.idleMode(IdleMode.kBrake);
|
||||
};
|
||||
}
|
||||
@@ -1,5 +1,6 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
|
||||
public class ManipulatorConstants {
|
||||
@@ -7,4 +8,9 @@ public class ManipulatorConstants {
|
||||
public static final int kCoralBeamBreakID = 2;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static{
|
||||
motorConfig.smartCurrentLimit(40)
|
||||
.idleMode(IdleMode.kBrake);
|
||||
};
|
||||
}
|
||||
|
||||
@@ -20,10 +20,12 @@ public class ManipulatorPivotConstants {
|
||||
|
||||
public static final double kPivotMaxVelocity = 2 * Math.PI;
|
||||
|
||||
public static final double kPositionalP = 4;
|
||||
public static final double kPositionalP = 4.5;
|
||||
public static final double kPositionalI = 0;
|
||||
public static final double kPositionalD = 0;
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(3);
|
||||
|
||||
public static final double kAlgaeP = 7;
|
||||
|
||||
public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
|
||||
public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
|
||||
@@ -34,11 +36,11 @@ public class ManipulatorPivotConstants {
|
||||
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
|
||||
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
|
||||
|
||||
public static final double kEncoderOffset = 0.78-0.25;
|
||||
public static final double kEncoderOffset = 0.04500000178813934;
|
||||
|
||||
public static final double kStartingPosition = Units.degreesToRadians(90);
|
||||
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
|
||||
public static final double kL1Position = Units.degreesToRadians(0.0+90);
|
||||
public static final double kL1Position = Units.degreesToRadians(246);
|
||||
public static final double kL2Position = Units.degreesToRadians(22.0+90);
|
||||
public static final double kL3Position = Units.degreesToRadians(22.0+90);
|
||||
public static final double kL4Position = Units.degreesToRadians(45.0+90);
|
||||
@@ -48,6 +50,8 @@ public class ManipulatorPivotConstants {
|
||||
public static final double kNetPosition = Units.degreesToRadians(175.0+90);
|
||||
/**The closest position to the elevator brace without hitting it */
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
|
||||
|
||||
public static final double kBargeShotPosition = Units.degreesToRadians(222);
|
||||
/**The forward rotation limit of the pivot */
|
||||
public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
|
||||
|
||||
|
||||
@@ -54,11 +54,37 @@ public class VisionConstants {
|
||||
{3.703, 3.975, 3.982, 2.806},
|
||||
{3.183, 4.191, 3.183, 3.857},
|
||||
{3.986, 5.24, 3.701, 5.076},
|
||||
{5.275, 5.075, 4.991, 5.246},
|
||||
{5.275, 5.075, 4.891, 5.284},//4.991, 5.246},
|
||||
{5.789, 3.862, 5.789, 4.194},
|
||||
{4.993, 2.816, 5.272, 2.996}
|
||||
};
|
||||
|
||||
public static final double latencyFudge = 0.080;
|
||||
public static final double[][] algaeSetpointsMap = {
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{13.906, 2.658},//6
|
||||
{14.661, 4.013},
|
||||
{13.834, 5.428},
|
||||
{12.263, 5.452},
|
||||
{11.412, 4.025},
|
||||
{12.191, 2.574},//11
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{3.649, 2.558},//17
|
||||
{2.776, 4.005},
|
||||
{3.644, 5.514},
|
||||
{5.296, 5.522},//4.991, 5.246},
|
||||
{6.225, 4.008},
|
||||
{5.322, 2.511}//22
|
||||
};
|
||||
|
||||
public static final double latencyFudge = 0.0;
|
||||
|
||||
}
|
||||
|
||||
@@ -8,6 +8,7 @@ import java.io.File;
|
||||
import java.util.Optional;
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.DoubleSupplier;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
@@ -18,7 +19,9 @@ import com.studica.frc.AHRS.NavXComType;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.controller.HolonomicDriveController;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
@@ -59,13 +62,16 @@ public class Drivetrain extends SubsystemBase {
|
||||
public Orchestra m_orchestra = new Orchestra();
|
||||
private Timer musicTimer = new Timer();
|
||||
|
||||
private PIDController pidHeading;
|
||||
private ProfiledPIDController pidHeading;
|
||||
private PIDController pidTranslationX;
|
||||
private PIDController pidTranslationY;
|
||||
|
||||
private HolonomicDriveController driveController;
|
||||
|
||||
public Vision vision;
|
||||
|
||||
public Pose2d orangePose2d;
|
||||
public Pose2d blackPose2d;
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public Drivetrain() {
|
||||
@@ -104,19 +110,21 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
new Pose2d()
|
||||
new Pose2d(),
|
||||
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
|
||||
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
|
||||
);
|
||||
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
|
||||
pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
|
||||
pidHeading.setTolerance(Units.degreesToRadians(3));
|
||||
pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
|
||||
|
||||
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
|
||||
pidHeading.enableContinuousInput(-180, 180);
|
||||
|
||||
pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
|
||||
pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0);
|
||||
pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
|
||||
pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
|
||||
pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
|
||||
pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
|
||||
|
||||
driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
|
||||
|
||||
AutoBuilder.configure(
|
||||
this::getPose,
|
||||
@@ -152,6 +160,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
vision = new Vision();
|
||||
orangePose2d = new Pose2d();
|
||||
blackPose2d = new Pose2d();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -166,14 +175,21 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
|
||||
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
|
||||
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
|
||||
|
||||
// if(vision.getOrangeDist() < 72){
|
||||
// m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
|
||||
//}
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
|
||||
|
||||
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
|
||||
if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
|
||||
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
|
||||
}else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
|
||||
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
|
||||
}
|
||||
|
||||
/*
|
||||
if(vision.getOrangeTagDetected()){
|
||||
// if the detected tags match your alliances reef tags use their pose estimates
|
||||
if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
|
||||
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
|
||||
@@ -182,29 +198,43 @@ public class Drivetrain extends SubsystemBase {
|
||||
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
|
||||
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
|
||||
}
|
||||
Logger.recordOutput("orange pose", new Pose3d());
|
||||
if(orangePose2d.equals(null)){
|
||||
Logger.recordOutput("orange pose", new Pose3d());
|
||||
}else{
|
||||
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
|
||||
}
|
||||
}
|
||||
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
|
||||
Logger.recordOutput("orange dist", vision.getOrangeDist());
|
||||
Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
|
||||
Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
|
||||
Logger.recordOutput("orange FPS", vision.getOrangeFPS());
|
||||
|
||||
|
||||
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
|
||||
if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
|
||||
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
|
||||
}else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
|
||||
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
|
||||
}
|
||||
|
||||
Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
|
||||
}
|
||||
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
|
||||
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
|
||||
|
||||
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
|
||||
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose));
|
||||
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
|
||||
|
||||
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
|
||||
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose));
|
||||
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
|
||||
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
|
||||
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
|
||||
Logger.recordOutput("black dist", vision.getBlackDist());
|
||||
Logger.recordOutput("black detected", vision.getBlackTagDetected());
|
||||
Logger.recordOutput("black tag", vision.getBlackTagDetected());
|
||||
Logger.recordOutput("black FPS", vision.getBlackFPS());
|
||||
|
||||
Logger.recordOutput("drive velocity", getVelocity());
|
||||
Logger.recordOutput("closest tag", getClosestTag());
|
||||
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
|
||||
|
||||
if(musicTimer.get()>10){
|
||||
@@ -249,7 +279,15 @@ public class Drivetrain extends SubsystemBase {
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_estimator.resetPose(
|
||||
|
||||
m_estimator.resetPosition(
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
pose
|
||||
);
|
||||
}
|
||||
@@ -292,7 +330,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
|
||||
Rotation2d.fromDegrees(getGyroValue()))
|
||||
new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
|
||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
||||
@@ -308,16 +346,33 @@ public class Drivetrain extends SubsystemBase {
|
||||
});
|
||||
}
|
||||
|
||||
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
|
||||
return run(() -> {
|
||||
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
|
||||
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1),
|
||||
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
|
||||
true);
|
||||
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
|
||||
return startRun(() -> {
|
||||
pidTranslationX.reset();
|
||||
pidTranslationY.reset();
|
||||
pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
|
||||
},
|
||||
() -> {
|
||||
ChassisSpeeds controlEffort = driveController.calculate(
|
||||
m_estimator.getEstimatedPosition(),
|
||||
new Pose2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble(),
|
||||
headingSetpoint.get()),
|
||||
0,
|
||||
headingSetpoint.get()
|
||||
);
|
||||
|
||||
double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
|
||||
if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
|
||||
double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
|
||||
controlEffort.vxMetersPerSecond *= mul;
|
||||
controlEffort.vyMetersPerSecond *= mul;
|
||||
}
|
||||
|
||||
driveWithChassisSpeeds(controlEffort);
|
||||
|
||||
Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
|
||||
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
|
||||
new Rotation2d(headingSetpoint.getAsDouble()))));
|
||||
headingSetpoint.get())));
|
||||
});
|
||||
}
|
||||
|
||||
@@ -407,7 +462,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
|
||||
return m_estimator.getEstimatedPosition().getRotation().getDegrees();
|
||||
}
|
||||
|
||||
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
|
||||
@@ -423,17 +478,17 @@ public class Drivetrain extends SubsystemBase {
|
||||
return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
|
||||
public double driveDistance(){
|
||||
return m_frontLeft.getTotalDist();
|
||||
}
|
||||
|
||||
public double getVelocity(){
|
||||
return m_frontLeft.getState().speedMetersPerSecond;
|
||||
}
|
||||
|
||||
public Command resetToVision(){
|
||||
return run(() -> {
|
||||
m_estimator.resetPose(orangePose2d);
|
||||
return runOnce(() -> {
|
||||
if(vision.getOrangeTagDetected()){
|
||||
m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
|
||||
}else if(vision.getBlackTagDetected()){
|
||||
m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2,15 +2,20 @@ package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.ClosedLoopSlot;
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.ControlType;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.ElevatorFeedforward;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
@@ -24,12 +29,14 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
private DigitalInput bottomLimitSwitch;
|
||||
|
||||
private PIDController pidControllerUp;
|
||||
private PIDController pidControllerDown;
|
||||
|
||||
private PIDController maintainPID;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
|
||||
private TrapezoidProfile trapProfile;
|
||||
private TrapezoidProfile trapProfileAlgae;
|
||||
private TrapezoidProfile.State goal;
|
||||
private TrapezoidProfile.State setpoint;
|
||||
|
||||
private SparkClosedLoopController controller;
|
||||
|
||||
public Elevator() {
|
||||
elevatorMotor1 = new SparkMax(
|
||||
@@ -60,33 +67,17 @@ public class Elevator extends SubsystemBase {
|
||||
ElevatorConstants.kBottomLimitSwitchID
|
||||
);
|
||||
|
||||
pidControllerDown = new PIDController(
|
||||
ElevatorConstants.kDownControllerP,
|
||||
ElevatorConstants.kDownControllerI,
|
||||
ElevatorConstants.kDownControllerD
|
||||
);
|
||||
pidControllerDown.setSetpoint(0);
|
||||
|
||||
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
pidControllerUp = new PIDController(
|
||||
ElevatorConstants.kUpControllerP,
|
||||
ElevatorConstants.kUpControllerI,
|
||||
ElevatorConstants.kUpControllerD
|
||||
);
|
||||
pidControllerUp.setSetpoint(0);
|
||||
|
||||
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
|
||||
|
||||
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardS,
|
||||
ElevatorConstants.kFeedForwardG,
|
||||
ElevatorConstants.kFeedForwardV
|
||||
);
|
||||
|
||||
trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration));
|
||||
|
||||
trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae));
|
||||
|
||||
controller = elevatorMotor1.getClosedLoopController();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -94,6 +85,8 @@ public class Elevator extends SubsystemBase {
|
||||
if (!getBottomLimitSwitch()) {
|
||||
encoder.setPosition(0);
|
||||
}
|
||||
|
||||
Logger.recordOutput("elevator position", getEncoderPosition());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -147,28 +140,15 @@ public class Elevator extends SubsystemBase {
|
||||
public Command maintainPosition() {
|
||||
|
||||
return startRun(() -> {
|
||||
maintainPID.reset();
|
||||
maintainPID.setSetpoint(pidControllerUp.getSetpoint());
|
||||
|
||||
},
|
||||
() -> {
|
||||
|
||||
double maintainOutput = maintainPID.calculate(getEncoderPosition());
|
||||
|
||||
if(!maintainPID.atSetpoint())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
maintainOutput + feedForward.calculate(0), -2, 2)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
*/
|
||||
controller.setReference(
|
||||
encoder.getPosition(),
|
||||
ControlType.kPosition,
|
||||
ClosedLoopSlot.kSlot0,
|
||||
feedForward.calculate(0.0)
|
||||
);
|
||||
|
||||
});
|
||||
|
||||
@@ -186,109 +166,42 @@ public class Elevator extends SubsystemBase {
|
||||
* Moves the elevator to a target destination (setpoint).
|
||||
*
|
||||
* @param setpoint Target destination of the subsystem
|
||||
* @param timeout Time to achieve the setpoint before quitting
|
||||
* @return Sets motor voltage to achieve the target destination
|
||||
*/
|
||||
public Command goToSetpoint(DoubleSupplier setpoint) {
|
||||
|
||||
if (setpoint.getAsDouble() == 0) {
|
||||
return startRun(() -> {
|
||||
public Command goToSetpoint(DoubleSupplier setGoal) {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
|
||||
.andThen(runManualElevator(() -> -.5)
|
||||
.until(() -> encoder.getPosition() == 0));
|
||||
return startRun(() -> {
|
||||
goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
|
||||
}, () -> {
|
||||
setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
|
||||
|
||||
} else {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
controller.setReference(
|
||||
setpoint.position,
|
||||
ControlType.kPosition,
|
||||
ClosedLoopSlot.kSlot0,
|
||||
feedForward.calculate(encoder.getVelocity())
|
||||
);
|
||||
*/
|
||||
/*
|
||||
if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
|
||||
});*/
|
||||
}
|
||||
|
||||
}).until(() -> trapProfile.isFinished(encoder.getPosition()));
|
||||
|
||||
}
|
||||
|
||||
public Command goToSetpointAlgae(DoubleSupplier setGoal) {
|
||||
|
||||
/*
|
||||
if(encoder.getPosition() >= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerUp.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
|
||||
);
|
||||
}else if(encoder.getPosition() <= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerDown.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
|
||||
);
|
||||
}
|
||||
*/
|
||||
return startRun(() -> {
|
||||
goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
|
||||
}, () -> {
|
||||
setpoint = trapProfileAlgae.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
|
||||
|
||||
controller.setReference(
|
||||
setpoint.position,
|
||||
ControlType.kPosition,
|
||||
ClosedLoopSlot.kSlot0,
|
||||
feedForward.calculate(setpoint.velocity)
|
||||
);
|
||||
|
||||
}).until(() -> trapProfileAlgae.isFinished(encoder.getPosition()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current encoder position
|
||||
@@ -327,19 +240,4 @@ public class Elevator extends SubsystemBase {
|
||||
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
|
||||
}
|
||||
|
||||
public double getPIDUpSetpoint() {
|
||||
return pidControllerUp.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDUpError() {
|
||||
return pidControllerUp.getError();
|
||||
}
|
||||
|
||||
public double getPIDDownSetpoint() {
|
||||
return pidControllerDown.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDDownError() {
|
||||
return pidControllerDown.getError();
|
||||
}
|
||||
}
|
||||
@@ -144,7 +144,4 @@ public class MAXSwerveModule {
|
||||
m_drive.setPosition(0);
|
||||
}
|
||||
|
||||
public double getTotalDist(){
|
||||
return m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2,6 +2,8 @@ package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
@@ -10,11 +12,14 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.IndexerConstants;
|
||||
import frc.robot.constants.ManipulatorConstants;
|
||||
|
||||
public class Manipulator extends SubsystemBase {
|
||||
private SparkMax manipulatorMotor;
|
||||
|
||||
private SparkMax indexerMotor;
|
||||
|
||||
private DigitalInput coralBeamBreak;
|
||||
|
||||
public Manipulator() {
|
||||
@@ -29,9 +34,27 @@ public class Manipulator extends SubsystemBase {
|
||||
PersistMode.kPersistParameters
|
||||
);
|
||||
|
||||
indexerMotor = new SparkMax(
|
||||
IndexerConstants.kIndexerMotorID,
|
||||
MotorType.kBrushless
|
||||
);
|
||||
|
||||
indexerMotor.configure(
|
||||
IndexerConstants.motorConfig,
|
||||
ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters
|
||||
);
|
||||
|
||||
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
super.periodic();
|
||||
|
||||
Logger.recordOutput("coral beam break", getCoralBeamBreak());
|
||||
}
|
||||
|
||||
/**
|
||||
* The default command for the manipulator that either stops the manipulator or slowly
|
||||
* runs the manipulator to retain the algae
|
||||
@@ -56,6 +79,8 @@ public class Manipulator extends SubsystemBase {
|
||||
manipulatorMotor.set(
|
||||
coral ? speed.getAsDouble() : speed.getAsDouble() * -1
|
||||
);
|
||||
|
||||
indexerMotor.set(0);
|
||||
});
|
||||
}
|
||||
|
||||
@@ -68,15 +93,33 @@ public class Manipulator extends SubsystemBase {
|
||||
*/
|
||||
public Command runUntilCollected(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
manipulatorMotor.set(
|
||||
speed.getAsDouble()
|
||||
manipulatorMotor.setVoltage(
|
||||
speed.getAsDouble()*12
|
||||
);
|
||||
}).until(() -> !coralBeamBreak.get());
|
||||
|
||||
indexerMotor.set(1);
|
||||
|
||||
}).unless(() -> !coralBeamBreak.get())
|
||||
.until(() -> !coralBeamBreak.get());
|
||||
/*
|
||||
return run(() -> {
|
||||
if(getCoralBeamBreak()) {
|
||||
manipulatorMotor.set(
|
||||
speed.getAsDouble()
|
||||
);
|
||||
} else {
|
||||
manipulatorMotor.set(
|
||||
speed.getAsDouble()
|
||||
);
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
public Command retractCommand(DoubleSupplier retractSpeed){
|
||||
return run(() -> {
|
||||
manipulatorMotor.set(-retractSpeed.getAsDouble());
|
||||
|
||||
indexerMotor.set(0);
|
||||
}
|
||||
).until(() -> coralBeamBreak.get());
|
||||
}
|
||||
|
||||
@@ -4,12 +4,13 @@ import com.revrobotics.spark.SparkMax;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.spark.SparkAbsoluteEncoder;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.ArmFeedforward;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
@@ -26,6 +27,8 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
|
||||
private PIDController pidController;
|
||||
|
||||
private PIDController algaePIDController;
|
||||
|
||||
public ManipulatorPivot() {
|
||||
pivotMotor = new SparkMax(
|
||||
ManipulatorPivotConstants.kPivotMotorID,
|
||||
@@ -45,10 +48,22 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
ManipulatorPivotConstants.kPositionalI,
|
||||
ManipulatorPivotConstants.kPositionalD
|
||||
);
|
||||
pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
|
||||
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidController.enableContinuousInput(0, 280);
|
||||
|
||||
algaePIDController = new PIDController(
|
||||
ManipulatorPivotConstants.kAlgaeP,
|
||||
0,
|
||||
0);
|
||||
algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
|
||||
|
||||
algaePIDController.setSetpoint(0);
|
||||
|
||||
algaePIDController.enableContinuousInput(0, 280);
|
||||
|
||||
feedForward = new ArmFeedforward(
|
||||
ManipulatorPivotConstants.kFeedForwardS,
|
||||
ManipulatorPivotConstants.kFeedForwardG,
|
||||
@@ -56,6 +71,14 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
super.periodic();
|
||||
|
||||
Logger.recordOutput("manipulator position", getEncoderPosition());
|
||||
Logger.recordOutput("manipulator setpoint", pidController.getSetpoint());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether or not the motion is safe relative to the encoder's current position
|
||||
* and the arm safe stow position
|
||||
@@ -126,6 +149,38 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
}).until(() -> pidController.atSetpoint());
|
||||
}
|
||||
|
||||
public Command goToSetpointAlgae(DoubleSupplier setpoint) {
|
||||
return startRun(() -> {
|
||||
|
||||
algaePIDController.setSetpoint(setpoint.getAsDouble());
|
||||
algaePIDController.reset();
|
||||
pidController.setSetpoint(setpoint.getAsDouble());
|
||||
pidController.reset();
|
||||
},
|
||||
() -> {
|
||||
/*
|
||||
if (!pidController.atSetpoint()) {
|
||||
pivotMotor.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
setpoint.getAsDouble()
|
||||
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
|
||||
);
|
||||
} else {
|
||||
pivotMotor.setVoltage(
|
||||
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
|
||||
);
|
||||
}
|
||||
*/
|
||||
pivotMotor.setVoltage(
|
||||
algaePIDController.calculate(
|
||||
encoder.getPosition(),
|
||||
setpoint.getAsDouble()
|
||||
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
|
||||
);
|
||||
}).until(() -> algaePIDController.atSetpoint());
|
||||
}
|
||||
|
||||
public Command maintainPosition() {
|
||||
return startRun(() -> {
|
||||
|
||||
|
||||
@@ -15,6 +15,8 @@ import frc.robot.constants.VisionConstants;
|
||||
|
||||
public class Vision{
|
||||
|
||||
private NetworkTable blackVisionTable;
|
||||
|
||||
private DoubleSubscriber black_tx;
|
||||
private DoubleSubscriber black_ty;
|
||||
private DoubleSubscriber black_dist;
|
||||
@@ -24,6 +26,8 @@ public class Vision{
|
||||
|
||||
private DoubleSubscriber blackFramerate;
|
||||
|
||||
private NetworkTable orangeVisionTable;
|
||||
|
||||
private DoubleSubscriber orange_tx;
|
||||
private DoubleSubscriber orange_ty;
|
||||
private DoubleSubscriber orange_dist;
|
||||
@@ -39,8 +43,8 @@ public class Vision{
|
||||
public Vision(){
|
||||
NetworkTableInstance inst = NetworkTableInstance.getDefault();
|
||||
|
||||
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
|
||||
NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
|
||||
blackVisionTable = inst.getTable("black_Fiducial");
|
||||
orangeVisionTable = inst.getTable("orange_Fiducial");
|
||||
|
||||
black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
|
||||
black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
|
||||
@@ -80,9 +84,9 @@ public class Vision{
|
||||
|
||||
// System.out.println(gyroBuffer.getSample(timestamp));
|
||||
|
||||
double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-Units.degreesToRadians( camPose[1]) + Units.degreesToRadians(ty));
|
||||
double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-camPose[1] + Units.degreesToRadians(ty));
|
||||
|
||||
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(Units.degreesToRadians(camPose[2])).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
|
||||
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
|
||||
|
||||
Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
|
||||
Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
|
||||
@@ -93,8 +97,8 @@ public class Vision{
|
||||
.getTranslation();
|
||||
Pose2d robotPose = new Pose2d(
|
||||
fieldToCameraTranslation,
|
||||
new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(Units.degreesToRadians( camPose[2]))))
|
||||
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(Units.degreesToRadians(camPose[2]))), Pose2d.kZero));
|
||||
new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(camPose[2])))
|
||||
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
|
||||
|
||||
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
|
||||
|
||||
@@ -102,8 +106,8 @@ public class Vision{
|
||||
}
|
||||
|
||||
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
|
||||
return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
|
||||
getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
|
||||
return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
|
||||
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
|
||||
}
|
||||
|
||||
public double getBlackTX(){
|
||||
@@ -123,7 +127,7 @@ public class Vision{
|
||||
}
|
||||
|
||||
public double getBlackTimeStamp(){
|
||||
return black_tx.getLastChange();
|
||||
return black_tx.getLastChange()-VisionConstants.latencyFudge;
|
||||
}
|
||||
|
||||
public boolean getBlackTagDetected(){
|
||||
@@ -137,7 +141,7 @@ public class Vision{
|
||||
public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
|
||||
if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
|
||||
return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
|
||||
orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
|
||||
orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
|
||||
);
|
||||
}else{
|
||||
return new Pose2d();
|
||||
@@ -171,5 +175,13 @@ public class Vision{
|
||||
public double getOrangeFPS(){
|
||||
return orangeFramerate.get();
|
||||
}
|
||||
|
||||
public boolean isBlackConnected(){
|
||||
return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
|
||||
}
|
||||
|
||||
public boolean isOrangeConnected(){
|
||||
return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user