Compare commits
19 Commits
grimpe
...
8bc8f0390f
| Author | SHA1 | Date | |
|---|---|---|---|
|
8bc8f0390f | ||
|
|
c0c48a3f24 | ||
|
|
cbeb99c1a5 | ||
|
|
8bd9ce36a2 | ||
|
|
ab241f2f65 | ||
|
|
9f017968e1 | ||
|
|
029bba7bb6 | ||
|
|
6fb4e0c1ac | ||
|
|
9af46de189 | ||
|
|
de156e3789 | ||
|
688315bcd0 | ||
|
|
cf29380c64 | ||
|
|
a5df627b64 | ||
|
|
b0ce8aaad7 | ||
|
|
76ea02c3dd | ||
|
|
bd180e9153 | ||
|
|
773e3b2244 | ||
|
|
bff426ef0f | ||
|
|
4b0262fa4d |
1
Reefscape-tuner-project.json
Normal file
1
Reefscape-tuner-project.json
Normal file
File diff suppressed because one or more lines are too long
@@ -1,9 +1,4 @@
|
|||||||
{
|
{
|
||||||
"System Joysticks": {
|
|
||||||
"window": {
|
|
||||||
"enabled": false
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"keyboardJoysticks": [
|
"keyboardJoysticks": [
|
||||||
{
|
{
|
||||||
"axisConfig": [
|
"axisConfig": [
|
||||||
|
|||||||
57
src/main/deploy/pathplanner/autos/BlueBas1.auto
Normal file
57
src/main/deploy/pathplanner/autos/BlueBas1.auto
Normal file
@@ -0,0 +1,57 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
151
src/main/deploy/pathplanner/autos/BlueBas2.auto
Normal file
151
src/main/deploy/pathplanner/autos/BlueBas2.auto
Normal file
@@ -0,0 +1,151 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 1.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasChercher"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Station"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasPorter"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
245
src/main/deploy/pathplanner/autos/BlueBas3.auto
Normal file
245
src/main/deploy/pathplanner/autos/BlueBas3.auto
Normal file
@@ -0,0 +1,245 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 1.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasChercher"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Station"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralInspire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasPorter"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 1.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasChercher2"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralInspire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralInspire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueBasPorter2"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
57
src/main/deploy/pathplanner/autos/BlueHaut1.auto
Normal file
57
src/main/deploy/pathplanner/autos/BlueHaut1.auto
Normal file
@@ -0,0 +1,57 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
151
src/main/deploy/pathplanner/autos/BlueHaut2.auto
Normal file
151
src/main/deploy/pathplanner/autos/BlueHaut2.auto
Normal file
@@ -0,0 +1,151 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 1.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautChercher"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Station"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralInspire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautPorter"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
245
src/main/deploy/pathplanner/autos/BlueHaut3.auto
Normal file
245
src/main/deploy/pathplanner/autos/BlueHaut3.auto
Normal file
@@ -0,0 +1,245 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 1.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautChercher"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Station"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralInspire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautPorter"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 1.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautChercher2"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Station"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralInspire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueHautPorter2"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
87
src/main/deploy/pathplanner/autos/MilieuAlgue.auto
Normal file
87
src/main/deploy/pathplanner/autos/MilieuAlgue.auto
Normal file
@@ -0,0 +1,87 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueMilieuStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L3"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoraletAlgue"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
57
src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto
Normal file
57
src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto
Normal file
@@ -0,0 +1,57 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BlueMilieuStart"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "AprilTag"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "wait",
|
||||||
|
"data": {
|
||||||
|
"waitTime": 0.5
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "L4"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "CoralExpire"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
19
src/main/deploy/pathplanner/autos/Sortir.auto
Normal file
19
src/main/deploy/pathplanner/autos/Sortir.auto
Normal file
@@ -0,0 +1,19 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "Sortir"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
1
src/main/deploy/pathplanner/navgrid.json
Normal file
1
src/main/deploy/pathplanner/navgrid.json
Normal file
File diff suppressed because one or more lines are too long
70
src/main/deploy/pathplanner/paths/BlueBasChercher.path
Normal file
70
src/main/deploy/pathplanner/paths/BlueBasChercher.path
Normal file
@@ -0,0 +1,70 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.226639344262295,
|
||||||
|
"y": 2.748309426229507
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 4.699180327868851,
|
||||||
|
"y": 2.1728995901639334
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 3.296618852459016,
|
||||||
|
"y": 1.6334528688524586
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 4.5553278688524586,
|
||||||
|
"y": 1.9091700819672122
|
||||||
|
},
|
||||||
|
"nextControl": {
|
||||||
|
"x": 2.853313965378558,
|
||||||
|
"y": 1.5363479888253109
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.2227459016393443,
|
||||||
|
"y": 1.141956967213115
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 2.2217919676666584,
|
||||||
|
"y": 1.3902362024139756
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -125.00000000000001
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 119.99999999999999
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueBasChercher2.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueBasChercher2.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 3.7761270491803276,
|
||||||
|
"y": 2.7602971311475404
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 3.152766393442623,
|
||||||
|
"y": 1.2138831967213104
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.1987704918032787,
|
||||||
|
"y": 1.153944672131147
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 2.049897540983607,
|
||||||
|
"y": 1.1899077868852443
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -125.00000000000001
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 59.99999999999999
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueBasPorter.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueBasPorter.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.1987704918032787,
|
||||||
|
"y": 1.153944672131147
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 2.1697745901639345,
|
||||||
|
"y": 1.0820184426229504
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 3.788114754098361,
|
||||||
|
"y": 2.7722848360655736
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 3.5483606557377048,
|
||||||
|
"y": 1.1899077868852452
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 59.99999999999999
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -125.00000000000001
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueBasPorter2.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueBasPorter2.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.2107581967213117,
|
||||||
|
"y": 1.141956967213115
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 3.392520491803279,
|
||||||
|
"y": 1.7293545081967205
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 2.9969262295081966,
|
||||||
|
"y": 4.019006147540983
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 1.738217213114754,
|
||||||
|
"y": 3.5035348360655734
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 0.0
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -125.00000000000001
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueBasStart.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueBasStart.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 7.204610655737704,
|
||||||
|
"y": 1.058043032786885
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 6.400623816881145,
|
||||||
|
"y": 1.8077934662846444
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.214651639344262,
|
||||||
|
"y": 2.7722848360655736
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 6.021565749152438,
|
||||||
|
"y": 2.0272293626478377
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 119.99999999999999
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 119.99999999999999
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueHautChercher.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueHautChercher.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.262602459016393,
|
||||||
|
"y": 5.33765368852459
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 4.567315573770491,
|
||||||
|
"y": 6.272694672131147
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.3066598360655735,
|
||||||
|
"y": 6.920030737704917
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 2.074600566094311,
|
||||||
|
"y": 6.608197684403563
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 125.00000000000001
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -119.99999999999999
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueHautChercher2.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueHautChercher2.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 3.728176229508197,
|
||||||
|
"y": 5.277715163934426
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 3.6884488572050125,
|
||||||
|
"y": 5.738000701502482
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.2946721311475409,
|
||||||
|
"y": 6.88406762295082
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 1.5370661625758562,
|
||||||
|
"y": 6.822870126858832
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 125.00000000000001
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -59.99999999999999
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueHautPorter.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueHautPorter.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.2946721311475409,
|
||||||
|
"y": 6.88406762295082
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 1.8197466808473133,
|
||||||
|
"y": 6.152201615570423
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 3.7401639344262296,
|
||||||
|
"y": 5.313678278688524
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 2.9533875048545335,
|
||||||
|
"y": 6.158498893994983
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -59.99999999999999
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 125.00000000000001
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueHautPorter2.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueHautPorter2.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.2467213114754097,
|
||||||
|
"y": 6.896055327868852
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 3.728176229508196,
|
||||||
|
"y": 6.28468237704918
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 2.9969262295081966,
|
||||||
|
"y": 3.9950307377049183
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 2.6372950819672134,
|
||||||
|
"y": 4.906096311475409
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 0.0
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 125.00000000000001
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueHautStart.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueHautStart.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 7.22858606557377,
|
||||||
|
"y": 7.075870901639344
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 5.75022271481963,
|
||||||
|
"y": 5.75328924697144
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.226639344262295,
|
||||||
|
"y": 5.313678278688524
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 6.307613733266616,
|
||||||
|
"y": 6.263998815998496
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -119.99999999999999
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -119.99999999999999
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/BlueMilieuStart.path
Normal file
54
src/main/deploy/pathplanner/paths/BlueMilieuStart.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 7.300512295081968,
|
||||||
|
"y": 3.971055327868852
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 6.294286688638351,
|
||||||
|
"y": 3.9754565484205786
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.9818647540983605,
|
||||||
|
"y": 3.971055327868852
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 6.956229117816136,
|
||||||
|
"y": 3.9576132249181755
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 180.0
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 180.0
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
54
src/main/deploy/pathplanner/paths/Sortir.path
Normal file
54
src/main/deploy/pathplanner/paths/Sortir.path
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 7.504303278688524,
|
||||||
|
"y": 6.464497950819672
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 7.084733606557376,
|
||||||
|
"y": 6.488473360655737
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.849999999999999,
|
||||||
|
"y": 6.464497950819672
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 6.32950819672131,
|
||||||
|
"y": 6.464497950819672
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 540.0,
|
||||||
|
"maxAngularAcceleration": 720.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 180.0
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 180.0
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
32
src/main/deploy/pathplanner/settings.json
Normal file
32
src/main/deploy/pathplanner/settings.json
Normal file
@@ -0,0 +1,32 @@
|
|||||||
|
{
|
||||||
|
"robotWidth": 0.9,
|
||||||
|
"robotLength": 0.9,
|
||||||
|
"holonomicMode": true,
|
||||||
|
"pathFolders": [],
|
||||||
|
"autoFolders": [],
|
||||||
|
"defaultMaxVel": 3.0,
|
||||||
|
"defaultMaxAccel": 3.0,
|
||||||
|
"defaultMaxAngVel": 540.0,
|
||||||
|
"defaultMaxAngAccel": 720.0,
|
||||||
|
"defaultNominalVoltage": 12.0,
|
||||||
|
"robotMass": 74.088,
|
||||||
|
"robotMOI": 6.883,
|
||||||
|
"robotTrackwidth": 0.546,
|
||||||
|
"driveWheelRadius": 0.048,
|
||||||
|
"driveGearing": 5.143,
|
||||||
|
"maxDriveSpeed": 5.45,
|
||||||
|
"driveMotorType": "krakenX60",
|
||||||
|
"driveCurrentLimit": 60.0,
|
||||||
|
"wheelCOF": 1.2,
|
||||||
|
"flModuleX": 0.35,
|
||||||
|
"flModuleY": 0.267,
|
||||||
|
"frModuleX": 0.35,
|
||||||
|
"frModuleY": -0.267,
|
||||||
|
"blModuleX": -0.35,
|
||||||
|
"blModuleY": 0.267,
|
||||||
|
"brModuleX": -0.35,
|
||||||
|
"brModuleY": -0.267,
|
||||||
|
"bumperOffsetX": 0.0,
|
||||||
|
"bumperOffsetY": 0.0,
|
||||||
|
"robotFeatures": []
|
||||||
|
}
|
||||||
@@ -4,42 +4,91 @@
|
|||||||
|
|
||||||
package frc.robot;
|
package frc.robot;
|
||||||
|
|
||||||
|
import static edu.wpi.first.units.Units.*;
|
||||||
|
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||||
|
import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt;
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||||
|
|
||||||
|
import com.pathplanner.lib.auto.AutoBuilder;
|
||||||
|
import com.pathplanner.lib.auto.NamedCommands;
|
||||||
|
|
||||||
|
import edu.wpi.first.cameraserver.CameraServer;
|
||||||
import edu.wpi.first.math.MathUtil;
|
import edu.wpi.first.math.MathUtil;
|
||||||
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||||
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.Commands;
|
import edu.wpi.first.wpilibj2.command.Commands;
|
||||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||||
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||||
import frc.robot.commands.GrimperHaut;
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
|
||||||
import frc.robot.commands.GrimpeurBas;
|
|
||||||
import frc.robot.commands.GrimpeurManuel;
|
import frc.robot.TunerConstants.TunerConstants;
|
||||||
import frc.robot.commands.ResetGrimpeur;
|
import frc.robot.commands.RainBow;
|
||||||
import frc.robot.subsystems.Bougie;
|
import frc.robot.subsystems.Bougie;
|
||||||
|
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||||
import frc.robot.subsystems.Grimpeur;
|
import frc.robot.subsystems.Grimpeur;
|
||||||
|
|
||||||
|
|
||||||
public class RobotContainer {
|
public class RobotContainer {
|
||||||
CommandXboxController manette1 = new CommandXboxController(0);
|
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
|
||||||
CommandXboxController manette2 = new CommandXboxController(1);
|
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
||||||
Grimpeur grimpeur = new Grimpeur();
|
|
||||||
|
/* Setting up bindings for necessary control of the swerve drive platform */
|
||||||
|
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
||||||
|
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
||||||
|
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
||||||
|
|
||||||
|
private final Telemetry logger = new Telemetry(MaxSpeed);
|
||||||
|
|
||||||
|
private final CommandXboxController manette1 = new CommandXboxController(0);
|
||||||
|
private final CommandXboxController manette2 = new CommandXboxController(0);
|
||||||
|
|
||||||
|
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||||
|
|
||||||
|
private final SendableChooser<Command> autoChooser;
|
||||||
Bougie bougie = new Bougie();
|
Bougie bougie = new Bougie();
|
||||||
GrimpeurManuel grimpeurManuel = new GrimpeurManuel(grimpeur, manette2::getLeftY);
|
|
||||||
|
|
||||||
public RobotContainer() {
|
public RobotContainer() {
|
||||||
grimpeur.setDefaultCommand(new RunCommand(()->{
|
autoChooser = AutoBuilder.buildAutoChooser("New Auto");
|
||||||
if(grimpeur.stop()){
|
SmartDashboard.putData("Auto Mode", autoChooser);
|
||||||
grimpeur.grimpe(0);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
grimpeur.grimpe(MathUtil.applyDeadband(manette2.getLeftY(), 0.2));
|
|
||||||
}}, grimpeur));
|
|
||||||
configureBindings();
|
configureBindings();
|
||||||
}
|
}
|
||||||
|
|
||||||
private void configureBindings() {
|
private void configureBindings() {
|
||||||
manette2.leftBumper().whileTrue(new GrimperHaut(grimpeur,bougie));
|
// Note that X is defined as forward according to WPILib convention,
|
||||||
manette2.rightBumper().whileTrue(new GrimpeurBas(grimpeur));
|
// and Y is defined as to the left according to WPILib convention.
|
||||||
manette2.a().whileTrue(new ResetGrimpeur(grimpeur));
|
drivetrain.setDefaultCommand(
|
||||||
|
// Drivetrain will execute this command periodically
|
||||||
|
drivetrain.applyRequest(() ->
|
||||||
|
drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY(), 0.5)) // Drive forward with negative Y (forward)
|
||||||
|
.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left)
|
||||||
|
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left)
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// reset the field-centric heading on left bumper press
|
||||||
|
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
|
||||||
|
|
||||||
|
drivetrain.registerTelemetry(logger::telemeterize);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
public Command getAutonomousCommand() {
|
public Command getAutonomousCommand() {
|
||||||
return Commands.print("No autonomous command configured");
|
return
|
||||||
|
new
|
||||||
|
SequentialCommandGroup(
|
||||||
|
autoChooser
|
||||||
|
.getSelected().andThen(
|
||||||
|
new
|
||||||
|
RainBow
|
||||||
|
(
|
||||||
|
bougie
|
||||||
|
)
|
||||||
|
));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
124
src/main/java/frc/robot/Telemetry.java
Normal file
124
src/main/java/frc/robot/Telemetry.java
Normal file
@@ -0,0 +1,124 @@
|
|||||||
|
package frc.robot;
|
||||||
|
|
||||||
|
import com.ctre.phoenix6.SignalLogger;
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||||
|
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||||
|
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||||
|
import edu.wpi.first.networktables.DoubleArrayPublisher;
|
||||||
|
import edu.wpi.first.networktables.DoublePublisher;
|
||||||
|
import edu.wpi.first.networktables.NetworkTable;
|
||||||
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||||
|
import edu.wpi.first.networktables.StringPublisher;
|
||||||
|
import edu.wpi.first.networktables.StructArrayPublisher;
|
||||||
|
import edu.wpi.first.networktables.StructPublisher;
|
||||||
|
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
|
||||||
|
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
|
||||||
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
|
import edu.wpi.first.wpilibj.util.Color;
|
||||||
|
import edu.wpi.first.wpilibj.util.Color8Bit;
|
||||||
|
|
||||||
|
public class Telemetry {
|
||||||
|
private final double MaxSpeed;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Construct a telemetry object, with the specified max speed of the robot
|
||||||
|
*
|
||||||
|
* @param maxSpeed Maximum speed in meters per second
|
||||||
|
*/
|
||||||
|
public Telemetry(double maxSpeed) {
|
||||||
|
MaxSpeed = maxSpeed;
|
||||||
|
SignalLogger.start();
|
||||||
|
}
|
||||||
|
|
||||||
|
/* What to publish over networktables for telemetry */
|
||||||
|
private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
|
||||||
|
|
||||||
|
/* Robot swerve drive state */
|
||||||
|
private final NetworkTable driveStateTable = inst.getTable("DriveState");
|
||||||
|
private final StructPublisher<Pose2d> drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
|
||||||
|
private final StructPublisher<ChassisSpeeds> driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
|
||||||
|
private final StructArrayPublisher<SwerveModuleState> driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish();
|
||||||
|
private final StructArrayPublisher<SwerveModuleState> driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish();
|
||||||
|
private final StructArrayPublisher<SwerveModulePosition> driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish();
|
||||||
|
private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish();
|
||||||
|
private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish();
|
||||||
|
|
||||||
|
/* Robot pose for field positioning */
|
||||||
|
private final NetworkTable table = inst.getTable("Pose");
|
||||||
|
private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
|
||||||
|
private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
|
||||||
|
|
||||||
|
/* Mechanisms to represent the swerve module states */
|
||||||
|
private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
|
||||||
|
new Mechanism2d(1, 1),
|
||||||
|
new Mechanism2d(1, 1),
|
||||||
|
new Mechanism2d(1, 1),
|
||||||
|
new Mechanism2d(1, 1),
|
||||||
|
};
|
||||||
|
/* A direction and length changing ligament for speed representation */
|
||||||
|
private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
|
||||||
|
m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
||||||
|
m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
||||||
|
m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
||||||
|
m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
||||||
|
};
|
||||||
|
/* A direction changing and length constant ligament for module direction */
|
||||||
|
private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
|
||||||
|
m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
|
||||||
|
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
||||||
|
m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
|
||||||
|
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
||||||
|
m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
|
||||||
|
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
||||||
|
m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
|
||||||
|
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
||||||
|
};
|
||||||
|
|
||||||
|
private final double[] m_poseArray = new double[3];
|
||||||
|
private final double[] m_moduleStatesArray = new double[8];
|
||||||
|
private final double[] m_moduleTargetsArray = new double[8];
|
||||||
|
|
||||||
|
/** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
|
||||||
|
public void telemeterize(SwerveDriveState state) {
|
||||||
|
/* Telemeterize the swerve drive state */
|
||||||
|
drivePose.set(state.Pose);
|
||||||
|
driveSpeeds.set(state.Speeds);
|
||||||
|
driveModuleStates.set(state.ModuleStates);
|
||||||
|
driveModuleTargets.set(state.ModuleTargets);
|
||||||
|
driveModulePositions.set(state.ModulePositions);
|
||||||
|
driveTimestamp.set(state.Timestamp);
|
||||||
|
driveOdometryFrequency.set(1.0 / state.OdometryPeriod);
|
||||||
|
|
||||||
|
/* Also write to log file */
|
||||||
|
m_poseArray[0] = state.Pose.getX();
|
||||||
|
m_poseArray[1] = state.Pose.getY();
|
||||||
|
m_poseArray[2] = state.Pose.getRotation().getDegrees();
|
||||||
|
for (int i = 0; i < 4; ++i) {
|
||||||
|
m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians();
|
||||||
|
m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
|
||||||
|
m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians();
|
||||||
|
m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
|
||||||
|
}
|
||||||
|
|
||||||
|
SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
|
||||||
|
SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
|
||||||
|
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
|
||||||
|
SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
|
||||||
|
|
||||||
|
/* Telemeterize the pose to a Field2d */
|
||||||
|
fieldTypePub.set("Field2d");
|
||||||
|
fieldPub.set(m_poseArray);
|
||||||
|
|
||||||
|
/* Telemeterize the module states to a Mechanism2d */
|
||||||
|
for (int i = 0; i < 4; ++i) {
|
||||||
|
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
|
||||||
|
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
|
||||||
|
m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
|
||||||
|
|
||||||
|
SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
286
src/main/java/frc/robot/TunerConstants/TunerConstants.java
Normal file
286
src/main/java/frc/robot/TunerConstants/TunerConstants.java
Normal file
@@ -0,0 +1,286 @@
|
|||||||
|
package frc.robot.TunerConstants;
|
||||||
|
|
||||||
|
import static edu.wpi.first.units.Units.*;
|
||||||
|
|
||||||
|
import com.ctre.phoenix6.CANBus;
|
||||||
|
import com.ctre.phoenix6.configs.*;
|
||||||
|
import com.ctre.phoenix6.hardware.*;
|
||||||
|
import com.ctre.phoenix6.signals.*;
|
||||||
|
import com.ctre.phoenix6.swerve.*;
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.Matrix;
|
||||||
|
import edu.wpi.first.math.numbers.N1;
|
||||||
|
import edu.wpi.first.math.numbers.N3;
|
||||||
|
import edu.wpi.first.units.measure.*;
|
||||||
|
|
||||||
|
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||||
|
|
||||||
|
// Generated by the Tuner X Swerve Project Generator
|
||||||
|
// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
|
||||||
|
public class TunerConstants {
|
||||||
|
// Both sets of gains need to be tuned to your individual robot.
|
||||||
|
|
||||||
|
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
||||||
|
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
||||||
|
private static final Slot0Configs steerGains = new Slot0Configs()
|
||||||
|
.withKP(100).withKI(0).withKD(0.5)
|
||||||
|
.withKS(0.1).withKV(2.66).withKA(0)
|
||||||
|
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||||
|
// When using closed-loop control, the drive motor uses the control
|
||||||
|
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
||||||
|
private static final Slot0Configs driveGains = new Slot0Configs()
|
||||||
|
.withKP(0.1).withKI(0).withKD(0)
|
||||||
|
.withKS(0).withKV(0.124);
|
||||||
|
|
||||||
|
// The closed-loop output type to use for the steer motors;
|
||||||
|
// This affects the PID/FF gains for the steer motors
|
||||||
|
private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
||||||
|
// The closed-loop output type to use for the drive motors;
|
||||||
|
// This affects the PID/FF gains for the drive motors
|
||||||
|
private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
||||||
|
|
||||||
|
// The type of motor used for the drive motor
|
||||||
|
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
|
||||||
|
// The type of motor used for the drive motor
|
||||||
|
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
|
||||||
|
|
||||||
|
// The remote sensor feedback type to use for the steer motors;
|
||||||
|
// When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
|
||||||
|
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
|
||||||
|
|
||||||
|
// The stator current at which the wheels start to slip;
|
||||||
|
// This needs to be tuned to your individual robot
|
||||||
|
private static final Current kSlipCurrent = Amps.of(120.0);
|
||||||
|
|
||||||
|
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
|
||||||
|
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
|
||||||
|
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
|
||||||
|
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
|
||||||
|
.withCurrentLimits(
|
||||||
|
new CurrentLimitsConfigs()
|
||||||
|
// Swerve azimuth does not require much torque output, so we can set a relatively low
|
||||||
|
// stator current limit to help avoid brownouts without impacting performance.
|
||||||
|
.withStatorCurrentLimit(Amps.of(60))
|
||||||
|
.withStatorCurrentLimitEnable(true)
|
||||||
|
);
|
||||||
|
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
|
||||||
|
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
|
||||||
|
private static final Pigeon2Configuration pigeonConfigs = null;
|
||||||
|
|
||||||
|
// CAN bus that the devices are located on;
|
||||||
|
// All swerve devices must share the same CAN bus
|
||||||
|
public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot");
|
||||||
|
|
||||||
|
// Theoretical free speed (m/s) at 12 V applied output;
|
||||||
|
// This needs to be tuned to your individual robot
|
||||||
|
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21);
|
||||||
|
|
||||||
|
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
|
||||||
|
// This may need to be tuned to your individual robot
|
||||||
|
private static final double kCoupleRatio = 3.5714285714285716;
|
||||||
|
|
||||||
|
private static final double kDriveGearRatio = 6.122448979591837;
|
||||||
|
private static final double kSteerGearRatio = 21.428571428571427;
|
||||||
|
private static final Distance kWheelRadius = Inches.of(2);
|
||||||
|
|
||||||
|
private static final boolean kInvertLeftSide = false;
|
||||||
|
private static final boolean kInvertRightSide = true;
|
||||||
|
|
||||||
|
private static final int kPigeonId = 13;
|
||||||
|
|
||||||
|
// These are only used for simulation
|
||||||
|
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
|
||||||
|
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
|
||||||
|
// Simulated voltage necessary to overcome friction
|
||||||
|
private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
|
||||||
|
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
|
||||||
|
|
||||||
|
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
|
||||||
|
.withCANBusName(kCANBus.getName())
|
||||||
|
.withPigeon2Id(kPigeonId)
|
||||||
|
.withPigeon2Configs(pigeonConfigs);
|
||||||
|
|
||||||
|
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
|
||||||
|
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
|
||||||
|
.withDriveMotorGearRatio(kDriveGearRatio)
|
||||||
|
.withSteerMotorGearRatio(kSteerGearRatio)
|
||||||
|
.withCouplingGearRatio(kCoupleRatio)
|
||||||
|
.withWheelRadius(kWheelRadius)
|
||||||
|
.withSteerMotorGains(steerGains)
|
||||||
|
.withDriveMotorGains(driveGains)
|
||||||
|
.withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
|
||||||
|
.withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
|
||||||
|
.withSlipCurrent(kSlipCurrent)
|
||||||
|
.withSpeedAt12Volts(kSpeedAt12Volts)
|
||||||
|
.withDriveMotorType(kDriveMotorType)
|
||||||
|
.withSteerMotorType(kSteerMotorType)
|
||||||
|
.withFeedbackSource(kSteerFeedbackType)
|
||||||
|
.withDriveMotorInitialConfigs(driveInitialConfigs)
|
||||||
|
.withSteerMotorInitialConfigs(steerInitialConfigs)
|
||||||
|
.withEncoderInitialConfigs(encoderInitialConfigs)
|
||||||
|
.withSteerInertia(kSteerInertia)
|
||||||
|
.withDriveInertia(kDriveInertia)
|
||||||
|
.withSteerFrictionVoltage(kSteerFrictionVoltage)
|
||||||
|
.withDriveFrictionVoltage(kDriveFrictionVoltage);
|
||||||
|
|
||||||
|
|
||||||
|
// Front Left
|
||||||
|
private static final int kFrontLeftDriveMotorId = 4;
|
||||||
|
private static final int kFrontLeftSteerMotorId = 5;
|
||||||
|
private static final int kFrontLeftEncoderId = 12;
|
||||||
|
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.353271484375);
|
||||||
|
private static final boolean kFrontLeftSteerMotorInverted = true;
|
||||||
|
private static final boolean kFrontLeftEncoderInverted = false;
|
||||||
|
|
||||||
|
private static final Distance kFrontLeftXPos = Inches.of(13.5);
|
||||||
|
private static final Distance kFrontLeftYPos = Inches.of(10.5);
|
||||||
|
|
||||||
|
// Front Right
|
||||||
|
private static final int kFrontRightDriveMotorId = 2;
|
||||||
|
private static final int kFrontRightSteerMotorId = 6;
|
||||||
|
private static final int kFrontRightEncoderId = 9;
|
||||||
|
private static final Angle kFrontRightEncoderOffset = Rotations.of(0.2119140625);
|
||||||
|
private static final boolean kFrontRightSteerMotorInverted = true;
|
||||||
|
private static final boolean kFrontRightEncoderInverted = false;
|
||||||
|
|
||||||
|
private static final Distance kFrontRightXPos = Inches.of(13.5);
|
||||||
|
private static final Distance kFrontRightYPos = Inches.of(-10.5);
|
||||||
|
|
||||||
|
// Back Left
|
||||||
|
private static final int kBackLeftDriveMotorId = 18;
|
||||||
|
private static final int kBackLeftSteerMotorId = 8;
|
||||||
|
private static final int kBackLeftEncoderId = 11;
|
||||||
|
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.236572265625);
|
||||||
|
private static final boolean kBackLeftSteerMotorInverted = true;
|
||||||
|
private static final boolean kBackLeftEncoderInverted = false;
|
||||||
|
|
||||||
|
private static final Distance kBackLeftXPos = Inches.of(-13.5);
|
||||||
|
private static final Distance kBackLeftYPos = Inches.of(10.5);
|
||||||
|
|
||||||
|
// Back Right
|
||||||
|
private static final int kBackRightDriveMotorId = 3;
|
||||||
|
private static final int kBackRightSteerMotorId = 7;
|
||||||
|
private static final int kBackRightEncoderId = 10;
|
||||||
|
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.330078125);
|
||||||
|
private static final boolean kBackRightSteerMotorInverted = true;
|
||||||
|
private static final boolean kBackRightEncoderInverted = false;
|
||||||
|
|
||||||
|
private static final Distance kBackRightXPos = Inches.of(-13.5);
|
||||||
|
private static final Distance kBackRightYPos = Inches.of(-10.5);
|
||||||
|
|
||||||
|
|
||||||
|
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
|
||||||
|
ConstantCreator.createModuleConstants(
|
||||||
|
kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
|
||||||
|
kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
|
||||||
|
);
|
||||||
|
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
|
||||||
|
ConstantCreator.createModuleConstants(
|
||||||
|
kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
|
||||||
|
kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
|
||||||
|
);
|
||||||
|
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
|
||||||
|
ConstantCreator.createModuleConstants(
|
||||||
|
kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
|
||||||
|
kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
|
||||||
|
);
|
||||||
|
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
|
||||||
|
ConstantCreator.createModuleConstants(
|
||||||
|
kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
|
||||||
|
kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
|
||||||
|
);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a CommandSwerveDrivetrain instance.
|
||||||
|
* This should only be called once in your robot program,.
|
||||||
|
*/
|
||||||
|
public static CommandSwerveDrivetrain createDrivetrain() {
|
||||||
|
return new CommandSwerveDrivetrain(
|
||||||
|
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
|
||||||
|
*/
|
||||||
|
public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
|
||||||
|
/**
|
||||||
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||||
|
* <p>
|
||||||
|
* This constructs the underlying hardware devices, so users should not construct
|
||||||
|
* the devices themselves. If they need the devices, they can access them through
|
||||||
|
* getters in the classes.
|
||||||
|
*
|
||||||
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||||
|
* @param modules Constants for each specific module
|
||||||
|
*/
|
||||||
|
public TunerSwerveDrivetrain(
|
||||||
|
SwerveDrivetrainConstants drivetrainConstants,
|
||||||
|
SwerveModuleConstants<?, ?, ?>... modules
|
||||||
|
) {
|
||||||
|
super(
|
||||||
|
TalonFX::new, TalonFX::new, CANcoder::new,
|
||||||
|
drivetrainConstants, modules
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||||
|
* <p>
|
||||||
|
* This constructs the underlying hardware devices, so users should not construct
|
||||||
|
* the devices themselves. If they need the devices, they can access them through
|
||||||
|
* getters in the classes.
|
||||||
|
*
|
||||||
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||||
|
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
||||||
|
* unspecified or set to 0 Hz, this is 250 Hz on
|
||||||
|
* CAN FD, and 100 Hz on CAN 2.0.
|
||||||
|
* @param modules Constants for each specific module
|
||||||
|
*/
|
||||||
|
public TunerSwerveDrivetrain(
|
||||||
|
SwerveDrivetrainConstants drivetrainConstants,
|
||||||
|
double odometryUpdateFrequency,
|
||||||
|
SwerveModuleConstants<?, ?, ?>... modules
|
||||||
|
) {
|
||||||
|
super(
|
||||||
|
TalonFX::new, TalonFX::new, CANcoder::new,
|
||||||
|
drivetrainConstants, odometryUpdateFrequency, modules
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||||
|
* <p>
|
||||||
|
* This constructs the underlying hardware devices, so users should not construct
|
||||||
|
* the devices themselves. If they need the devices, they can access them through
|
||||||
|
* getters in the classes.
|
||||||
|
*
|
||||||
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||||
|
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
||||||
|
* unspecified or set to 0 Hz, this is 250 Hz on
|
||||||
|
* CAN FD, and 100 Hz on CAN 2.0.
|
||||||
|
* @param odometryStandardDeviation The standard deviation for odometry calculation
|
||||||
|
* in the form [x, y, theta]ᵀ, with units in meters
|
||||||
|
* and radians
|
||||||
|
* @param visionStandardDeviation The standard deviation for vision calculation
|
||||||
|
* in the form [x, y, theta]ᵀ, with units in meters
|
||||||
|
* and radians
|
||||||
|
* @param modules Constants for each specific module
|
||||||
|
*/
|
||||||
|
public TunerSwerveDrivetrain(
|
||||||
|
SwerveDrivetrainConstants drivetrainConstants,
|
||||||
|
double odometryUpdateFrequency,
|
||||||
|
Matrix<N3, N1> odometryStandardDeviation,
|
||||||
|
Matrix<N3, N1> visionStandardDeviation,
|
||||||
|
SwerveModuleConstants<?, ?, ?>... modules
|
||||||
|
) {
|
||||||
|
super(
|
||||||
|
TalonFX::new, TalonFX::new, CANcoder::new,
|
||||||
|
drivetrainConstants, odometryUpdateFrequency,
|
||||||
|
odometryStandardDeviation, visionStandardDeviation, modules
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -1,56 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package frc.robot.commands;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Grimpeur;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class GrimperHaut extends Command {
|
|
||||||
private Grimpeur grimpeur;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new Grimper. */
|
|
||||||
public GrimperHaut(Grimpeur grimpeur, Bougie bougie) {
|
|
||||||
this.grimpeur = grimpeur;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(grimpeur,bougie);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
if(grimpeur.stop()==true){
|
|
||||||
grimpeur.grimpe(0);
|
|
||||||
grimpeur.reset();
|
|
||||||
bougie.RainBow();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
grimpeur.grimpe(0.5);
|
|
||||||
bougie.RainBowStop();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
grimpeur.grimpe(0);
|
|
||||||
if(grimpeur.stop()){
|
|
||||||
bougie.RainBow();
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return grimpeur.stop()==true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,48 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package frc.robot.commands;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Grimpeur;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class GrimpeurBas extends Command {
|
|
||||||
private Grimpeur grimpeur;
|
|
||||||
/** Creates a new GrimpeurBas. */
|
|
||||||
public GrimpeurBas(Grimpeur grimpeur) {
|
|
||||||
this.grimpeur = grimpeur;
|
|
||||||
addRequirements(grimpeur);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
if(grimpeur.encodeur()>=-38.5 && grimpeur.encodeur()<=-39.19){
|
|
||||||
grimpeur.grimpe(0);
|
|
||||||
}
|
|
||||||
else if(grimpeur.encodeur()>=-38.5){
|
|
||||||
grimpeur.grimpe(-0.5);
|
|
||||||
}
|
|
||||||
else{grimpeur.grimpe(0.5);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
grimpeur.grimpe(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,51 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package frc.robot.commands;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Grimpeur;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class GrimpeurManuel extends Command {
|
|
||||||
private Grimpeur grimpeur;
|
|
||||||
private DoubleSupplier x;
|
|
||||||
/** Creates a new GrimpeurManuel. */
|
|
||||||
public GrimpeurManuel(Grimpeur grimpeur,DoubleSupplier x) {
|
|
||||||
this.grimpeur = grimpeur;
|
|
||||||
this.x = x;
|
|
||||||
addRequirements(grimpeur);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
if(grimpeur.stop()){
|
|
||||||
grimpeur.grimpe(0);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
grimpeur.grimpe(x.getAsDouble());
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
grimpeur.grimpe(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -9,8 +9,8 @@ import frc.robot.subsystems.Bougie;
|
|||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||||
public class RainBow extends Command {
|
public class RainBow extends Command {
|
||||||
|
Bougie bougie;
|
||||||
/** Creates a new RainBow. */
|
/** Creates a new RainBow. */
|
||||||
private Bougie bougie;
|
|
||||||
public RainBow(Bougie bougie) {
|
public RainBow(Bougie bougie) {
|
||||||
this.bougie = bougie;
|
this.bougie = bougie;
|
||||||
addRequirements(bougie);
|
addRequirements(bougie);
|
||||||
@@ -19,19 +19,15 @@ public class RainBow extends Command {
|
|||||||
|
|
||||||
// Called when the command is initially scheduled.
|
// Called when the command is initially scheduled.
|
||||||
@Override
|
@Override
|
||||||
public void initialize() {}
|
public void initialize() {bougie.RainBow();}
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
// Called every time the scheduler runs while the command is scheduled.
|
||||||
@Override
|
@Override
|
||||||
public void execute() {
|
public void execute() {}
|
||||||
bougie.RainBow();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
// Called once the command ends or is interrupted.
|
||||||
@Override
|
@Override
|
||||||
public void end(boolean interrupted) {
|
public void end(boolean interrupted) {bougie.RainBowStop();}
|
||||||
bougie.RainBowStop();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
// Returns true when the command should end.
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -1,39 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package frc.robot.commands;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Grimpeur;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class ResetGrimpeur extends Command {
|
|
||||||
private Grimpeur grimpeur;
|
|
||||||
/** Creates a new ResetGrimpeur. */
|
|
||||||
public ResetGrimpeur(Grimpeur grimpeur) {
|
|
||||||
this.grimpeur = grimpeur;
|
|
||||||
addRequirements(grimpeur);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
grimpeur.reset();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
294
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
Normal file
294
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
Normal file
@@ -0,0 +1,294 @@
|
|||||||
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
|
import static edu.wpi.first.units.Units.*;
|
||||||
|
|
||||||
|
import java.util.function.Supplier;
|
||||||
|
|
||||||
|
import com.ctre.phoenix6.SignalLogger;
|
||||||
|
import com.ctre.phoenix6.Utils;
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||||
|
|
||||||
|
import com.pathplanner.lib.auto.AutoBuilder;
|
||||||
|
import com.pathplanner.lib.config.PIDConstants;
|
||||||
|
import com.pathplanner.lib.config.RobotConfig;
|
||||||
|
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.Matrix;
|
||||||
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
|
import edu.wpi.first.math.numbers.N1;
|
||||||
|
import edu.wpi.first.math.numbers.N3;
|
||||||
|
import edu.wpi.first.wpilibj.DriverStation;
|
||||||
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||||
|
import edu.wpi.first.wpilibj.Notifier;
|
||||||
|
import edu.wpi.first.wpilibj.RobotController;
|
||||||
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
|
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||||
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||||
|
import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
|
||||||
|
* Subsystem so it can easily be used in command-based projects.
|
||||||
|
*/
|
||||||
|
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
|
||||||
|
|
||||||
|
private static final double kSimLoopPeriod = 0.005; // 5 ms
|
||||||
|
private Notifier m_simNotifier = null;
|
||||||
|
private double m_lastSimTime;
|
||||||
|
|
||||||
|
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
|
||||||
|
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
|
||||||
|
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
|
||||||
|
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
|
||||||
|
/* Keep track if we've ever applied the operator perspective before or not */
|
||||||
|
private boolean m_hasAppliedOperatorPerspective = false;
|
||||||
|
|
||||||
|
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
|
||||||
|
|
||||||
|
/* Swerve requests to apply during SysId characterization */
|
||||||
|
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
|
||||||
|
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
|
||||||
|
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
|
||||||
|
|
||||||
|
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
|
||||||
|
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
|
||||||
|
new SysIdRoutine.Config(
|
||||||
|
null, // Use default ramp rate (1 V/s)
|
||||||
|
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
|
||||||
|
null, // Use default timeout (10 s)
|
||||||
|
// Log state with SignalLogger class
|
||||||
|
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
|
||||||
|
),
|
||||||
|
new SysIdRoutine.Mechanism(
|
||||||
|
output -> setControl(m_translationCharacterization.withVolts(output)),
|
||||||
|
null,
|
||||||
|
this
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
|
||||||
|
private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
|
||||||
|
new SysIdRoutine.Config(
|
||||||
|
null, // Use default ramp rate (1 V/s)
|
||||||
|
Volts.of(7), // Use dynamic voltage of 7 V
|
||||||
|
null, // Use default timeout (10 s)
|
||||||
|
// Log state with SignalLogger class
|
||||||
|
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
|
||||||
|
),
|
||||||
|
new SysIdRoutine.Mechanism(
|
||||||
|
volts -> setControl(m_steerCharacterization.withVolts(volts)),
|
||||||
|
null,
|
||||||
|
this
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* SysId routine for characterizing rotation.
|
||||||
|
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
|
||||||
|
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
|
||||||
|
*/
|
||||||
|
private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
|
||||||
|
new SysIdRoutine.Config(
|
||||||
|
/* This is in radians per second², but SysId only supports "volts per second" */
|
||||||
|
Volts.of(Math.PI / 6).per(Second),
|
||||||
|
/* This is in radians per second, but SysId only supports "volts" */
|
||||||
|
Volts.of(Math.PI),
|
||||||
|
null, // Use default timeout (10 s)
|
||||||
|
// Log state with SignalLogger class
|
||||||
|
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
|
||||||
|
),
|
||||||
|
new SysIdRoutine.Mechanism(
|
||||||
|
output -> {
|
||||||
|
/* output is actually radians per second, but SysId only supports "volts" */
|
||||||
|
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
|
||||||
|
/* also log the requested output for SysId */
|
||||||
|
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
|
||||||
|
},
|
||||||
|
null,
|
||||||
|
this
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
private void configureAutoBuilder() {
|
||||||
|
try {
|
||||||
|
var config = RobotConfig.fromGUISettings();
|
||||||
|
AutoBuilder.configure(
|
||||||
|
() -> getState().Pose, // Supplier of current robot pose
|
||||||
|
this::resetPose, // Consumer for seeding pose against auto
|
||||||
|
() -> getState().Speeds, // Supplier of current robot speeds
|
||||||
|
// Consumer of ChassisSpeeds and feedforwards to drive the robot
|
||||||
|
(speeds, feedforwards) -> setControl(
|
||||||
|
m_pathApplyRobotSpeeds.withSpeeds(speeds)
|
||||||
|
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
|
||||||
|
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
|
||||||
|
),
|
||||||
|
new PPHolonomicDriveController(
|
||||||
|
// PID constants for translation
|
||||||
|
new PIDConstants(10, 0, 0),
|
||||||
|
// PID constants for rotation
|
||||||
|
new PIDConstants(7, 0, 0)
|
||||||
|
),
|
||||||
|
config,
|
||||||
|
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
|
||||||
|
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
|
||||||
|
this // Subsystem for requirements
|
||||||
|
);
|
||||||
|
} catch (Exception ex) {
|
||||||
|
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* The SysId routine to test */
|
||||||
|
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||||
|
* <p>
|
||||||
|
* This constructs the underlying hardware devices, so users should not construct
|
||||||
|
* the devices themselves. If they need the devices, they can access them through
|
||||||
|
* getters in the classes.
|
||||||
|
*
|
||||||
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||||
|
* @param modules Constants for each specific module
|
||||||
|
*/
|
||||||
|
public CommandSwerveDrivetrain(
|
||||||
|
SwerveDrivetrainConstants drivetrainConstants,
|
||||||
|
SwerveModuleConstants<?, ?, ?>... modules
|
||||||
|
) {
|
||||||
|
super(drivetrainConstants, modules);
|
||||||
|
if (Utils.isSimulation()) {
|
||||||
|
startSimThread();
|
||||||
|
}
|
||||||
|
configureAutoBuilder();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||||
|
* <p>
|
||||||
|
* This constructs the underlying hardware devices, so users should not construct
|
||||||
|
* the devices themselves. If they need the devices, they can access them through
|
||||||
|
* getters in the classes.
|
||||||
|
*
|
||||||
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||||
|
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
||||||
|
* unspecified or set to 0 Hz, this is 250 Hz on
|
||||||
|
* CAN FD, and 100 Hz on CAN 2.0.
|
||||||
|
* @param modules Constants for each specific module
|
||||||
|
*/
|
||||||
|
public CommandSwerveDrivetrain(
|
||||||
|
SwerveDrivetrainConstants drivetrainConstants,
|
||||||
|
double odometryUpdateFrequency,
|
||||||
|
SwerveModuleConstants<?, ?, ?>... modules
|
||||||
|
) {
|
||||||
|
super(drivetrainConstants, odometryUpdateFrequency, modules);
|
||||||
|
if (Utils.isSimulation()) {
|
||||||
|
startSimThread();
|
||||||
|
}
|
||||||
|
configureAutoBuilder();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||||
|
* <p>
|
||||||
|
* This constructs the underlying hardware devices, so users should not construct
|
||||||
|
* the devices themselves. If they need the devices, they can access them through
|
||||||
|
* getters in the classes.
|
||||||
|
*
|
||||||
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
||||||
|
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
||||||
|
* unspecified or set to 0 Hz, this is 250 Hz on
|
||||||
|
* CAN FD, and 100 Hz on CAN 2.0.
|
||||||
|
* @param odometryStandardDeviation The standard deviation for odometry calculation
|
||||||
|
* in the form [x, y, theta]ᵀ, with units in meters
|
||||||
|
* and radians
|
||||||
|
* @param visionStandardDeviation The standard deviation for vision calculation
|
||||||
|
* in the form [x, y, theta]ᵀ, with units in meters
|
||||||
|
* and radians
|
||||||
|
* @param modules Constants for each specific module
|
||||||
|
*/
|
||||||
|
public CommandSwerveDrivetrain(
|
||||||
|
SwerveDrivetrainConstants drivetrainConstants,
|
||||||
|
double odometryUpdateFrequency,
|
||||||
|
Matrix<N3, N1> odometryStandardDeviation,
|
||||||
|
Matrix<N3, N1> visionStandardDeviation,
|
||||||
|
SwerveModuleConstants<?, ?, ?>... modules
|
||||||
|
) {
|
||||||
|
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
|
||||||
|
if (Utils.isSimulation()) {
|
||||||
|
startSimThread();
|
||||||
|
}
|
||||||
|
configureAutoBuilder();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Returns a command that applies the specified control request to this swerve drivetrain.
|
||||||
|
*
|
||||||
|
* @param request Function returning the request to apply
|
||||||
|
* @return Command to run
|
||||||
|
*/
|
||||||
|
public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
|
||||||
|
return run(() -> this.setControl(requestSupplier.get()));
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Runs the SysId Quasistatic test in the given direction for the routine
|
||||||
|
* specified by {@link #m_sysIdRoutineToApply}.
|
||||||
|
*
|
||||||
|
* @param direction Direction of the SysId Quasistatic test
|
||||||
|
* @return Command to run
|
||||||
|
*/
|
||||||
|
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
|
||||||
|
return m_sysIdRoutineToApply.quasistatic(direction);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Runs the SysId Dynamic test in the given direction for the routine
|
||||||
|
* specified by {@link #m_sysIdRoutineToApply}.
|
||||||
|
*
|
||||||
|
* @param direction Direction of the SysId Dynamic test
|
||||||
|
* @return Command to run
|
||||||
|
*/
|
||||||
|
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
|
||||||
|
return m_sysIdRoutineToApply.dynamic(direction);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void periodic() {
|
||||||
|
/*
|
||||||
|
* Periodically try to apply the operator perspective.
|
||||||
|
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
|
||||||
|
* This allows us to correct the perspective in case the robot code restarts mid-match.
|
||||||
|
* Otherwise, only check and apply the operator perspective if the DS is disabled.
|
||||||
|
* This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
|
||||||
|
*/
|
||||||
|
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
|
||||||
|
DriverStation.getAlliance().ifPresent(allianceColor -> {
|
||||||
|
setOperatorPerspectiveForward(
|
||||||
|
allianceColor == Alliance.Red
|
||||||
|
? kRedAlliancePerspectiveRotation
|
||||||
|
: kBlueAlliancePerspectiveRotation
|
||||||
|
);
|
||||||
|
m_hasAppliedOperatorPerspective = true;
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void startSimThread() {
|
||||||
|
m_lastSimTime = Utils.getCurrentTimeSeconds();
|
||||||
|
|
||||||
|
/* Run simulation at a faster rate so PID gains behave more reasonably */
|
||||||
|
m_simNotifier = new Notifier(() -> {
|
||||||
|
final double currentTime = Utils.getCurrentTimeSeconds();
|
||||||
|
double deltaTime = currentTime - m_lastSimTime;
|
||||||
|
m_lastSimTime = currentTime;
|
||||||
|
|
||||||
|
/* use the measured time delta, get battery voltage from WPILib */
|
||||||
|
updateSimState(deltaTime, RobotController.getBatteryVoltage());
|
||||||
|
});
|
||||||
|
m_simNotifier.startPeriodic(kSimLoopPeriod);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -4,34 +4,20 @@
|
|||||||
|
|
||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import com.revrobotics.spark.SparkMax;
|
|
||||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
import edu.wpi.first.wpilibj.DigitalInput;
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
|
||||||
public class Grimpeur extends SubsystemBase {
|
public class Grimpeur extends SubsystemBase {
|
||||||
/** Creates a new Grimpeur. */
|
/** Creates a new Grimpeur. */
|
||||||
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
public Grimpeur() {}
|
||||||
public Grimpeur() {
|
final Spark grimpeur = new Spark(0);
|
||||||
teb.addBoolean("limit grimpeur", this::stop);
|
final DigitalInput limit1 = new DigitalInput(0);
|
||||||
teb.addDouble("encodeur grimpeur", this::encodeur);
|
|
||||||
}
|
|
||||||
final SparkMax grimpeur = new SparkMax(21,MotorType.kBrushless);
|
|
||||||
final DigitalInput limit1 = new DigitalInput(2);
|
|
||||||
public void grimpe(double vitesse){
|
public void grimpe(double vitesse){
|
||||||
grimpeur.set(vitesse);
|
grimpeur.set(vitesse);
|
||||||
}
|
}
|
||||||
public boolean stop(){
|
final void stop(){
|
||||||
return limit1.get();
|
limit1.get();
|
||||||
}
|
|
||||||
public double encodeur(){
|
|
||||||
return grimpeur.getEncoder().getPosition();
|
|
||||||
}
|
|
||||||
public void reset(){
|
|
||||||
grimpeur.getEncoder().setPosition(0);
|
|
||||||
}
|
}
|
||||||
@Override
|
@Override
|
||||||
public void periodic() {
|
public void periodic() {
|
||||||
|
|||||||
1
tuner-swerve-project2025.json
Normal file
1
tuner-swerve-project2025.json
Normal file
File diff suppressed because one or more lines are too long
Reference in New Issue
Block a user