Compare commits
No commits in common. "main" and "MIAMI-VALLEY-POST" have entirely different histories.
main
...
MIAMI-VALL
|
|
@ -1 +1 @@
|
||||||
Subproject commit c0478b01a526712b8308ac329017857177a3c876
|
Subproject commit 840c851253187081a50ca9492c032cec8d376e7b
|
||||||
|
|
@ -1,88 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "1.1 Hub to Shoot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "deadline",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "ChassisAlignShoot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 10.0
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "1.2 Shoot to Depot Path"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "deadline",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "RunIntake"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 10.0
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "1.3 Depot to Shoot Path"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "deadline",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "ChassisAlignShoot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 10.0
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
|
|
@ -1,25 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "9.1 Chaos Monkey Path"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Shoot preload"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
|
|
@ -1,19 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "HUB_START"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
|
|
@ -1,19 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "LEFT_TRENCH_START"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
|
|
@ -1,19 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "RIGHT_TRENCH_START"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
|
|
@ -23,7 +23,7 @@
|
||||||
{
|
{
|
||||||
"type": "named",
|
"type": "named",
|
||||||
"data": {
|
"data": {
|
||||||
"name": "TurretShoot"
|
"name": "ChassisAlignShoot"
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
]
|
]
|
||||||
|
|
|
||||||
|
|
@ -1,58 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.5,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 3.0,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.0,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 2.5,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 1.5,
|
|
||||||
"maxAcceleration": 1.5,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -177.9047474356738
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
<<<<<<< HEAD:src/main/deploy/pathplanner/paths/Hub to Depot.path
|
|
||||||
"rotation": -179.99999999992184
|
|
||||||
=======
|
|
||||||
"rotation": -179.69626968995792
|
|
||||||
>>>>>>> c2fa27aa5cc22124b930645e802b8176658edb7a:src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": false
|
|
||||||
}
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.5,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 2.75,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.0,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 2.75,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 360.0,
|
|
||||||
"maxAngularAcceleration": 720.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 177.4888349270938
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -179.0393266954795
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.0,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 2.2144854970378125,
|
|
||||||
"y": 3.8554998234204434
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.0,
|
|
||||||
"y": 6.0
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 1.2166471375173105,
|
|
||||||
"y": 5.875244167248359
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 360.0,
|
|
||||||
"maxAngularAcceleration": 720.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -178.09121133641756
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.4910281334858283
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.0,
|
|
||||||
"y": 6.0
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 1.453699105174549,
|
|
||||||
"y": 6.084627986890304
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.0,
|
|
||||||
"y": 4.0
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 2.266577414275921,
|
|
||||||
"y": 3.829411518660614
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 1.5,
|
|
||||||
"maxAcceleration": 1.5,
|
|
||||||
"maxAngularVelocity": 100.0,
|
|
||||||
"maxAngularAcceleration": 100.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -179.02897806892076
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -179.69626968995792
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": false
|
|
||||||
}
|
|
||||||
|
|
@ -1,102 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.5184925161825644,
|
|
||||||
"y": 7.5345006466771025
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 4.489963675146704,
|
|
||||||
"y": 7.219438447522688
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 7.6,
|
|
||||||
"y": 6.812200117368956
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 7.532870181778978,
|
|
||||||
"y": 7.8099443669167
|
|
||||||
},
|
|
||||||
"nextControl": {
|
|
||||||
"x": 7.650893142287796,
|
|
||||||
"y": 6.055780073270545
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 7.6,
|
|
||||||
"y": 0.9617601315610045
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 7.654625343431328,
|
|
||||||
"y": 1.468826273060325
|
|
||||||
},
|
|
||||||
"nextControl": {
|
|
||||||
"x": 7.527819365080411,
|
|
||||||
"y": 0.2917349428743933
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 0.5,
|
|
||||||
"y": 0.6692650602433468
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 1.096874747061515,
|
|
||||||
"y": 0.6440356562159093
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [
|
|
||||||
{
|
|
||||||
"waypointRelativePos": 1.2332261810856255,
|
|
||||||
"rotationDegrees": -90.0
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"waypointRelativePos": 1.7516108262639798,
|
|
||||||
"rotationDegrees": 0.0
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [
|
|
||||||
{
|
|
||||||
"name": "",
|
|
||||||
"waypointRelativePos": 0,
|
|
||||||
"endWaypointRelativePos": null,
|
|
||||||
"command": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 360.0,
|
|
||||||
"maxAngularAcceleration": 720.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -1.2282908533168928
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -0.1533900770684255
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.519556041182133,
|
|
||||||
"y": 4.059804750409042
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 3.769556041182133,
|
|
||||||
"y": 4.059804750409042
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.519556041182133,
|
|
||||||
"y": 4.059804750409042
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 3.269556041182133,
|
|
||||||
"y": 4.059804750409042
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 360.0,
|
|
||||||
"maxAngularAcceleration": 720.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 4.0,
|
|
||||||
"y": 7.423545951843262
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 4.25,
|
|
||||||
"y": 7.423545951843262
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 4.0,
|
|
||||||
"y": 7.423545951843262
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 3.75,
|
|
||||||
"y": 7.423545951843262
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 360.0,
|
|
||||||
"maxAngularAcceleration": 720.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
|
|
@ -88,7 +88,7 @@
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 3.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 360.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
|
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 4.0,
|
|
||||||
"y": 0.6236959156237152
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 4.25,
|
|
||||||
"y": 0.6236959156237152
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 4.0,
|
|
||||||
"y": 0.6236959156237152
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 3.75,
|
|
||||||
"y": 0.6236959156237152
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 360.0,
|
|
||||||
"maxAngularAcceleration": 720.0,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
|
|
@ -3,13 +3,13 @@
|
||||||
"waypoints": [
|
"waypoints": [
|
||||||
{
|
{
|
||||||
"anchor": {
|
"anchor": {
|
||||||
"x": 3.5383012048192777,
|
"x": 3.5055180722891564,
|
||||||
"y": 7.4553734939759035
|
"y": 7.466301204819277
|
||||||
},
|
},
|
||||||
"prevControl": null,
|
"prevControl": null,
|
||||||
"nextControl": {
|
"nextControl": {
|
||||||
"x": 4.538301204819281,
|
"x": 4.505518072289158,
|
||||||
"y": 7.4553734939759035
|
"y": 7.466301204819277
|
||||||
},
|
},
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
"linkedName": null
|
"linkedName": null
|
||||||
|
|
@ -174,7 +174,7 @@
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 3.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 360.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
|
|
|
||||||
|
|
@ -85,7 +85,7 @@
|
||||||
},
|
},
|
||||||
"prevControl": {
|
"prevControl": {
|
||||||
"x": 6.020824888067715,
|
"x": 6.020824888067715,
|
||||||
"y": 0.8531028227756465
|
"y": 0.8531028227756466
|
||||||
},
|
},
|
||||||
"nextControl": {
|
"nextControl": {
|
||||||
"x": 5.667271497474452,
|
"x": 5.667271497474452,
|
||||||
|
|
@ -178,7 +178,7 @@
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 3.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 360.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
|
|
|
||||||
|
|
@ -32,12 +32,12 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"anchor": {
|
"anchor": {
|
||||||
"x": 2.677383349903843,
|
"x": 2.9483700538099855,
|
||||||
"y": 4.00221686746988
|
"y": 4.00221686746988
|
||||||
},
|
},
|
||||||
"prevControl": {
|
"prevControl": {
|
||||||
"x": 2.4274139067638107,
|
"x": 2.5986833068220334,
|
||||||
"y": 4.006125511257969
|
"y": 4.024072289156626
|
||||||
},
|
},
|
||||||
"nextControl": null,
|
"nextControl": null,
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
|
|
@ -68,7 +68,7 @@
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 3.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 360.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
|
|
|
||||||
|
|
@ -16,27 +16,27 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"anchor": {
|
"anchor": {
|
||||||
"x": 2.1723373493975906,
|
"x": 2.3581084337349396,
|
||||||
"y": 4.00221686746988
|
"y": 4.00221686746988
|
||||||
},
|
},
|
||||||
"prevControl": {
|
"prevControl": {
|
||||||
"x": 1.8772891566265066,
|
"x": 1.3581084337349396,
|
||||||
"y": 4.024072289156626
|
"y": 4.00221686746988
|
||||||
},
|
},
|
||||||
"nextControl": {
|
"nextControl": {
|
||||||
"x": 2.421654288011768,
|
"x": 3.3581084337349396,
|
||||||
"y": 3.9837489460910525
|
"y": 4.00221686746988
|
||||||
},
|
},
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
"linkedName": null
|
"linkedName": null
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"anchor": {
|
"anchor": {
|
||||||
"x": 2.1723373493975906,
|
"x": 3.48366265060241,
|
||||||
"y": 4.00221686746988
|
"y": 4.00221686746988
|
||||||
},
|
},
|
||||||
"prevControl": {
|
"prevControl": {
|
||||||
"x": 1.8226506024096385,
|
"x": 3.133975903614458,
|
||||||
"y": 3.9912891566265065
|
"y": 3.9912891566265065
|
||||||
},
|
},
|
||||||
"nextControl": null,
|
"nextControl": null,
|
||||||
|
|
@ -44,7 +44,12 @@
|
||||||
"linkedName": null
|
"linkedName": null
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"rotationTargets": [],
|
"rotationTargets": [
|
||||||
|
{
|
||||||
|
"waypointRelativePos": 0.936692913385826,
|
||||||
|
"rotationDegrees": 180.0
|
||||||
|
}
|
||||||
|
],
|
||||||
"constraintZones": [],
|
"constraintZones": [],
|
||||||
"pointTowardsZones": [],
|
"pointTowardsZones": [],
|
||||||
"eventMarkers": [
|
"eventMarkers": [
|
||||||
|
|
@ -63,7 +68,7 @@
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 3.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 360.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
|
|
|
||||||
|
|
@ -68,7 +68,7 @@
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 3.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 360.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
|
|
|
||||||
|
|
@ -72,7 +72,7 @@
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 3.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 360.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
|
|
|
||||||
|
|
@ -6,7 +6,7 @@
|
||||||
"autoFolders": [],
|
"autoFolders": [],
|
||||||
"defaultMaxVel": 3.0,
|
"defaultMaxVel": 3.0,
|
||||||
"defaultMaxAccel": 3.0,
|
"defaultMaxAccel": 3.0,
|
||||||
"defaultMaxAngVel": 360.0,
|
"defaultMaxAngVel": 540.0,
|
||||||
"defaultMaxAngAccel": 720.0,
|
"defaultMaxAngAccel": 720.0,
|
||||||
"defaultNominalVoltage": 12.0,
|
"defaultNominalVoltage": 12.0,
|
||||||
"robotMass": 44.4521,
|
"robotMass": 44.4521,
|
||||||
|
|
|
||||||
|
|
@ -1,42 +1,38 @@
|
||||||
// Copyright (c) FIRST and other WPILib contributors.
|
// Copyright (c) FIRST and other WPILib contributors.
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
// 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.
|
// the WPILib BSD license file in the root directory of this project.
|
||||||
package frc.robot;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.signals.InvertedValue;
|
package frc.robot;
|
||||||
|
|
||||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||||
import edu.wpi.first.apriltag.AprilTagFields;
|
import edu.wpi.first.apriltag.AprilTagFields;
|
||||||
import edu.wpi.first.math.geometry.Pose3d;
|
import edu.wpi.first.math.geometry.Pose3d;
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.math.geometry.Rotation3d;
|
|
||||||
import edu.wpi.first.math.geometry.Transform2d;
|
|
||||||
import edu.wpi.first.math.geometry.Transform3d;
|
import edu.wpi.first.math.geometry.Transform3d;
|
||||||
|
import edu.wpi.first.math.geometry.Transform2d;
|
||||||
|
import edu.wpi.first.units.Units;
|
||||||
import edu.wpi.first.units.measure.Angle;
|
import edu.wpi.first.units.measure.Angle;
|
||||||
|
|
||||||
|
import com.ctre.phoenix6.signals.InvertedValue;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The Constants class provides a convenient place for teams to hold robot-wide
|
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||||
* numerical or boolean constants. This class should not be used for any other
|
* constants. This class should not be used for any other purpose. All constants should be declared
|
||||||
* purpose. All constants should be declared globally (i.e. public static). Do
|
* globally (i.e. public static). Do not put anything functional in this class.
|
||||||
* not put anything functional in this class.
|
|
||||||
*
|
*
|
||||||
* <p>
|
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||||
* It is advised to statically import this class (or one of its inner classes)
|
* constants are needed, to reduce verbosity.
|
||||||
* wherever the constants are needed, to reduce verbosity.
|
|
||||||
*/
|
*/
|
||||||
public final class Constants {
|
public final class Constants {
|
||||||
|
|
||||||
public static class OperatorConstants {
|
public static class OperatorConstants {
|
||||||
|
|
||||||
public static final int kDriverControllerPort = 0;
|
public static final int kDriverControllerPort = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class CameraConstanst {
|
public static class CameraConstanst {
|
||||||
|
|
||||||
public static class IDFilters {
|
public static class IDFilters {
|
||||||
|
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 };
|
||||||
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
|
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 };
|
||||||
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public static final String BACK_CAMERA_NAME = "limelight-back";
|
public static final String BACK_CAMERA_NAME = "limelight-back";
|
||||||
|
|
@ -46,21 +42,11 @@ public final class Constants {
|
||||||
|
|
||||||
public static final String FRONT_CAMERA_NAME = "limelight-front";
|
public static final String FRONT_CAMERA_NAME = "limelight-front";
|
||||||
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
|
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
|
||||||
public static final double[] BLUE_ZONE_COOR = new double[]{0, 3.5};
|
|
||||||
public static final double[] RED_ZONE_COOR = new double[]{13, 16};
|
|
||||||
public static final double[] NEUTRAL_ZONE_COOR = new double[]{5.5, 11};
|
|
||||||
public static final double[] NEUTRAL1_ZONE_COOR = new double[]{5.5, 11, 0, 3};
|
|
||||||
public static final double[] NEUTRAL2_ZONE_COOR = new double[]{5.5, 11, 5, 8};
|
|
||||||
public static final double[] BLUE1_ZONE_COOR = new double[]{0, 3.5, 0, 3};
|
|
||||||
public static final double[] BLUE2_ZONE_COOR = new double[]{0, 3.5, 5, 8};
|
|
||||||
public static final double[] RED1_ZONE_COOR = new double[]{13, 16, 0, 3};
|
|
||||||
public static final double[] RED2_ZONE_COOR = new double[]{13, 16, 5, 8};
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class AdvantageKitConstants {
|
public static class AdvantageKitConstants{
|
||||||
|
public enum RobotType{
|
||||||
public enum RobotType {
|
|
||||||
SIM,
|
SIM,
|
||||||
REAL,
|
REAL,
|
||||||
REPLAY
|
REPLAY
|
||||||
|
|
@ -68,15 +54,12 @@ public final class Constants {
|
||||||
public static final RobotType CURRENT_MODE = RobotType.REAL;
|
public static final RobotType CURRENT_MODE = RobotType.REAL;
|
||||||
}
|
}
|
||||||
// Swerve offsets as of 3/2/26
|
// Swerve offsets as of 3/2/26
|
||||||
|
|
||||||
public static class SwerveDriveOffsets {
|
public static class SwerveDriveOffsets {
|
||||||
|
|
||||||
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
|
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
|
||||||
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
|
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
|
||||||
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
|
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
|
||||||
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
|
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
|
* Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
|
||||||
* Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
|
* Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
|
||||||
|
|
@ -86,7 +69,6 @@ public final class Constants {
|
||||||
* Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
|
* Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
|
||||||
*/
|
*/
|
||||||
public static class SwerveDriveIDs {
|
public static class SwerveDriveIDs {
|
||||||
|
|
||||||
public static final int FRONT_LEFT_STEER_ID = 11;
|
public static final int FRONT_LEFT_STEER_ID = 11;
|
||||||
public static final int FRONT_LEFT_DRIVE_ID = 12;
|
public static final int FRONT_LEFT_DRIVE_ID = 12;
|
||||||
public static final int FRONT_LEFT_ENCODER_ID = 13;
|
public static final int FRONT_LEFT_ENCODER_ID = 13;
|
||||||
|
|
@ -101,9 +83,8 @@ public final class Constants {
|
||||||
public static final int BACK_RIGHT_ENCODER_ID = 43;
|
public static final int BACK_RIGHT_ENCODER_ID = 43;
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class RobotChassisLimits {
|
public static class RobotChassisLimits{
|
||||||
// Max velocity in meters per second
|
// Max velocity in meters per second
|
||||||
|
|
||||||
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
|
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
|
||||||
// In meters per second squared
|
// In meters per second squared
|
||||||
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
|
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
|
||||||
|
|
@ -116,12 +97,10 @@ public final class Constants {
|
||||||
|
|
||||||
//Bird
|
//Bird
|
||||||
public static class ExtraIDEntities {
|
public static class ExtraIDEntities {
|
||||||
|
|
||||||
public static final int PIGEON_ID = 1;
|
public static final int PIGEON_ID = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class IntakeConstants {
|
public static class IntakeConstants {
|
||||||
|
|
||||||
public static final int DEPLOY_MOTOR_ID = 17;
|
public static final int DEPLOY_MOTOR_ID = 17;
|
||||||
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
|
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
|
||||||
|
|
||||||
|
|
@ -130,7 +109,6 @@ public final class Constants {
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class ShooterConstants {
|
public static class ShooterConstants {
|
||||||
|
|
||||||
public static final int LAUNCHER_MOTOR_ID = 3;
|
public static final int LAUNCHER_MOTOR_ID = 3;
|
||||||
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
|
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
|
||||||
|
|
||||||
|
|
@ -139,7 +117,7 @@ public final class Constants {
|
||||||
public static final int TURRET_LIMIT_CHANNEL = 9;
|
public static final int TURRET_LIMIT_CHANNEL = 9;
|
||||||
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
|
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
|
||||||
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
|
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
|
||||||
public static final double HOMING_SPEED = .05;
|
public static final double HOMING_SPEED = .35;
|
||||||
public static final double TURRET_GEAR_RATIO = 6;
|
public static final double TURRET_GEAR_RATIO = 6;
|
||||||
|
|
||||||
public static final int HOOD_SERVO_CHANNEL = 0;
|
public static final int HOOD_SERVO_CHANNEL = 0;
|
||||||
|
|
@ -151,12 +129,11 @@ public final class Constants {
|
||||||
public static final double DEFLECTOR_STORED_ANGLE = 0;
|
public static final double DEFLECTOR_STORED_ANGLE = 0;
|
||||||
public static final double DEFLECTOR_SET_ANGLE1 = 100;
|
public static final double DEFLECTOR_SET_ANGLE1 = 100;
|
||||||
public static final double DEFLECTOR_SET_ANGLE2 = 200;
|
public static final double DEFLECTOR_SET_ANGLE2 = 200;
|
||||||
public static final double DEFLECTOR_SET_ANGLE3 = 300;
|
public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
|
||||||
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
|
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class HopperConstants {
|
public static class HopperConstants {
|
||||||
|
|
||||||
public static final int KICKER_MOTOR_ID = 9;
|
public static final int KICKER_MOTOR_ID = 9;
|
||||||
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
|
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
|
||||||
|
|
||||||
|
|
@ -165,56 +142,10 @@ public final class Constants {
|
||||||
}
|
}
|
||||||
|
|
||||||
public static class FieldPoses {
|
public static class FieldPoses {
|
||||||
|
|
||||||
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
|
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
|
||||||
|
|
||||||
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
||||||
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
||||||
|
|
||||||
// These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side.
|
|
||||||
// Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
|
|
||||||
// Rotation are zero because they don't matter.
|
|
||||||
public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
|
|
||||||
public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 1.72, 0, new Rotation3d());
|
|
||||||
public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
|
|
||||||
public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
|
|
||||||
|
|
||||||
public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d());
|
|
||||||
public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d());
|
|
||||||
public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
|
|
||||||
public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// These represent the dividing lines for the regions in our shooting strategy.
|
|
||||||
public static class ShootingRegionDimensions {
|
|
||||||
|
|
||||||
public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
|
|
||||||
public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0;
|
|
||||||
public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5;
|
|
||||||
public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0;
|
|
||||||
public static double BLUE_RIGHT_REGION_Y_MAX = 3.0;
|
|
||||||
public static double RED_LEFT_REGION_Y_MAX = 3.0; // same meaning as the previous one, but from red perspective
|
|
||||||
public static double BLUE_LEFT_REGION_Y_MIN = 5.0;
|
|
||||||
public static double RED_RIGHT_REGION_Y_MIN = 5.0; // same meaning as the previous one, but from red perspective
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Regions that define the AutoTurret shooting behaviors.
|
|
||||||
// Color ID in the name reflects robot's alliance since region definitions vary by alliance.
|
|
||||||
// Left/Right are from driver's perspective.
|
|
||||||
public enum ShootingRegion {
|
|
||||||
NON_SHOOTING_REGION, // Any part of the field that's not in one of the regions
|
|
||||||
BLUE_OWN_ALLIANCE_ZONE,
|
|
||||||
BLUE_LEFT_NEUTRAL_ZONE,
|
|
||||||
BLUE_RIGHT_NEUTRAL_ZONE,
|
|
||||||
BLUE_LEFT_OPPONENT_ALLIANCE_ZONE,
|
|
||||||
BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE,
|
|
||||||
RED_OWN_ALLIANCE_ZONE,
|
|
||||||
RED_LEFT_NEUTRAL_ZONE,
|
|
||||||
RED_RIGHT_NEUTRAL_ZONE,
|
|
||||||
RED_LEFT_OPPONENT_ALLIANCE_ZONE,
|
|
||||||
RED_RIGHT_OPPONENT_ALLIANCE_ZONE
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -14,8 +14,6 @@ import org.littletonrobotics.junction.Logger;
|
||||||
import org.littletonrobotics.junction.networktables.NT4Publisher;
|
import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||||
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModule;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.Filesystem;
|
import edu.wpi.first.wpilibj.Filesystem;
|
||||||
import edu.wpi.first.wpilibj.PowerDistribution;
|
import edu.wpi.first.wpilibj.PowerDistribution;
|
||||||
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
|
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
|
||||||
|
|
@ -92,9 +90,6 @@ public class Robot extends LoggedRobot {
|
||||||
|
|
||||||
// Run all POST tests after robot has been initialized
|
// Run all POST tests after robot has been initialized
|
||||||
PostyManager.getInstance().runAllPOSTs();
|
PostyManager.getInstance().runAllPOSTs();
|
||||||
|
|
||||||
|
|
||||||
m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(false);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -122,7 +117,6 @@ public class Robot extends LoggedRobot {
|
||||||
/** This function is called once each time the robot enters Disabled mode. */
|
/** This function is called once each time the robot enters Disabled mode. */
|
||||||
@Override
|
@Override
|
||||||
public void disabledInit() {
|
public void disabledInit() {
|
||||||
m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(true);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|
|
||||||
|
|
@ -1,20 +1,24 @@
|
||||||
// Copyright (c) FIRST and other WPILib contributors.
|
// Copyright (c) FIRST and other WPILib contributors.
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
// 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.
|
// the WPILib BSD license file in the root directory of this project.
|
||||||
|
|
||||||
package frc.robot;
|
package frc.robot;
|
||||||
|
|
||||||
import com.pathplanner.lib.auto.NamedCommands;
|
import com.pathplanner.lib.auto.NamedCommands;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
import edu.wpi.first.wpilibj.XboxController;
|
||||||
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.button.CommandXboxController;
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||||
import frc.robot.Constants.OperatorConstants;
|
import frc.robot.Constants.OperatorConstants;
|
||||||
import frc.robot.commands.AutoShootCommandChassisTurret;
|
import frc.robot.commands.AutoShootCommandChassisTurret;
|
||||||
|
import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
|
||||||
import frc.robot.commands.AutoTurretCommand;
|
import frc.robot.commands.AutoTurretCommand;
|
||||||
import frc.robot.commands.AutoTurretShootCommand;
|
import frc.robot.commands.ManualHopperCommand;
|
||||||
import frc.robot.commands.Reverse;
|
import frc.robot.commands.ManualIntake;
|
||||||
|
import frc.robot.commands.ManualShootCommand;
|
||||||
|
import frc.robot.commands.RelayCommand;
|
||||||
import frc.robot.commands.SetDeflectorCommand;
|
import frc.robot.commands.SetDeflectorCommand;
|
||||||
import frc.robot.subsystems.DriveSubsystem;
|
import frc.robot.subsystems.DriveSubsystem;
|
||||||
import frc.robot.subsystems.HopperSubsystem;
|
import frc.robot.subsystems.HopperSubsystem;
|
||||||
|
|
@ -22,18 +26,16 @@ import frc.robot.subsystems.IntakeSubsystem;
|
||||||
import frc.robot.subsystems.TurretSubsystem;
|
import frc.robot.subsystems.TurretSubsystem;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This class is where the bulk of the robot should be declared. Since
|
* This class is where the bulk of the robot should be declared. Since Command-based is a
|
||||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
|
||||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
|
||||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
* subsystems, commands, and trigger mappings) should be declared here.
|
||||||
* commands, and trigger mappings) should be declared here.
|
|
||||||
*/
|
*/
|
||||||
public class RobotContainer {
|
public class RobotContainer {
|
||||||
// The robot's subsystems and commands are defined here...
|
// The robot's subsystems and commands are defined here...
|
||||||
// Replace with CommandPS4Controller or CommandJoystick if needed
|
// Replace with CommandPS4Controller or CommandJoystick if needed
|
||||||
|
private final CommandXboxController m_driverController =
|
||||||
private final CommandXboxController m_driverController
|
new CommandXboxController(OperatorConstants.kDriverControllerPort);
|
||||||
= new CommandXboxController(OperatorConstants.kDriverControllerPort);
|
|
||||||
|
|
||||||
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
|
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
|
||||||
public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
|
public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
|
||||||
|
|
@ -44,49 +46,44 @@ public class RobotContainer {
|
||||||
|
|
||||||
public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
|
public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
|
||||||
|
|
||||||
/**
|
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
||||||
* The container for the robot. Contains subsystems, OI devices, and
|
|
||||||
* commands.
|
|
||||||
*/
|
|
||||||
public RobotContainer() {
|
public RobotContainer() {
|
||||||
|
|
||||||
// turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
|
// turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
|
||||||
// turretSubsystem.homeTurret();
|
// turretSubsystem.homeTurret();
|
||||||
|
|
||||||
// Configure the trigger bindings
|
// Configure the trigger bindings
|
||||||
configureBindings();
|
configureBindings();
|
||||||
|
|
||||||
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
|
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
|
||||||
NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake(m_driverController.getHID()));
|
|
||||||
NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
|
|
||||||
|
|
||||||
turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Use this method to define your trigger->command mappings. Triggers can be
|
* Use this method to define your trigger->command mappings. Triggers can be created via the
|
||||||
* created via the
|
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
|
||||||
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor
|
* predicate, or via the named factories in {@link
|
||||||
* with an arbitrary predicate, or via the named factories in {@link
|
|
||||||
* edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
|
* edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
|
||||||
* CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
|
* CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
|
||||||
* PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
|
* PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
|
||||||
* joysticks}.
|
* joysticks}.
|
||||||
*
|
* */
|
||||||
*/
|
|
||||||
private void configureBindings() {
|
private void configureBindings() {
|
||||||
// m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
|
m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
|
||||||
// m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
|
m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
|
||||||
// m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
|
m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
|
||||||
// m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
|
m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
|
||||||
|
|
||||||
|
m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
|
||||||
|
//m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
|
||||||
|
// m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
|
||||||
|
|
||||||
//m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
|
|
||||||
m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
|
m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
|
||||||
m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
|
m_driverController.rightBumper().onTrue(intakeSubsystem.intake());
|
||||||
|
m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
|
||||||
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
|
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
|
||||||
m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
|
m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
|
||||||
m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
|
|
||||||
m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
|
|
||||||
m_driverController.y().onTrue(intakeSubsystem.deploy());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -96,8 +93,6 @@ public class RobotContainer {
|
||||||
*/
|
*/
|
||||||
public Command getAutonomousCommand() {
|
public Command getAutonomousCommand() {
|
||||||
// An example command will be run in autonomous
|
// An example command will be run in autonomous
|
||||||
return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> {
|
return Commands.parallel(intakeSubsystem.deploy(), Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem), driveSubsystem.getAutoCommand());
|
||||||
turretSubsystem.homeTurret();
|
|
||||||
}, turretSubsystem), driveSubsystem.getAutoCommand());
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,15 +1,5 @@
|
||||||
package frc.robot;
|
package frc.robot;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
||||||
import frc.robot.Constants;
|
|
||||||
import frc.robot.Constants.ShootingRegion;
|
|
||||||
import frc.robot.Constants.ShootingRegionDimensions;
|
|
||||||
|
|
||||||
import org.littletonrobotics.junction.Logger;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Pose3d;
|
|
||||||
import edu.wpi.first.units.measure.Angle;
|
|
||||||
|
|
||||||
public class Utils {
|
public class Utils {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -32,142 +22,4 @@ public class Utils {
|
||||||
return robotTurretRotations;
|
return robotTurretRotations;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Determines the region for the robot based on its location and alliance.
|
|
||||||
* Assumes the robot is a 27" square. Thresholds based on observations from simulator.
|
|
||||||
* @param robotPose: the robot's pose
|
|
||||||
* @param alliance: the red/blue alliance of the robot, as a sanity check
|
|
||||||
* @return the robot's shooting region
|
|
||||||
*/
|
|
||||||
public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
|
|
||||||
ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION; // the default return value if robot isn't in a valid region
|
|
||||||
double robotX = robotPose.getX();
|
|
||||||
double robotY = robotPose.getY();
|
|
||||||
|
|
||||||
// Receommended: Add a sanity check for invalid robot poses.
|
|
||||||
|
|
||||||
if (alliance == Alliance.Blue) {
|
|
||||||
if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
|
|
||||||
region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
||||||
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
||||||
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
|
|
||||||
region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
||||||
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
||||||
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
|
|
||||||
region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
|
|
||||||
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
|
|
||||||
region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
|
|
||||||
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
|
|
||||||
region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (alliance == Alliance.Red) {
|
|
||||||
if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
|
|
||||||
region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
||||||
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
||||||
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
|
|
||||||
region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
||||||
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
||||||
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
|
|
||||||
region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
|
|
||||||
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
|
|
||||||
region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
|
|
||||||
}
|
|
||||||
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
|
|
||||||
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
|
|
||||||
region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Logger.recordOutput("Utils-ShootingRegion", region);
|
|
||||||
|
|
||||||
return region;
|
|
||||||
}
|
|
||||||
|
|
||||||
public static double getHoodAngleForDistance(double distanceInMeters){
|
|
||||||
if(distanceInMeters >= 4){
|
|
||||||
return 200;
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
public static double getLauncherRPMForDistance(double distanceInMeters){
|
|
||||||
return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Determines the appropriate shooting target for the robot based on its alliance
|
|
||||||
* and its current region on the field. There are 10 possible targets:
|
|
||||||
* Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
|
|
||||||
* and these exist for both red and blue alliances.
|
|
||||||
* Right/left are from driver's perspective.
|
|
||||||
* @param region: the identifier of the robot's current field region
|
|
||||||
* @param alliance: the red/blue alliance of the robot, as a sanity check
|
|
||||||
* @return the shooting target for a robot with the provided properties. Returns all-zero
|
|
||||||
* Pose3d object if the region/alliance combination is invalid.
|
|
||||||
*/
|
|
||||||
public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
|
|
||||||
|
|
||||||
Pose3d shootingTarget = new Pose3d();
|
|
||||||
if (alliance == Alliance.Blue) {
|
|
||||||
switch (region) {
|
|
||||||
case BLUE_OWN_ALLIANCE_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
|
|
||||||
break;
|
|
||||||
case BLUE_LEFT_NEUTRAL_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
case BLUE_RIGHT_NEUTRAL_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if (alliance == Alliance.Red) {
|
|
||||||
switch (region) {
|
|
||||||
case RED_OWN_ALLIANCE_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
|
|
||||||
break;
|
|
||||||
case RED_LEFT_NEUTRAL_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
case RED_RIGHT_NEUTRAL_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
|
|
||||||
shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return shootingTarget;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -75,7 +75,7 @@ public class AutoShootCommandChassisTurret extends Command{
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
|
|
||||||
turretSubsystem.setLauncher(3000);
|
turretSubsystem.setLauncher(6000);
|
||||||
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
|
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
|
||||||
hopperSubsystem.spindexerOn();
|
hopperSubsystem.spindexerOn();
|
||||||
cameraAutoCenterController.calculate(-100);
|
cameraAutoCenterController.calculate(-100);
|
||||||
|
|
|
||||||
|
|
@ -17,8 +17,6 @@ import edu.wpi.first.wpilibj.DriverStation;
|
||||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import frc.robot.Constants;
|
import frc.robot.Constants;
|
||||||
import frc.robot.Constants.ShootingRegion;
|
|
||||||
import frc.robot.Utils;
|
|
||||||
import frc.robot.subsystems.DriveSubsystem;
|
import frc.robot.subsystems.DriveSubsystem;
|
||||||
import frc.robot.subsystems.TurretSubsystem;
|
import frc.robot.subsystems.TurretSubsystem;
|
||||||
|
|
||||||
|
|
@ -29,7 +27,10 @@ public class AutoTurretCommand extends Command {
|
||||||
private TurretSubsystem turretSubsystem;
|
private TurretSubsystem turretSubsystem;
|
||||||
private DriveSubsystem driveSubsystem;
|
private DriveSubsystem driveSubsystem;
|
||||||
|
|
||||||
private Alliance alliance;
|
private int[] shootingTagFilters;
|
||||||
|
|
||||||
|
private Pose3d closestTag;
|
||||||
|
private Pose3d scoringHubPose;
|
||||||
|
|
||||||
public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
|
public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
|
||||||
this.turretSubsystem = turretSubsystem;
|
this.turretSubsystem = turretSubsystem;
|
||||||
|
|
@ -45,12 +46,13 @@ public class AutoTurretCommand extends Command {
|
||||||
|
|
||||||
DriverStation.getAlliance().ifPresent((aliance) -> {
|
DriverStation.getAlliance().ifPresent((aliance) -> {
|
||||||
if (aliance == Alliance.Blue) {
|
if (aliance == Alliance.Blue) {
|
||||||
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
|
shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
|
||||||
aliance = Alliance.Blue;
|
scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE;
|
||||||
} else {
|
} else {
|
||||||
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
|
shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
|
||||||
aliance = Alliance.Red;
|
scoringHubPose = Constants.FieldPoses.RED_HUB_POSE;
|
||||||
}
|
}
|
||||||
|
Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
|
||||||
});
|
});
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
@ -58,28 +60,18 @@ public class AutoTurretCommand extends Command {
|
||||||
@Override
|
@Override
|
||||||
public void execute() {
|
public void execute() {
|
||||||
|
|
||||||
ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), DriverStation.getAlliance().get());
|
|
||||||
|
|
||||||
if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){
|
|
||||||
Pose3d targetPose = Utils.findShootingTarget(shootingRegion, DriverStation.getAlliance().get());
|
|
||||||
|
|
||||||
turretSubsystem.setTurretTargetPose(targetPose);
|
|
||||||
}
|
|
||||||
|
|
||||||
Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
|
|
||||||
|
|
||||||
Pose3d botPose = driveSubsystem.getBotPose();
|
Pose3d botPose = driveSubsystem.getBotPose();
|
||||||
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
|
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
|
||||||
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
|
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative()))));
|
||||||
Logger.recordOutput("AutoTurret-TurretPose", turretPose);
|
Logger.recordOutput("AutoTurret-TurretPose", turretPose);
|
||||||
|
|
||||||
Translation2d targetTranslation = turretSubsystem.getTurretTargetPose().getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
|
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
|
||||||
Logger.recordOutput("AutoTurret-Translation", targetTranslation);
|
Logger.recordOutput("AutoTurret-Translation", targetTranslation);
|
||||||
|
|
||||||
|
|
||||||
if(turretSubsystem.isTurretReady()){
|
if(turretSubsystem.isTurretReady()){
|
||||||
turretSubsystem.setTurretFieldRelative(botPose, targetTranslation.getAngle().getRotations());
|
// turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
|
||||||
// turretSubsystem.setTurretRobotRelative(0.25);
|
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
|
||||||
// turretSubsystem.setTurretMotorRotation(0);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,90 +0,0 @@
|
||||||
package frc.robot.commands;
|
|
||||||
|
|
||||||
import org.littletonrobotics.junction.Logger;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Pose3d;
|
|
||||||
import edu.wpi.first.units.Units;
|
|
||||||
import edu.wpi.first.units.measure.Angle;
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Commands;
|
|
||||||
import frc.robot.Constants;
|
|
||||||
import frc.robot.Utils;
|
|
||||||
import frc.robot.subsystems.DriveSubsystem;
|
|
||||||
import frc.robot.subsystems.HopperSubsystem;
|
|
||||||
import frc.robot.subsystems.IntakeSubsystem;
|
|
||||||
import frc.robot.subsystems.TurretSubsystem;
|
|
||||||
|
|
||||||
public class AutoTurretShootCommand extends Command{
|
|
||||||
|
|
||||||
private HopperSubsystem hopperSubsystem;
|
|
||||||
private TurretSubsystem turretSubsystem;
|
|
||||||
private IntakeSubsystem intakeSubsystem;
|
|
||||||
private DriveSubsystem driveSubsystem;
|
|
||||||
|
|
||||||
private Command waitCommand = Commands.waitSeconds(1.5);
|
|
||||||
|
|
||||||
private Alliance alliance;
|
|
||||||
public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
|
|
||||||
this.hopperSubsystem = hopperSubsystem;
|
|
||||||
this.turretSubsystem = turretSubsystem;
|
|
||||||
this.intakeSubsystem = intakeSubsystem;
|
|
||||||
this.driveSubsystem = driveSubsystem;
|
|
||||||
|
|
||||||
addRequirements(hopperSubsystem, intakeSubsystem);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
hopperSubsystem.spindexerOn();
|
|
||||||
CommandScheduler.getInstance().schedule(waitCommand);
|
|
||||||
alliance = DriverStation.getAlliance().get();
|
|
||||||
Logger.recordOutput("AutoTurretShoot-State", "SHOOTING");
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d());
|
|
||||||
Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance);
|
|
||||||
|
|
||||||
turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(distance));
|
|
||||||
turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(distance));
|
|
||||||
|
|
||||||
if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
|
|
||||||
hopperSubsystem.feedKicker();
|
|
||||||
}else{
|
|
||||||
hopperSubsystem.stopKicker();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
waitCommand.cancel();
|
|
||||||
hopperSubsystem.spindexerOff();
|
|
||||||
hopperSubsystem.stopKicker();
|
|
||||||
|
|
||||||
turretSubsystem.setLauncher(0);
|
|
||||||
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
|
|
||||||
|
|
||||||
Logger.recordOutput("AutoTurretShoot-State", "NOT SHOOTING");
|
|
||||||
|
|
||||||
DriverStation.getAlliance().ifPresent((aliance) -> {
|
|
||||||
if (aliance == Alliance.Blue) {
|
|
||||||
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
|
|
||||||
} else {
|
|
||||||
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
|
|
||||||
// turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
@ -0,0 +1,58 @@
|
||||||
|
// 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 frc.robot.subsystems.HopperSubsystem;
|
||||||
|
import edu.wpi.first.wpilibj.XboxController;
|
||||||
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
|
|
||||||
|
/** An example command that uses an example subsystem. */
|
||||||
|
public class ManualHopperCommand extends Command {
|
||||||
|
private final HopperSubsystem m_subsystem;
|
||||||
|
private final XboxController m_xboxController;
|
||||||
|
|
||||||
|
public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) {
|
||||||
|
m_subsystem = subsystem;
|
||||||
|
m_xboxController = xboxController;
|
||||||
|
addRequirements(subsystem);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Called when the command is initially scheduled.
|
||||||
|
@Override
|
||||||
|
public void initialize() {
|
||||||
|
m_subsystem.spindexerOn();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Called every time the scheduler runs while the command is scheduled.
|
||||||
|
@Override
|
||||||
|
public void execute() {
|
||||||
|
if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
|
||||||
|
{
|
||||||
|
m_subsystem.feedKicker();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_subsystem.stopKicker();
|
||||||
|
}
|
||||||
|
if(m_xboxController.getRightBumper())
|
||||||
|
{
|
||||||
|
m_subsystem.spindexerOn();
|
||||||
|
}
|
||||||
|
if(m_xboxController.getLeftBumper())
|
||||||
|
{
|
||||||
|
m_subsystem.spindexerOff();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,52 @@
|
||||||
|
// 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 frc.robot.subsystems.IntakeSubsystem;
|
||||||
|
|
||||||
|
import edu.wpi.first.wpilibj.XboxController;
|
||||||
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
|
|
||||||
|
/** An example command that uses an example subsystem. */
|
||||||
|
public class ManualIntake extends Command {
|
||||||
|
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
|
||||||
|
private final IntakeSubsystem m_subsystem;
|
||||||
|
private final XboxController m_XboxController;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new ExampleCommand.
|
||||||
|
*
|
||||||
|
* @param subsystem The subsystem used by this command.
|
||||||
|
*/
|
||||||
|
public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) {
|
||||||
|
m_subsystem = subsystem;
|
||||||
|
m_XboxController = xboxController;
|
||||||
|
// Use addRequirements() here to declare subsystem dependencies.
|
||||||
|
addRequirements(subsystem);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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(m_XboxController.getYButton()==true)
|
||||||
|
{
|
||||||
|
m_subsystem.deploy();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,48 @@
|
||||||
|
// 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 frc.robot.subsystems.TurretSubsystem;
|
||||||
|
import edu.wpi.first.wpilibj.XboxController;
|
||||||
|
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
||||||
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
|
|
||||||
|
/** An example command that uses an example subsystem. */
|
||||||
|
public class ManualShootCommand extends Command {
|
||||||
|
private final TurretSubsystem m_subsystem;
|
||||||
|
private final XboxController m_controller;
|
||||||
|
|
||||||
|
public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) {
|
||||||
|
m_subsystem = subsystem;
|
||||||
|
m_controller = controller;
|
||||||
|
// Use addRequirements() here to declare subsystem dependencies.
|
||||||
|
addRequirements(subsystem);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Called every time the scheduler runs while the command is scheduled.
|
||||||
|
@Override
|
||||||
|
public void execute() {
|
||||||
|
if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0)
|
||||||
|
{
|
||||||
|
m_subsystem.turretRotateDir(-.5);
|
||||||
|
m_controller.setRumble(RumbleType.kRightRumble, 1);
|
||||||
|
}
|
||||||
|
else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0)
|
||||||
|
{
|
||||||
|
m_subsystem.turretRotateDir(.5);
|
||||||
|
m_controller.setRumble(RumbleType.kLeftRumble, 1);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_subsystem.turretRotateDir(0);
|
||||||
|
m_controller.setRumble(RumbleType.kBothRumble, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean isFinished() {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,63 @@
|
||||||
|
package frc.robot.commands;
|
||||||
|
|
||||||
|
import edu.wpi.first.units.Units;
|
||||||
|
import edu.wpi.first.units.measure.Angle;
|
||||||
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
|
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||||
|
import edu.wpi.first.wpilibj2.command.Commands;
|
||||||
|
import frc.robot.Constants;
|
||||||
|
import frc.robot.subsystems.HopperSubsystem;
|
||||||
|
import frc.robot.subsystems.IntakeSubsystem;
|
||||||
|
import frc.robot.subsystems.TurretSubsystem;
|
||||||
|
|
||||||
|
public class RelayCommand extends Command{
|
||||||
|
|
||||||
|
private HopperSubsystem hopperSubsystem;
|
||||||
|
private TurretSubsystem turretSubsystem;
|
||||||
|
private IntakeSubsystem intakeSubsystem;
|
||||||
|
|
||||||
|
private Command waitCommand = Commands.waitSeconds(1.5);
|
||||||
|
|
||||||
|
public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){
|
||||||
|
this.hopperSubsystem = hopperSubsystem;
|
||||||
|
this.turretSubsystem = turretSubsystem;
|
||||||
|
this.intakeSubsystem = intakeSubsystem;
|
||||||
|
|
||||||
|
addRequirements(hopperSubsystem, turretSubsystem, intakeSubsystem);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void initialize() {
|
||||||
|
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
|
||||||
|
turretSubsystem.setLauncher(6000);
|
||||||
|
hopperSubsystem.spindexerOn();
|
||||||
|
CommandScheduler.getInstance().schedule(waitCommand);
|
||||||
|
turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void execute() {
|
||||||
|
if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
|
||||||
|
hopperSubsystem.feedKicker();
|
||||||
|
}else{
|
||||||
|
hopperSubsystem.stopKicker();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public boolean isFinished() {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void end(boolean interrupted) {
|
||||||
|
waitCommand.cancel();
|
||||||
|
hopperSubsystem.spindexerOff();
|
||||||
|
hopperSubsystem.stopKicker();
|
||||||
|
|
||||||
|
turretSubsystem.setLauncher(0);
|
||||||
|
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
|
||||||
|
turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -1,57 +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.wpilibj.XboxController;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.HopperSubsystem;
|
|
||||||
import frc.robot.subsystems.IntakeSubsystem;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* An example command that uses an example subsystem.
|
|
||||||
*/
|
|
||||||
public class Reverse extends Command {
|
|
||||||
|
|
||||||
private IntakeSubsystem intakeSubsystem;
|
|
||||||
private HopperSubsystem hopperSubsystem;
|
|
||||||
private XboxController m_XboxController;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates a new ExampleCommand.
|
|
||||||
*
|
|
||||||
* @param subsystem The subsystem used by this command.
|
|
||||||
*/
|
|
||||||
public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem, XboxController m_XboxController) {
|
|
||||||
this.intakeSubsystem = intakeSubsystem;
|
|
||||||
this.hopperSubsystem = hopperSubsystem;
|
|
||||||
this.m_XboxController = m_XboxController;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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() {
|
|
||||||
intakeSubsystem.reverseIntake();
|
|
||||||
hopperSubsystem.spindexerReverse();
|
|
||||||
hopperSubsystem.reverseKicker();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
hopperSubsystem.stopKicker();
|
|
||||||
hopperSubsystem.spindexerOff();
|
|
||||||
intakeSubsystem.stopIntake(m_XboxController);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -1,86 +0,0 @@
|
||||||
package frc.robot.commands;
|
|
||||||
|
|
||||||
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
|
|
||||||
import org.littletonrobotics.junction.Logger;
|
|
||||||
|
|
||||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
|
||||||
import edu.wpi.first.apriltag.AprilTagFields;
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.Constants;
|
|
||||||
import frc.robot.subsystems.DriveSubsystem;
|
|
||||||
|
|
||||||
public class ZoneCheck extends Command {
|
|
||||||
private Zone zone;
|
|
||||||
private DriveSubsystem driveSubsystem;
|
|
||||||
|
|
||||||
public ZoneCheck(DriveSubsystem driveSubsystem)
|
|
||||||
{
|
|
||||||
this.driveSubsystem = driveSubsystem;
|
|
||||||
}
|
|
||||||
|
|
||||||
public enum Zone {
|
|
||||||
Blue,
|
|
||||||
Red,
|
|
||||||
Neutral,
|
|
||||||
INIT;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
zone = Zone.INIT;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
getZone();
|
|
||||||
Logger.recordOutput("FieldZone", zone);
|
|
||||||
DriverStation.getAlliance().ifPresent((aliance) -> {
|
|
||||||
if (aliance == Alliance.Blue) {
|
|
||||||
switch (zone) {
|
|
||||||
case Blue:
|
|
||||||
System.out.println("BLUE");
|
|
||||||
break;
|
|
||||||
case Neutral:
|
|
||||||
System.out.println("NEUTRAL");
|
|
||||||
break;
|
|
||||||
case Red:
|
|
||||||
System.out.println("RED");
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
System.out.println("****");
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
switch (zone) {
|
|
||||||
case Blue:
|
|
||||||
System.out.println("BLUE");
|
|
||||||
break;
|
|
||||||
case Neutral:
|
|
||||||
System.out.println("NEUTRAL");
|
|
||||||
break;
|
|
||||||
case Red:
|
|
||||||
System.out.println("RED");
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
System.out.println("****");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
private void getZone() {
|
|
||||||
if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
|
|
||||||
zone = Zone.Red;
|
|
||||||
} else if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
|
|
||||||
zone = Zone.Neutral;
|
|
||||||
} else {
|
|
||||||
zone = Zone.Blue;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -1,9 +1,8 @@
|
||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
|
|
||||||
|
|
||||||
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
||||||
import org.fairportrobotics.frc.posty.test.PostTest;
|
import org.fairportrobotics.frc.posty.test.PostTest;
|
||||||
|
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
|
||||||
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
|
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
|
||||||
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
|
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
|
||||||
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
|
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
|
||||||
|
|
@ -11,10 +10,11 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
|
||||||
import org.littletonrobotics.junction.Logger;
|
import org.littletonrobotics.junction.Logger;
|
||||||
|
|
||||||
import com.pathplanner.lib.auto.AutoBuilder;
|
import com.pathplanner.lib.auto.AutoBuilder;
|
||||||
|
import com.pathplanner.lib.auto.NamedCommands;
|
||||||
|
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||||
import com.pathplanner.lib.config.PIDConstants;
|
import com.pathplanner.lib.config.PIDConstants;
|
||||||
import com.pathplanner.lib.config.RobotConfig;
|
import com.pathplanner.lib.config.RobotConfig;
|
||||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||||
import com.pathplanner.lib.util.PathPlannerLogging;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.VecBuilder;
|
import edu.wpi.first.math.VecBuilder;
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
|
|
@ -22,12 +22,12 @@ import edu.wpi.first.math.geometry.Pose3d;
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
import edu.wpi.first.math.geometry.Translation2d;
|
||||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
import edu.wpi.first.wpilibj.DriverStation;
|
||||||
import edu.wpi.first.wpilibj.RobotState;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
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.button.CommandXboxController;
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||||
|
|
||||||
import frc.robot.Constants;
|
import frc.robot.Constants;
|
||||||
|
|
||||||
public class DriveSubsystem extends TestableSubsystem {
|
public class DriveSubsystem extends TestableSubsystem {
|
||||||
|
|
@ -35,7 +35,7 @@ public class DriveSubsystem extends TestableSubsystem {
|
||||||
private SendableChooser<Command> autoChooser;
|
private SendableChooser<Command> autoChooser;
|
||||||
|
|
||||||
private SwerveBuilder swerveBuilder = new SwerveBuilder();
|
private SwerveBuilder swerveBuilder = new SwerveBuilder();
|
||||||
public SwerveDriveSystem driveSystem;
|
private SwerveDriveSystem driveSystem;
|
||||||
|
|
||||||
private double driveP = 0.1;
|
private double driveP = 0.1;
|
||||||
private double driveI = 0.0;
|
private double driveI = 0.0;
|
||||||
|
|
@ -135,8 +135,9 @@ public class DriveSubsystem extends TestableSubsystem {
|
||||||
.build())
|
.build())
|
||||||
.build();
|
.build();
|
||||||
|
|
||||||
|
|
||||||
// Set vision measurement confidence values
|
// Set vision measurement confidence values
|
||||||
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.1));
|
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));
|
||||||
|
|
||||||
this.setDefaultCommand(Commands.run(new Runnable() {
|
this.setDefaultCommand(Commands.run(new Runnable() {
|
||||||
|
|
||||||
|
|
@ -149,6 +150,7 @@ public class DriveSubsystem extends TestableSubsystem {
|
||||||
|
|
||||||
Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
|
Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
|
||||||
Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
|
Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
|
||||||
|
Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());
|
||||||
}
|
}
|
||||||
|
|
||||||
}, this));
|
}, this));
|
||||||
|
|
@ -161,10 +163,10 @@ public class DriveSubsystem extends TestableSubsystem {
|
||||||
driveSystem::getRobotPose2d,
|
driveSystem::getRobotPose2d,
|
||||||
driveSystem::setPose2d,
|
driveSystem::setPose2d,
|
||||||
driveSystem::getRobotRelativeSpeeds,
|
driveSystem::getRobotRelativeSpeeds,
|
||||||
(speeds, feedforwards) -> {driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(-speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond, driveSystem.getRobotHeading()), new Translation2d());},
|
(speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
|
||||||
new PPHolonomicDriveController(
|
new PPHolonomicDriveController(
|
||||||
new PIDConstants(5, 0, 0),
|
new PIDConstants(1.5, 0, 0),
|
||||||
new PIDConstants(Math.PI, 0, 0)),
|
new PIDConstants(Math.PI/2, 0, 0)),
|
||||||
config,
|
config,
|
||||||
() -> {
|
() -> {
|
||||||
var alliance = DriverStation.getAlliance();
|
var alliance = DriverStation.getAlliance();
|
||||||
|
|
@ -179,15 +181,10 @@ public class DriveSubsystem extends TestableSubsystem {
|
||||||
autoChooser.setDefaultOption("Do Nothing", Commands.none());
|
autoChooser.setDefaultOption("Do Nothing", Commands.none());
|
||||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||||
|
|
||||||
// PathPlannerLogging.setLogActivePathCallback((List<Pose2d> pose) -> {});
|
|
||||||
PathPlannerLogging.setLogTargetPoseCallback((Pose2d targetPose) -> { Logger.recordOutput("PathPlanner-TargetPose", targetPose);});
|
|
||||||
|
|
||||||
} catch (Exception e) {
|
} catch (Exception e) {
|
||||||
e.printStackTrace();
|
e.printStackTrace();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|
@ -197,45 +194,16 @@ public class DriveSubsystem extends TestableSubsystem {
|
||||||
|
|
||||||
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
|
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
|
||||||
|
|
||||||
LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.FRONT_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
|
LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
|
||||||
LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.BACK_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
|
if(frontCameraPose.tagCount >= 1){
|
||||||
|
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
|
||||||
// MegaTag 2 impl
|
Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
|
||||||
if(RobotState.isDisabled()){
|
|
||||||
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1);
|
|
||||||
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1);
|
|
||||||
}else{
|
|
||||||
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4);
|
|
||||||
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4);
|
|
||||||
}
|
}
|
||||||
|
LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
|
||||||
LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME);
|
if(backCameraPose.tagCount >= 1){
|
||||||
LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME);
|
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
|
||||||
|
Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
|
||||||
if(frontTag.tagCount >= 2){
|
|
||||||
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds);
|
|
||||||
Logger.recordOutput("FrontPose-Estimate", frontTag.pose);
|
|
||||||
Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if(backTag.tagCount >= 2){
|
|
||||||
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds);
|
|
||||||
Logger.recordOutput("BackPose-Estimate", backTag.pose);
|
|
||||||
Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// MegaTag 1 Impl
|
|
||||||
// LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
|
|
||||||
// if (frontCameraPose.tagCount >= 2) {
|
|
||||||
// driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
|
|
||||||
// Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
|
|
||||||
// }
|
|
||||||
// LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
|
|
||||||
// if (backCameraPose.tagCount >= 2) {
|
|
||||||
// driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
|
|
||||||
// Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
|
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|
@ -244,15 +212,15 @@ public class DriveSubsystem extends TestableSubsystem {
|
||||||
driveSystem.simulationPeriodic();
|
driveSystem.simulationPeriodic();
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command getAutoCommand() {
|
public Command getAutoCommand(){
|
||||||
return autoChooser.getSelected();
|
return autoChooser.getSelected();
|
||||||
}
|
}
|
||||||
|
|
||||||
public void stopDrive() {
|
public void stopDrive(){
|
||||||
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
|
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
|
||||||
}
|
}
|
||||||
|
|
||||||
public void rotateChassis(double rotSpeed) {
|
public void rotateChassis(double rotSpeed){
|
||||||
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
|
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,112 +1,46 @@
|
||||||
// Copyright (c) FIRST and other WPILib contributors.
|
// Copyright (c) FIRST and other WPILib contributors.
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
// 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.
|
// the WPILib BSD license file in the root directory of this project.
|
||||||
|
|
||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
||||||
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
|
|
||||||
import org.fairportrobotics.frc.posty.test.PostTest;
|
import org.fairportrobotics.frc.posty.test.PostTest;
|
||||||
import org.littletonrobotics.junction.Logger;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.StatusSignal;
|
import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
|
||||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
|
||||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
|
||||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
|
||||||
import com.ctre.phoenix6.hardware.TalonFX;
|
import com.ctre.phoenix6.hardware.TalonFX;
|
||||||
|
|
||||||
import edu.wpi.first.units.Units;
|
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
|
||||||
import edu.wpi.first.units.measure.AngularVelocity;
|
|
||||||
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
|
||||||
import edu.wpi.first.wpilibj.XboxController;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.Constants;
|
import frc.robot.Constants;
|
||||||
|
|
||||||
public class HopperSubsystem extends TestableSubsystem {
|
public class HopperSubsystem extends TestableSubsystem {
|
||||||
|
|
||||||
private TalonFX spindexerMotor;
|
private TalonFX spindexerMotor;
|
||||||
|
|
||||||
private TalonFX kickerMotor;
|
private TalonFX kickerMotor;
|
||||||
private VelocityVoltage kickerControl = new VelocityVoltage(0);
|
/** Creates a new HopperSubsystem. */
|
||||||
private StatusSignal<AngularVelocity> kickerVelocity;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates a new HopperSubsystem.
|
|
||||||
*/
|
|
||||||
public HopperSubsystem() {
|
public HopperSubsystem() {
|
||||||
spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
|
spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
|
||||||
spindexerMotor.getConfigurator().apply(
|
spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION));
|
||||||
new TalonFXConfiguration()
|
// spindexerMotor.getConfigurator().apply(new ClosedLoopGeneralConfigs()
|
||||||
.withMotorOutput(
|
// .);
|
||||||
new MotorOutputConfigs()
|
|
||||||
.withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
|
|
||||||
.withSlot0(
|
|
||||||
new Slot0Configs()
|
|
||||||
.withKP(0)
|
|
||||||
.withKI(0)
|
|
||||||
.withKD(0)
|
|
||||||
.withKS(0)));
|
|
||||||
|
|
||||||
kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
|
kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
|
||||||
kickerMotor.getConfigurator().apply(
|
kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION));
|
||||||
new TalonFXConfiguration()
|
|
||||||
.withMotorOutput(
|
|
||||||
new MotorOutputConfigs()
|
|
||||||
.withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
|
|
||||||
)
|
|
||||||
.withSlot0(new Slot0Configs()
|
|
||||||
.withKP(0)
|
|
||||||
.withKI(0)
|
|
||||||
.withKD(0)
|
|
||||||
.withKS(0)
|
|
||||||
));
|
|
||||||
kickerVelocity = kickerMotor.getVelocity();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void feedKicker() {
|
public void feedKicker() {kickerMotor.set(0.75);}
|
||||||
//kickerMotor.setControl(kickerControl.withVelocity(1000));
|
|
||||||
kickerMotor.set(0.75);
|
|
||||||
}
|
|
||||||
|
|
||||||
public void reverseKicker() {
|
public void reverseKicker() {kickerMotor.set(-0.5);}
|
||||||
//kickerMotor.setControl(kickerControl.withVelocity(-500));
|
|
||||||
kickerMotor.set(-0.5);
|
|
||||||
}
|
|
||||||
|
|
||||||
public void stopKicker() {
|
public void stopKicker() {kickerMotor.stopMotor();}
|
||||||
kickerMotor.stopMotor();
|
|
||||||
}
|
|
||||||
|
|
||||||
public void spindexerOn() {
|
public void spindexerOn() { spindexerMotor.set(0.75);}
|
||||||
spindexerMotor.set(0.75);
|
|
||||||
}
|
|
||||||
|
|
||||||
public void spindexerReverse() {
|
public void spindexerOff() { spindexerMotor.stopMotor();}
|
||||||
spindexerMotor.set(-0.75);
|
|
||||||
}
|
|
||||||
|
|
||||||
public void spindexerOff() {
|
|
||||||
spindexerMotor.stopMotor();
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command spinny() {
|
|
||||||
return this.runOnce(() -> {
|
|
||||||
if (spindexerMotor.get() == 0) {
|
|
||||||
spindexerOn();
|
|
||||||
} else {
|
|
||||||
spindexerOff();
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
|
|
||||||
}
|
|
||||||
|
|
||||||
@PostTest(enabled = true)
|
@PostTest(enabled = true)
|
||||||
public void hopperSystem_CANDevicesConnected() {
|
public void hopperSystem_CANDevicesConnected(){
|
||||||
assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
|
assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
|
||||||
assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
|
assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,34 +1,32 @@
|
||||||
// Copyright (c) FIRST and other WPILib contributors.
|
// Copyright (c) FIRST and other WPILib contributors.
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
// 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.
|
// the WPILib BSD license file in the root directory of this project.
|
||||||
|
|
||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
||||||
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
|
|
||||||
import org.fairportrobotics.frc.posty.test.PostTest;
|
import org.fairportrobotics.frc.posty.test.PostTest;
|
||||||
import static org.fairportrobotics.frc.robolib.motors.Utils.SetMotorDirection;
|
|
||||||
|
|
||||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||||
import com.ctre.phoenix6.hardware.TalonFX;
|
import com.ctre.phoenix6.hardware.TalonFX;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.XboxController;
|
|
||||||
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Commands;
|
|
||||||
import frc.robot.Constants;
|
import frc.robot.Constants;
|
||||||
|
|
||||||
public class IntakeSubsystem extends TestableSubsystem {
|
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
|
||||||
|
import static org.fairportrobotics.frc.robolib.motors.Utils.*;
|
||||||
|
|
||||||
/**
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
* Creates a new ExampleSubsystem.
|
import edu.wpi.first.wpilibj2.command.Commands;
|
||||||
*/
|
|
||||||
|
public class IntakeSubsystem extends TestableSubsystem {
|
||||||
|
/** Creates a new ExampleSubsystem. */
|
||||||
|
|
||||||
private TalonFX intakeMotor;
|
private TalonFX intakeMotor;
|
||||||
private WPI_TalonSRX deployMotor;
|
private WPI_TalonSRX deployMotor;
|
||||||
|
|
||||||
public IntakeSubsystem() {
|
public IntakeSubsystem()
|
||||||
|
{
|
||||||
intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
|
intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
|
||||||
SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
|
SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
|
||||||
|
|
||||||
|
|
@ -38,55 +36,40 @@ public class IntakeSubsystem extends TestableSubsystem {
|
||||||
deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
|
deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command deploy() {
|
public Command deploy()
|
||||||
|
{
|
||||||
return Commands.sequence(
|
return Commands.sequence(
|
||||||
Commands.deadline(
|
Commands.deadline(
|
||||||
Commands.waitSeconds(2),
|
Commands.waitSeconds(2),
|
||||||
this.runOnce(
|
this.runOnce(
|
||||||
() -> {
|
() -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
|
||||||
deployMotor.set(1);
|
|
||||||
} // Bump this speed up if intake doesn't deploy
|
|
||||||
)
|
)
|
||||||
),
|
),
|
||||||
this.runOnce(() -> deployMotor.stopMotor())
|
this.runOnce(() -> deployMotor.stopMotor())
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command resetDeploy() {
|
public Command resetDeploy(){
|
||||||
return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
|
return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
|
||||||
}
|
}
|
||||||
|
|
||||||
public void setSpeed(double speed) {
|
public void setSpeed(double speed)
|
||||||
|
{
|
||||||
intakeMotor.set(speed);
|
intakeMotor.set(speed);
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command stopIntake(XboxController m_XboxController) {
|
public Command intake()
|
||||||
return this.runOnce(() -> {
|
{
|
||||||
intakeMotor.stopMotor();
|
return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
|
||||||
m_XboxController.setRumble(RumbleType.kBothRumble, .0);
|
|
||||||
});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command intake(XboxController m_XboxController) {
|
public Command reverseIntake()
|
||||||
return this.runOnce(() -> {
|
{
|
||||||
if (intakeMotor.get() == 0) {
|
return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
|
||||||
setSpeed(-.5);
|
|
||||||
m_XboxController.setRumble(RumbleType.kBothRumble, .5);
|
|
||||||
} else {
|
|
||||||
intakeMotor.stopMotor();
|
|
||||||
m_XboxController.setRumble(RumbleType.kBothRumble, .0);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command reverseIntake() {
|
|
||||||
return this.runEnd(() -> setSpeed(.5), () -> {
|
|
||||||
intakeMotor.stopMotor();
|
|
||||||
});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@PostTest(enabled = true)
|
@PostTest(enabled = true)
|
||||||
public void IntakeSubsystem_CANDevicesConnected() {
|
public void IntakeSubsystem_CANDevicesConnected(){
|
||||||
assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
|
assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
|
||||||
assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
|
assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -7,7 +7,6 @@ package frc.robot.subsystems;
|
||||||
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
||||||
import org.fairportrobotics.frc.posty.test.PostTest;
|
import org.fairportrobotics.frc.posty.test.PostTest;
|
||||||
import org.littletonrobotics.junction.Logger;
|
import org.littletonrobotics.junction.Logger;
|
||||||
import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
|
|
||||||
|
|
||||||
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
|
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
|
||||||
|
|
||||||
|
|
@ -35,9 +34,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||||
|
|
||||||
import frc.robot.Constants;
|
import frc.robot.Constants;
|
||||||
import frc.robot.Robot;
|
|
||||||
import edu.wpi.first.math.geometry.Pose3d;
|
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
|
||||||
import edu.wpi.first.units.AngleUnit;
|
import edu.wpi.first.units.AngleUnit;
|
||||||
import edu.wpi.first.units.Units;
|
import edu.wpi.first.units.Units;
|
||||||
import edu.wpi.first.units.measure.Angle;
|
import edu.wpi.first.units.measure.Angle;
|
||||||
|
|
@ -62,10 +58,6 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
private StatusSignal<Angle> turretMotorPosition;
|
private StatusSignal<Angle> turretMotorPosition;
|
||||||
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
|
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
|
||||||
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
|
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
|
||||||
private StatusSignal<Double> turretPositionError;
|
|
||||||
private double forwardLimit = 0;
|
|
||||||
private double reverseLimit = 0;
|
|
||||||
|
|
||||||
private SparkMax launcherMotor;
|
private SparkMax launcherMotor;
|
||||||
private SparkClosedLoopController launcherControler;
|
private SparkClosedLoopController launcherControler;
|
||||||
|
|
||||||
|
|
@ -73,8 +65,6 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
|
|
||||||
private Servo hood;
|
private Servo hood;
|
||||||
|
|
||||||
private Pose3d turretTarget;
|
|
||||||
|
|
||||||
public TurretSubsystem() {
|
public TurretSubsystem() {
|
||||||
|
|
||||||
// Turret configuration
|
// Turret configuration
|
||||||
|
|
@ -106,7 +96,6 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
turretMotorPosition = turretMotor.getPosition();
|
turretMotorPosition = turretMotor.getPosition();
|
||||||
turretMotorForwardLimit = turretMotor.getForwardLimit();
|
turretMotorForwardLimit = turretMotor.getForwardLimit();
|
||||||
turretMotorReverseLimit = turretMotor.getReverseLimit();
|
turretMotorReverseLimit = turretMotor.getReverseLimit();
|
||||||
turretPositionError = turretMotor.getClosedLoopError();
|
|
||||||
|
|
||||||
turretSim = turretMotor.getSimState();
|
turretSim = turretMotor.getSimState();
|
||||||
|
|
||||||
|
|
@ -115,8 +104,8 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
|
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
|
||||||
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
|
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
|
||||||
config.voltageCompensation(10);
|
config.voltageCompensation(10);
|
||||||
config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001
|
config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001
|
||||||
config.closedLoop.feedForward.kS(0.2).kV(0.00005).kA(0.001);
|
config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00);
|
||||||
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
|
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
|
||||||
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
|
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
|
||||||
config.closedLoop.positionWrappingEnabled(false);
|
config.closedLoop.positionWrappingEnabled(false);
|
||||||
|
|
@ -142,8 +131,7 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
public boolean isLauncherUpToSpeed() {
|
public boolean isLauncherUpToSpeed() {
|
||||||
// Valid if velcity is within 10% of setpoint.
|
// Valid if velcity is within 10% of setpoint.
|
||||||
// May need to lower this
|
// May need to lower this
|
||||||
return true;
|
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
|
||||||
// return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void setTargetElevation(double elev)
|
public void setTargetElevation(double elev)
|
||||||
|
|
@ -169,7 +157,7 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
public void homeTurret()
|
public void homeTurret()
|
||||||
{
|
{
|
||||||
if(turretState != TurretState.INIT){
|
if(turretState != TurretState.INIT){
|
||||||
return;
|
return; // Early return, we don't want to home if we already have
|
||||||
}
|
}
|
||||||
turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
|
turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
|
||||||
turretState = TurretState.HOMING;
|
turretState = TurretState.HOMING;
|
||||||
|
|
@ -189,80 +177,62 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
|
return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
|
||||||
}
|
}
|
||||||
|
|
||||||
public void setTurretTargetPose(Pose3d targetPose){
|
|
||||||
turretTarget = targetPose;
|
|
||||||
}
|
|
||||||
|
|
||||||
public Pose3d getTurretTargetPose(){
|
|
||||||
return turretTarget;
|
|
||||||
}
|
|
||||||
|
|
||||||
private double getTurretMotorPosition(){
|
private double getTurretMotorPosition(){
|
||||||
return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
|
return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void periodic() {
|
public void periodic() {
|
||||||
if (turretState == TurretState.HOMING) {
|
|
||||||
if (turretLimitSwitch.get()) { // Turret has tripped the switch
|
if(turretState == TurretState.HOMING)
|
||||||
|
{
|
||||||
|
turretMotor.set(0.05);
|
||||||
|
|
||||||
|
if(turretLimitSwitch.get())
|
||||||
|
{ // Turret has tripped the switch
|
||||||
turretMotor.stopMotor();
|
turretMotor.stopMotor();
|
||||||
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
|
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
|
||||||
if (!Robot.isReal()) {
|
|
||||||
turretOffsetInRotations = 0; // Flip turret around
|
|
||||||
}
|
|
||||||
turretState = TurretState.READY;
|
turretState = TurretState.READY;
|
||||||
forwardLimit = (1.5 + turretOffsetInRotations);
|
// Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
|
||||||
reverseLimit = (-1.5 + turretOffsetInRotations);
|
// Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
|
||||||
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
|
// Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
|
||||||
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
|
// Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
|
||||||
turretMotor.getConfigurator().apply(
|
turretMotor.getConfigurator().apply(
|
||||||
new SoftwareLimitSwitchConfigs()
|
new SoftwareLimitSwitchConfigs()
|
||||||
.withForwardSoftLimitEnable(true)
|
// .withForwardSoftLimitEnable(true)
|
||||||
.withForwardSoftLimitThreshold(forwardLimit)
|
// .withForwardSoftLimitThreshold(forwardLimit)
|
||||||
.withReverseSoftLimitEnable(true)
|
// .withReverseSoftLimitEnable(true)
|
||||||
.withReverseSoftLimitThreshold(reverseLimit));
|
// .withReverseSoftLimitThreshold(reverseLimit)
|
||||||
// setTurretRobotRelative(0);
|
);
|
||||||
// setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
|
// setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
|
||||||
|
setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (LOGGING) {
|
if(LOGGING)
|
||||||
|
{
|
||||||
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
|
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
|
||||||
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
|
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
|
||||||
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
|
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
|
||||||
Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
|
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative());
|
||||||
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
|
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
|
||||||
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw",
|
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
|
||||||
turretMotorPosition.refresh().getValue().in(Units.Rotations));
|
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
|
||||||
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw",
|
|
||||||
turretControlMode.getPositionMeasure().in(Units.Rotations));
|
|
||||||
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
|
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
|
||||||
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
|
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
|
||||||
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
|
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
|
||||||
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
|
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
|
||||||
Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble());
|
|
||||||
Logger.recordOutput("TurretSubsystem-LauncherSetpoint(RPM)", launcherControler.getSetpoint());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void setTurretFieldRelative(Pose3d robotPose, double pos)
|
public void setTurretFieldRelative(double pos)
|
||||||
{
|
{
|
||||||
if(turretState != TurretState.READY)
|
if(turretState != TurretState.READY)
|
||||||
{
|
{
|
||||||
// Not ready to set angle
|
// Not ready to set angle
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
double correctedPos = -pos; // Flip angle to match CCW positive. Add 0.5 to flip around
|
|
||||||
|
|
||||||
double robotRotations = robotPose.getRotation().toRotation2d().getRotations();
|
|
||||||
|
|
||||||
Rotation2d turretAngle = Rotation2d.fromRotations(correctedPos).plus(Rotation2d.fromRotations(robotRotations)).minus(Rotation2d.fromRotations(0.5));
|
|
||||||
|
|
||||||
Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations());
|
|
||||||
|
|
||||||
setTurretRobotRelative(((turretAngle.getRotations())));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void setTurretRobotRelative(double pos)
|
public void setTurretRobotRelative(double pos)
|
||||||
|
|
@ -273,36 +243,12 @@ public class TurretSubsystem extends TestableSubsystem {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
double correctedPos = -pos; // Flip angle to match CCW positive
|
setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
|
||||||
|
|
||||||
setTurretMotorRotation(correctedPos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void setTurretMotorRotation(double pos){
|
public void setTurretMotorRotation(double pos){
|
||||||
double posWithOffset = pos + turretOffsetInRotations;
|
turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations)));
|
||||||
|
turretSim.setRawRotorPosition(pos + turretOffsetInRotations);
|
||||||
Logger.recordOutput("Blah", posWithOffset);
|
|
||||||
|
|
||||||
if(Robot.isSimulation()){
|
|
||||||
if(posWithOffset >= forwardLimit){
|
|
||||||
turretSim.setForwardLimit(true);
|
|
||||||
} else {
|
|
||||||
turretSim.setForwardLimit(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(posWithOffset <= reverseLimit){
|
|
||||||
turretSim.setReverseLimit(true);
|
|
||||||
} else {
|
|
||||||
turretSim.setReverseLimit(false);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
turretMotor.setControl(turretControlMode.withPosition(posWithOffset));
|
|
||||||
turretSim.setRawRotorPosition(-posWithOffset);
|
|
||||||
}
|
|
||||||
|
|
||||||
public boolean isTurretAtTarget(){
|
|
||||||
return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private double getRequestedPosTurretRelative() {
|
private double getRequestedPosTurretRelative() {
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue