Removed the junk and matched to the REV template for anything that matters
This commit is contained in:
parent
08f80562b5
commit
626a241fff
@ -1,3 +0,0 @@
|
|||||||
Files placed in this directory will be deployed to the RoboRIO into the
|
|
||||||
'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
|
|
||||||
to get a proper path relative to the deploy directory.
|
|
||||||
@ -1,37 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"startingPose": {
|
|
||||||
"position": {
|
|
||||||
"x": 1.335622956146197,
|
|
||||||
"y": 5.5196408968316515
|
|
||||||
},
|
|
||||||
"rotation": 179.37978729180955
|
|
||||||
},
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Charge Shooter"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Speaker Note Shot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Center Subwoofer to Center"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"startingPose": {
|
|
||||||
"position": {
|
|
||||||
"x": 0.9847983228729987,
|
|
||||||
"y": 1.9646179463299098
|
|
||||||
},
|
|
||||||
"rotation": 0
|
|
||||||
},
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Just Backup"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -1,43 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"startingPose": {
|
|
||||||
"position": {
|
|
||||||
"x": 0.4936438362905214,
|
|
||||||
"y": 6.981410202136645
|
|
||||||
},
|
|
||||||
"rotation": 180.0
|
|
||||||
},
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Left Subwoofer"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Charge Shooter"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Speaker Note Shot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "L Sub to Center"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -1,74 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"startingPose": {
|
|
||||||
"position": {
|
|
||||||
"x": 1.1134340217398382,
|
|
||||||
"y": 6.525338178881486
|
|
||||||
},
|
|
||||||
"rotation": 42.79740183823424
|
|
||||||
},
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Speaker Note Shot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Left Preloaded+Pickup"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "parallel",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 2.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Amp Handoff"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Note 1 to amp"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Amp Note Shot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Leftmost Center Note"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Note 1 to amp"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -1,50 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"startingPose": null,
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Speaker Note Shot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Left Preloaded+Pickup"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "deadline",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Amp Handoff"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Note 1 to amp"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Amp Note Shot"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -1,43 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"startingPose": {
|
|
||||||
"position": {
|
|
||||||
"x": 1.1134340217398382,
|
|
||||||
"y": 6.525338178881486
|
|
||||||
},
|
|
||||||
"rotation": 33.13560954871888
|
|
||||||
},
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Left Preloaded+Pickup"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Note 1 to amp"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Leftmost Center Note"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Note 1 to amp"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -1,43 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"startingPose": {
|
|
||||||
"position": {
|
|
||||||
"x": 1.0578032584636603,
|
|
||||||
"y": 4.485794114445372
|
|
||||||
},
|
|
||||||
"rotation": 141.84277341263092
|
|
||||||
},
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Charge Shooter"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 2.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "Speaker Note Shot"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "Right Subwoofer to Center"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
File diff suppressed because one or more lines are too long
@ -1,74 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.335622956146197,
|
|
||||||
"y": 5.5196408968316515
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 2.3356229561461985,
|
|
||||||
"y": 5.5196408968316515
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 4.072055095677142,
|
|
||||||
"y": 5.028486410249174
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 3.364392809755599,
|
|
||||||
"y": 5.618204981850461
|
|
||||||
},
|
|
||||||
"nextControl": {
|
|
||||||
"x": 4.843869288878178,
|
|
||||||
"y": 4.385307915914977
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 7.299641721790566,
|
|
||||||
"y": 0.877061583182996
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 6.773404771880769,
|
|
||||||
"y": 4.046177437084219
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [
|
|
||||||
{
|
|
||||||
"waypointRelativePos": 0.9,
|
|
||||||
"rotationDegrees": 90.25335933192582,
|
|
||||||
"rotateFast": false
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": 179.04515874612784,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,52 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 0.9847983228729987,
|
|
||||||
"y": 1.9646179463299098
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 1.9847983228729973,
|
|
||||||
"y": 1.9646179463299098
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.6453682536994707,
|
|
||||||
"y": 1.9646179463299098
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 1.6453682536994707,
|
|
||||||
"y": 1.9646179463299098
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": 1.3971810272964678,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,52 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.2888463383764373,
|
|
||||||
"y": 6.642279723305886
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 2.2888463383764366,
|
|
||||||
"y": 6.642279723305886
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.159911049166827,
|
|
||||||
"y": 6.864468657712245
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 2.159911049166827,
|
|
||||||
"y": 6.864468657712245
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": 21.801409486351982,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,52 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.1134340217398382,
|
|
||||||
"y": 6.525338178881486
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 1.148516485067158,
|
|
||||||
"y": 7.191904982100564
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.3179319293111518,
|
|
||||||
"y": 6.981410202136645
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 1.8033891338437944,
|
|
||||||
"y": 6.981410202136645
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": 40.97173633351488,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,52 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 0.4936438362905214,
|
|
||||||
"y": 6.981410202136645
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 1.3941696612245824,
|
|
||||||
"y": 6.981410202136645
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.148516485067158,
|
|
||||||
"y": 6.560420642208806
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 1.5812001994374358,
|
|
||||||
"y": 6.864468657712245
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -146.30993247402017,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": -178.6677801461302,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,102 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.347017520241028,
|
|
||||||
"y": 6.993104356575341
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 4.446268037830382,
|
|
||||||
"y": 7.110045900999739
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 6.071755505338439,
|
|
||||||
"y": 7.297152372084174
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 5.077752377722138,
|
|
||||||
"y": 7.262069908751458
|
|
||||||
},
|
|
||||||
"nextControl": {
|
|
||||||
"x": 6.725659674531382,
|
|
||||||
"y": 7.320231342764915
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 7.63877220062539,
|
|
||||||
"y": 7.425788070951012
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 6.621380764133118,
|
|
||||||
"y": 7.460870534278332
|
|
||||||
},
|
|
||||||
"nextControl": {
|
|
||||||
"x": 8.656163637117663,
|
|
||||||
"y": 7.390705607623692
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 3.545818145767345,
|
|
||||||
"y": 6.642279723305886
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 6.323179825848877,
|
|
||||||
"y": 7.12758713266797
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [
|
|
||||||
{
|
|
||||||
"waypointRelativePos": 1,
|
|
||||||
"rotationDegrees": 172.50414236027015,
|
|
||||||
"rotateFast": false
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"constraintZones": [
|
|
||||||
{
|
|
||||||
"name": "New Constraints Zone",
|
|
||||||
"minWaypointRelativePos": 0.85,
|
|
||||||
"maxWaypointRelativePos": 1.35,
|
|
||||||
"constraints": {
|
|
||||||
"maxVelocity": 1.5,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
}
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 1.5,
|
|
||||||
"rotation": -179.23610153906992,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": 0,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,52 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.0,
|
|
||||||
"y": 7.0
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 3.0,
|
|
||||||
"y": 7.0
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.2654580294915574,
|
|
||||||
"y": 6.291455090032687
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 1.7098358983042754,
|
|
||||||
"y": 6.525338178881486
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": -145.88552705465875,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": 179.55484341368705,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,63 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 2.353014392638472,
|
|
||||||
"y": 6.993104356579084
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 1.768306670516475,
|
|
||||||
"y": 7.004798511021523
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.8384715971711143,
|
|
||||||
"y": 7.624588696470841
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 1.7950792079196904,
|
|
||||||
"y": 7.315863019194643
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [
|
|
||||||
{
|
|
||||||
"name": "New Event Marker",
|
|
||||||
"waypointRelativePos": 0,
|
|
||||||
"command": {
|
|
||||||
"type": "parallel",
|
|
||||||
"data": {
|
|
||||||
"commands": []
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0.0,
|
|
||||||
"rotation": 90.0,
|
|
||||||
"rotateFast": true
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": -178.36342295838335,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,52 +0,0 @@
|
|||||||
{
|
|
||||||
"version": 1.0,
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 1.0578032584636603,
|
|
||||||
"y": 4.485794114445372
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 1.5708842846257125,
|
|
||||||
"y": 2.6240429623716395
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 7.0095431619434665,
|
|
||||||
"y": 0.8355890997496291
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 6.0095431619434665,
|
|
||||||
"y": 0.8355890997496291
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 540.0,
|
|
||||||
"maxAngularAcceleration": 720.0
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0,
|
|
||||||
"rotateFast": false
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"previewStartingState": {
|
|
||||||
"rotation": 144.68878656036674,
|
|
||||||
"velocity": 0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": true
|
|
||||||
}
|
|
||||||
@ -1,13 +0,0 @@
|
|||||||
package frc.robot.Commands;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
|
||||||
import frc.robot.subsystems.Indexer;
|
|
||||||
import frc.robot.subsystems.Shooter;
|
|
||||||
|
|
||||||
public class AmpHandoff extends ParallelCommandGroup{
|
|
||||||
|
|
||||||
AmpHandoff(Indexer indexer, Shooter shooter){
|
|
||||||
//addCommands(indexer.shootNote(() -> 1), shooter.ampHandoff());
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,13 +0,0 @@
|
|||||||
package frc.robot.Commands;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
|
||||||
import frc.robot.constants.ShooterConstants;
|
|
||||||
import frc.robot.subsystems.Indexer;
|
|
||||||
import frc.robot.subsystems.Shooter;
|
|
||||||
|
|
||||||
public class SpeakerShot extends ParallelCommandGroup{
|
|
||||||
|
|
||||||
SpeakerShot(Indexer indexer, Shooter shooter){
|
|
||||||
//addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -4,134 +4,26 @@
|
|||||||
|
|
||||||
package frc.robot;
|
package frc.robot;
|
||||||
|
|
||||||
import java.io.IOException;
|
|
||||||
import java.util.Optional;
|
|
||||||
|
|
||||||
import com.pathplanner.lib.auto.AutoBuilder;
|
|
||||||
import com.pathplanner.lib.auto.NamedCommands;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.MathUtil;
|
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
import edu.wpi.first.math.geometry.Transform3d;
|
|
||||||
import edu.wpi.first.math.util.Units;
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.Commands;
|
|
||||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
||||||
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 frc.robot.constants.OIConstants;
|
import frc.robot.constants.OIConstants;
|
||||||
import frc.robot.constants.PhotonConstants;
|
|
||||||
import frc.robot.constants.ShooterConstants;
|
|
||||||
import frc.robot.subsystems.Climber;
|
|
||||||
import frc.robot.subsystems.Drivetrain;
|
import frc.robot.subsystems.Drivetrain;
|
||||||
import frc.robot.subsystems.Indexer;
|
|
||||||
import frc.robot.utilities.PhotonVision;
|
|
||||||
import frc.robot.subsystems.Intake;
|
|
||||||
import frc.robot.subsystems.Shooter;
|
|
||||||
|
|
||||||
public class RobotContainer {
|
public class RobotContainer {
|
||||||
private static final String kAutoTabName = "Autonomous";
|
|
||||||
private static final String kTeleopTabName = "Teloperated";
|
|
||||||
|
|
||||||
//private PhotonVision photonvision;
|
|
||||||
|
|
||||||
private Drivetrain drivetrain;
|
private Drivetrain drivetrain;
|
||||||
private Intake intake;
|
|
||||||
private Shooter shooter;
|
|
||||||
private Climber climber;
|
|
||||||
private Indexer indexer;
|
|
||||||
|
|
||||||
private CommandXboxController driver;
|
private CommandXboxController driver;
|
||||||
private CommandXboxController operator;
|
|
||||||
|
|
||||||
private Trigger isTeleopTrigger;
|
|
||||||
|
|
||||||
private SendableChooser<Command> autoChooser;
|
|
||||||
|
|
||||||
private int ampTagID;
|
|
||||||
|
|
||||||
// TODO There's more than one source tag, how do we want to handle this?
|
|
||||||
private int sourceTagID;
|
|
||||||
|
|
||||||
// TODO There's more than one speaker tag, how do we want to handle this?
|
|
||||||
private int speakerTag;
|
|
||||||
|
|
||||||
private boolean setAmpAngle;
|
|
||||||
|
|
||||||
public RobotContainer() {
|
public RobotContainer() {
|
||||||
/*try {
|
drivetrain = new Drivetrain(new Pose2d());
|
||||||
|
|
||||||
photonvision = new PhotonVision(
|
|
||||||
PhotonConstants.kCameraName,
|
|
||||||
PhotonConstants.kCameraTransform,
|
|
||||||
PhotonConstants.kCameraHeightMeters,
|
|
||||||
PhotonConstants.kCameraPitchRadians
|
|
||||||
);
|
|
||||||
} catch (IOException e) {
|
|
||||||
photonvision = null;
|
|
||||||
}*/
|
|
||||||
|
|
||||||
// TODO Need to provide a real initial pose
|
|
||||||
// TODO Need to provide a real IAprilTagProvider, null means we're not using one at all
|
|
||||||
// TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
|
|
||||||
drivetrain = new Drivetrain(new Pose2d(), null, null);
|
|
||||||
|
|
||||||
intake = new Intake();
|
|
||||||
|
|
||||||
indexer = new Indexer();
|
|
||||||
|
|
||||||
shooter = new Shooter(indexer::getBeamBreak);
|
|
||||||
|
|
||||||
climber = new Climber(shooter::getShooterAngle);
|
|
||||||
|
|
||||||
NamedCommands.registerCommand("Charge Shooter 2 Sec", shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0).withTimeout(2.0));
|
|
||||||
NamedCommands.registerCommand("Speaker Note Shot", Commands.parallel(shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0), indexer.shootNote(() -> 1.0)).withTimeout(2.0));
|
|
||||||
|
|
||||||
// An example Named Command, doesn't need to remain once we start actually adding real things
|
|
||||||
// ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
|
|
||||||
NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
|
|
||||||
|
|
||||||
// TODO Specify a default auto string once we have one
|
|
||||||
autoChooser = AutoBuilder.buildAutoChooser();
|
|
||||||
|
|
||||||
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
||||||
operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
|
|
||||||
|
|
||||||
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
|
|
||||||
|
|
||||||
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
|
|
||||||
|
|
||||||
if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
|
|
||||||
ampTagID = 5;
|
|
||||||
sourceTagID = 9;
|
|
||||||
speakerTag = 4;
|
|
||||||
} else {
|
|
||||||
ampTagID = 6;
|
|
||||||
sourceTagID = 1;
|
|
||||||
speakerTag = 8;
|
|
||||||
}
|
|
||||||
|
|
||||||
setAmpAngle = false;
|
|
||||||
|
|
||||||
configureBindings();
|
configureBindings();
|
||||||
shuffleboardSetup();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// The objective should be to have the subsystems expose methods that return commands
|
|
||||||
// that can be bound to the triggers provided by the CommandXboxController class.
|
|
||||||
// This mindset should help keep RobotContainer a little cleaner this year.
|
|
||||||
// This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example),
|
|
||||||
// but generally reduce/avoid any situation where the keyword "new" is involved if you're working
|
|
||||||
// with a subsystem
|
|
||||||
private void configureBindings() {
|
private void configureBindings() {
|
||||||
isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
|
|
||||||
|
|
||||||
drivetrain.setDefaultCommand(
|
drivetrain.setDefaultCommand(
|
||||||
drivetrain.teleopCommand(
|
drivetrain.teleopCommand(
|
||||||
driver::getLeftY,
|
driver::getLeftY,
|
||||||
@ -141,161 +33,11 @@ public class RobotContainer {
|
|||||||
)
|
)
|
||||||
);
|
);
|
||||||
|
|
||||||
//intake.setDefaultCommand(intake.intakeUpCommand());
|
driver.a().onTrue(drivetrain.zeroHeadingCommand());
|
||||||
intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, () -> 0.0));
|
|
||||||
|
|
||||||
shooter.setDefaultCommand(
|
|
||||||
shooter.angleSpeedsSetpoints(
|
|
||||||
() -> {
|
|
||||||
if (setAmpAngle) {
|
|
||||||
return ShooterConstants.kShooterAmpAngle;
|
|
||||||
} else {
|
|
||||||
return ShooterConstants.kShooterLoadAngle;
|
|
||||||
}
|
|
||||||
},
|
|
||||||
() -> {
|
|
||||||
if (driver.getLeftTriggerAxis() > .25) {
|
|
||||||
return -1.0;
|
|
||||||
}else if(driver.getRightTriggerAxis() > 0.25){
|
|
||||||
return 1.0;
|
|
||||||
} else {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
//shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
|
|
||||||
|
|
||||||
climber.setDefaultCommand(climber.stop());
|
|
||||||
|
|
||||||
indexer.setDefaultCommand(indexer.shootNote(
|
|
||||||
() -> {
|
|
||||||
if (driver.getLeftTriggerAxis() > .25) {
|
|
||||||
return -1.0;
|
|
||||||
}else {
|
|
||||||
return 0.0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
));
|
|
||||||
|
|
||||||
driver.povCenter().onFalse(
|
|
||||||
drivetrain.driveCardinal(
|
|
||||||
driver::getLeftY,
|
|
||||||
driver::getLeftX,
|
|
||||||
driver.getHID()::getPOV,
|
|
||||||
OIConstants.kTeleopDriveDeadband).until(
|
|
||||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
// Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
|
|
||||||
driver.leftBumper().onTrue(
|
|
||||||
drivetrain.driveAprilTagLock(
|
|
||||||
driver::getLeftY,
|
|
||||||
driver::getLeftX,
|
|
||||||
OIConstants.kTeleopDriveDeadband,
|
|
||||||
ampTagID
|
|
||||||
).until(
|
|
||||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
// Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
|
|
||||||
driver.b().onTrue(
|
|
||||||
drivetrain.driveAprilTagLock(
|
|
||||||
driver::getLeftY,
|
|
||||||
driver::getLeftX,
|
|
||||||
OIConstants.kTeleopDriveDeadband,
|
|
||||||
sourceTagID
|
|
||||||
).until(
|
|
||||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
// Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
|
|
||||||
driver.x().onTrue(
|
|
||||||
drivetrain.driveAprilTagLock(
|
|
||||||
driver::getLeftY,
|
|
||||||
driver::getLeftX,
|
|
||||||
OIConstants.kTeleopDriveDeadband,
|
|
||||||
speakerTag
|
|
||||||
).until(
|
|
||||||
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
// This was originally a run while held, not sure that's really necessary, change it if need be
|
|
||||||
driver.y().onTrue(drivetrain.zeroHeadingCommand());
|
|
||||||
|
|
||||||
// This was originally a run while held, not sure that's really necessary, change it if need be
|
|
||||||
driver.rightBumper().onTrue(drivetrain.setXCommand());
|
|
||||||
|
|
||||||
/*
|
|
||||||
* This has been added because interest has been expressed in trying field relative vs
|
|
||||||
* robot relative control. This should <i>default</i> to field relative, but give the option
|
|
||||||
* for the person practicing driving to push start on the driver's controller to quickly switch to
|
|
||||||
* the other style.
|
|
||||||
*
|
|
||||||
* If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
|
|
||||||
* mechanism will need to be devised, this will work for now.
|
|
||||||
*/
|
|
||||||
driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
|
|
||||||
|
|
||||||
operator.b().whileTrue(Commands.parallel(indexer.shootNote(() -> 1.0), shooter.ampHandoff(() -> driver.getRightTriggerAxis(), () -> {
|
|
||||||
if(operator.getRightTriggerAxis()>0.25){
|
|
||||||
return true;
|
|
||||||
}else{
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
})));
|
|
||||||
|
|
||||||
|
|
||||||
//driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
|
|
||||||
|
|
||||||
operator.a().onTrue(new InstantCommand(() -> { setAmpAngle = true; }));
|
|
||||||
operator.a().onFalse(new InstantCommand(() -> { setAmpAngle = false; }));
|
|
||||||
|
|
||||||
//operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0));
|
|
||||||
|
|
||||||
operator.y().whileTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState()));
|
|
||||||
|
|
||||||
driver.a().whileTrue(indexer.shootNote(() -> 1.0));
|
|
||||||
|
|
||||||
operator.back().toggleOnTrue(shooter.manualPivot(
|
|
||||||
() -> MathUtil.clamp(-operator.getLeftY(), -0.75, 0.75),
|
|
||||||
() -> MathUtil.clamp(operator.getRightTriggerAxis()-operator.getLeftTriggerAxis(), -0.75, 0.75)
|
|
||||||
));
|
|
||||||
|
|
||||||
driver.povDown().whileTrue(indexer.shootNote(() -> -1.0));
|
|
||||||
|
|
||||||
operator.start().onTrue(shooter.zeroEncoder());
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
private void shuffleboardSetup() {
|
|
||||||
ShuffleboardTab autoTab = Shuffleboard.getTab(kAutoTabName);
|
|
||||||
autoTab.add("Autonomous Selector", autoChooser)
|
|
||||||
.withPosition(0, 0)
|
|
||||||
.withSize(2, 1)
|
|
||||||
.withWidget(BuiltInWidgets.kComboBoxChooser);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//Always select the auto tab on startup
|
|
||||||
Shuffleboard.selectTab(kAutoTabName);
|
|
||||||
|
|
||||||
ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName);
|
|
||||||
teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl)
|
|
||||||
.withPosition(0, 0)
|
|
||||||
.withSize(2, 1)
|
|
||||||
.withWidget(BuiltInWidgets.kBooleanBox);
|
|
||||||
teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak);
|
|
||||||
teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
|
|
||||||
teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
|
|
||||||
teleopTab.addDouble("intake angle", intake::getIntakeAngle);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command getAutonomousCommand() {
|
public Command getAutonomousCommand() {
|
||||||
return autoChooser.getSelected();
|
return null;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,34 +0,0 @@
|
|||||||
package frc.robot.constants;
|
|
||||||
|
|
||||||
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
|
|
||||||
import com.pathplanner.lib.util.PIDConstants;
|
|
||||||
import com.pathplanner.lib.util.ReplanningConfig;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
|
||||||
import edu.wpi.first.math.util.Units;
|
|
||||||
|
|
||||||
public final class AutoConstants {
|
|
||||||
public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6
|
|
||||||
public static final double kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4
|
|
||||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
|
||||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
|
||||||
|
|
||||||
public static final double kPXController = 1.05;
|
|
||||||
public static final double kPYController = 1.05;
|
|
||||||
public static final double kPThetaController = 0.95; // needs to be separate from heading control
|
|
||||||
|
|
||||||
public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
|
|
||||||
public static final double kDHeadingController = 0.0025;
|
|
||||||
|
|
||||||
public static final HolonomicPathFollowerConfig kPFConfig = new HolonomicPathFollowerConfig(
|
|
||||||
new PIDConstants(kPXController),
|
|
||||||
new PIDConstants(kPThetaController),
|
|
||||||
kMaxSpeedMetersPerSecond,
|
|
||||||
Units.inchesToMeters(Math.sqrt(Math.pow(14-17.5, 2)+ Math.pow(14-1.75, 2))),
|
|
||||||
new ReplanningConfig()
|
|
||||||
);
|
|
||||||
|
|
||||||
// Constraint for the motion profiled robot angle controller
|
|
||||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
|
|
||||||
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
|
|
||||||
}
|
|
||||||
@ -1,6 +0,0 @@
|
|||||||
package frc.robot.constants;
|
|
||||||
|
|
||||||
public class ClimberConstants {
|
|
||||||
public static final int kClimberMotor1CANID = 14;
|
|
||||||
public static final int kClimberMotor2CANID = 15;
|
|
||||||
}
|
|
||||||
@ -1,7 +0,0 @@
|
|||||||
package frc.robot.constants;
|
|
||||||
|
|
||||||
public class IndexerConstants {
|
|
||||||
public static final int kIndexerID = 13;
|
|
||||||
|
|
||||||
public static final int kIndexerBeamBreakChannel = 2;
|
|
||||||
}
|
|
||||||
@ -1,22 +0,0 @@
|
|||||||
package frc.robot.constants;
|
|
||||||
|
|
||||||
public class IntakeConstants {
|
|
||||||
public static final int kIntakePivotID = 10;
|
|
||||||
public static final int kIntakeRollerID = 12;
|
|
||||||
|
|
||||||
public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0));
|
|
||||||
|
|
||||||
public static final int kPivotCurrentLimit = 40;
|
|
||||||
|
|
||||||
public static final double kPIntake = 0;
|
|
||||||
public static final double kIIntake = 0;
|
|
||||||
public static final double kDIntake = 0;
|
|
||||||
|
|
||||||
public static final double kSFeedForward = 0;
|
|
||||||
public static final double kGFeedForward = 1.11;
|
|
||||||
public static final double kVFeedForward = 0.73;
|
|
||||||
|
|
||||||
public static final double kStartingAngle = Math.toRadians(105.0);
|
|
||||||
public static final double kUpAngle = Math.toRadians(90.0);
|
|
||||||
public static final double kDownAngle = Math.toRadians(-34.0);
|
|
||||||
}
|
|
||||||
@ -1,17 +0,0 @@
|
|||||||
package frc.robot.constants;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Transform3d;
|
|
||||||
import edu.wpi.first.math.util.Units;
|
|
||||||
|
|
||||||
public class PhotonConstants {
|
|
||||||
public static final String kCameraName = "Camera_Module_v1";
|
|
||||||
|
|
||||||
// TODO Set this to something real if visual pose estimation is used
|
|
||||||
public static final Transform3d kCameraTransform = new Transform3d();
|
|
||||||
|
|
||||||
// TODO The camera will be moved, this is an old value that needs to update
|
|
||||||
public static final double kCameraHeightMeters = .517525;
|
|
||||||
|
|
||||||
// TODO The camera will be moved, this is an old value that needs to update
|
|
||||||
public static final double kCameraPitchRadians = Units.degreesToRadians(15);
|
|
||||||
}
|
|
||||||
@ -1,34 +0,0 @@
|
|||||||
package frc.robot.constants;
|
|
||||||
|
|
||||||
public class ShooterConstants {
|
|
||||||
|
|
||||||
public static final int kTopShooterID = 9;
|
|
||||||
public static final int kBottomShooterID = 11;
|
|
||||||
public static final int kShooterPivotID = 16;
|
|
||||||
|
|
||||||
public static final double kShooterP = 0.0;
|
|
||||||
public static final double kShooterI = 0.0;
|
|
||||||
public static final double kShooterD = 0.0;
|
|
||||||
|
|
||||||
public static final double kShooterFF = 0.0;
|
|
||||||
|
|
||||||
public static final double kShooterPivotP = 3.0;
|
|
||||||
public static final double kShooterPivotI = 0.0;
|
|
||||||
public static final double kShooterPivotD = 0.0;
|
|
||||||
|
|
||||||
public static final double kShooterLoadAngle = Math.toRadians(-20.0);
|
|
||||||
public static final double kShooterAmpAngle = Math.toRadians(105.0);
|
|
||||||
|
|
||||||
public static final double kPivotConversion = 1/(40.0*(28.0/15.0));
|
|
||||||
|
|
||||||
public static final double kSShooterPivotFF = 0.0;
|
|
||||||
public static final double kGShooterPivotFF = 0.33;
|
|
||||||
public static final double kVShooterPivotFF = 1.44;
|
|
||||||
|
|
||||||
public static final double kMaxPivotSpeed = 0.0;
|
|
||||||
public static final double kMaxPivotAcceleration = 0.0;
|
|
||||||
|
|
||||||
public static final int kShooterEncoderChannelA = 0;
|
|
||||||
public static final int kShooterEncoderChannelB = 1;
|
|
||||||
public static final int kShooterBeamBreakChannel = 3;
|
|
||||||
}
|
|
||||||
@ -1,34 +0,0 @@
|
|||||||
package frc.robot.interfaces;
|
|
||||||
|
|
||||||
import java.util.OptionalDouble;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* An interface which ensures a class can provide common AprilTag oriented
|
|
||||||
* information from various sources in a consistent way.
|
|
||||||
*/
|
|
||||||
public interface IAprilTagProvider {
|
|
||||||
/**
|
|
||||||
* A method to get the distance from <i>the camera</i> to the AprilTag specified
|
|
||||||
*
|
|
||||||
* @param id The ID of the AprilTag to give a distance to
|
|
||||||
* @param targetHeightMeters The height of the AprilTag off the ground, in meters
|
|
||||||
* @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
|
|
||||||
*/
|
|
||||||
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* A method to get the pitch from the center of the image of a particular AprilTag
|
|
||||||
*
|
|
||||||
* @param id The ID of the AprilTag to get the pitch of
|
|
||||||
* @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
|
|
||||||
*/
|
|
||||||
public OptionalDouble getTagPitchByID(int id);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* A method to get the yaw from the center of the image of a particular AprilTag
|
|
||||||
*
|
|
||||||
* @param id The ID of the AprilTag to get the yaw of
|
|
||||||
* @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
|
|
||||||
*/
|
|
||||||
public OptionalDouble getTagYawByID(int id);
|
|
||||||
}
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
package frc.robot.interfaces;
|
|
||||||
|
|
||||||
import java.util.Optional;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* An interface which ensures a class' ability to provide visual pose information
|
|
||||||
* in a consistent way
|
|
||||||
*/
|
|
||||||
public interface IVisualPoseProvider {
|
|
||||||
/**
|
|
||||||
* A record that can contain the two elements necessary for a WPILIB
|
|
||||||
* pose estimator to use the information from a vision system as part of a full
|
|
||||||
* robot pose estimation
|
|
||||||
*/
|
|
||||||
public record VisualPose(Pose2d visualPose, double timestamp) {}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Return a VisualPose or null if an empty Optional if none is available.
|
|
||||||
* Implementation should provide an empty response if it's unable to provide
|
|
||||||
* a reliable pose, or any pose at all.
|
|
||||||
*
|
|
||||||
* @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
|
|
||||||
*/
|
|
||||||
public Optional<VisualPose> getVisualPose();
|
|
||||||
}
|
|
||||||
@ -1,47 +0,0 @@
|
|||||||
package frc.robot.subsystems;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import com.ctre.phoenix.motorcontrol.VictorSPXControlMode;
|
|
||||||
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import frc.robot.constants.ClimberConstants;
|
|
||||||
import frc.robot.constants.ShooterConstants;
|
|
||||||
|
|
||||||
public class Climber extends SubsystemBase {
|
|
||||||
private VictorSPX motor1;
|
|
||||||
private VictorSPX motor2;
|
|
||||||
|
|
||||||
private DoubleSupplier shooterAngle;
|
|
||||||
|
|
||||||
public Climber(DoubleSupplier shooterAngle) {
|
|
||||||
motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
|
|
||||||
motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
|
|
||||||
|
|
||||||
|
|
||||||
motor2.follow(motor1);
|
|
||||||
motor1.setInverted(true);
|
|
||||||
motor2.setInverted(true);
|
|
||||||
this.shooterAngle = shooterAngle;
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command setSpeedWithSupplier(DoubleSupplier speed) {
|
|
||||||
return run(() -> {
|
|
||||||
|
|
||||||
motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
|
|
||||||
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command setSpeed(double speed) {
|
|
||||||
return run(() -> {
|
|
||||||
motor1.set(VictorSPXControlMode.PercentOutput, speed);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command stop() {
|
|
||||||
return setSpeed(0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,7 +1,6 @@
|
|||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import edu.wpi.first.math.MathUtil;
|
import edu.wpi.first.math.MathUtil;
|
||||||
import edu.wpi.first.math.controller.PIDController;
|
|
||||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||||
import edu.wpi.first.math.filter.SlewRateLimiter;
|
import edu.wpi.first.math.filter.SlewRateLimiter;
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
@ -12,26 +11,15 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
|||||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||||
import edu.wpi.first.util.WPIUtilJNI;
|
import edu.wpi.first.util.WPIUtilJNI;
|
||||||
|
|
||||||
import java.util.Optional;
|
|
||||||
import java.util.OptionalDouble;
|
|
||||||
import java.util.function.BooleanSupplier;
|
|
||||||
import java.util.function.DoubleSupplier;
|
import java.util.function.DoubleSupplier;
|
||||||
|
|
||||||
import com.kauailabs.navx.frc.AHRS;
|
import com.kauailabs.navx.frc.AHRS;
|
||||||
import com.pathplanner.lib.auto.AutoBuilder;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
|
||||||
import edu.wpi.first.wpilibj.SPI;
|
import edu.wpi.first.wpilibj.SPI;
|
||||||
import frc.robot.constants.AutoConstants;
|
|
||||||
import frc.robot.constants.DrivetrainConstants;
|
import frc.robot.constants.DrivetrainConstants;
|
||||||
import frc.robot.utilities.MAXSwerveModule;
|
import frc.robot.utilities.MAXSwerveModule;
|
||||||
import frc.robot.utilities.SwerveUtils;
|
import frc.robot.utilities.SwerveUtils;
|
||||||
import frc.robot.interfaces.IAprilTagProvider;
|
|
||||||
import frc.robot.interfaces.IVisualPoseProvider;
|
|
||||||
import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.PIDCommand;
|
|
||||||
import edu.wpi.first.wpilibj2.command.PrintCommand;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
|
||||||
public class Drivetrain extends SubsystemBase {
|
public class Drivetrain extends SubsystemBase {
|
||||||
@ -47,15 +35,9 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
private final SlewRateLimiter m_magLimiter;
|
private final SlewRateLimiter m_magLimiter;
|
||||||
private final SlewRateLimiter m_rotLimiter;
|
private final SlewRateLimiter m_rotLimiter;
|
||||||
|
|
||||||
private final IAprilTagProvider m_aprilTagProvider;
|
|
||||||
|
|
||||||
private final IVisualPoseProvider m_visualPoseProvider;
|
|
||||||
|
|
||||||
// Odometry class for tracking robot pose
|
// Odometry class for tracking robot pose
|
||||||
private final SwerveDrivePoseEstimator m_poseEstimator;
|
private final SwerveDrivePoseEstimator m_poseEstimator;
|
||||||
|
|
||||||
private boolean fieldRelativeControl;
|
|
||||||
|
|
||||||
// Slew rate filter variables for controlling lateral acceleration
|
// Slew rate filter variables for controlling lateral acceleration
|
||||||
private double m_currentRotation;
|
private double m_currentRotation;
|
||||||
private double m_currentTranslationDir;
|
private double m_currentTranslationDir;
|
||||||
@ -63,10 +45,8 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
|
|
||||||
private double m_prevTime;
|
private double m_prevTime;
|
||||||
|
|
||||||
private double gyroOffset;
|
|
||||||
|
|
||||||
/** Creates a new DriveSubsystem. */
|
/** Creates a new DriveSubsystem. */
|
||||||
public Drivetrain(Pose2d initialPose, IAprilTagProvider aprilTagProvider, IVisualPoseProvider visualPoseProvider) {
|
public Drivetrain(Pose2d initialPose) {
|
||||||
m_frontLeft = new MAXSwerveModule(
|
m_frontLeft = new MAXSwerveModule(
|
||||||
DrivetrainConstants.kFrontLeftDrivingCanId,
|
DrivetrainConstants.kFrontLeftDrivingCanId,
|
||||||
DrivetrainConstants.kFrontLeftTurningCanId,
|
DrivetrainConstants.kFrontLeftTurningCanId,
|
||||||
@ -107,36 +87,10 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
},
|
},
|
||||||
initialPose
|
initialPose
|
||||||
);
|
);
|
||||||
|
|
||||||
m_aprilTagProvider = aprilTagProvider;
|
|
||||||
|
|
||||||
m_visualPoseProvider = visualPoseProvider;
|
|
||||||
|
|
||||||
fieldRelativeControl = true;
|
|
||||||
|
|
||||||
m_currentRotation = 0.0;
|
m_currentRotation = 0.0;
|
||||||
m_currentTranslationDir = 0.0;
|
m_currentTranslationDir = 0.0;
|
||||||
m_currentTranslationMag = 0.0;
|
m_currentTranslationMag = 0.0;
|
||||||
m_prevTime = WPIUtilJNI.now() * 1e-6;
|
m_prevTime = WPIUtilJNI.now() * 1e-6;
|
||||||
|
|
||||||
AutoBuilder.configureHolonomic(
|
|
||||||
this::getPose,
|
|
||||||
this::resetOdometry,
|
|
||||||
this::getCurrentChassisSpeeds,
|
|
||||||
this::driveWithChassisSpeeds,
|
|
||||||
AutoConstants.kPFConfig,
|
|
||||||
() -> {
|
|
||||||
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
|
|
||||||
if (alliance.isPresent()) {
|
|
||||||
return alliance.get() == DriverStation.Alliance.Red;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
},
|
|
||||||
this
|
|
||||||
);
|
|
||||||
|
|
||||||
gyroOffset = DrivetrainConstants.kRobotStartOffset;
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
@ -151,32 +105,6 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
m_rearRight.getPosition()
|
m_rearRight.getPosition()
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
|
||||||
if (m_visualPoseProvider != null) {
|
|
||||||
Optional<VisualPose> vPose = m_visualPoseProvider.getVisualPose();
|
|
||||||
|
|
||||||
if (vPose.isPresent()) {
|
|
||||||
m_poseEstimator.addVisionMeasurement(vPose.get().visualPose(), vPose.get().timestamp());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
public double getRobotStartOffset() {
|
|
||||||
return gyroOffset;
|
|
||||||
}
|
|
||||||
|
|
||||||
public void setRobotStartOffset(double offset) {
|
|
||||||
gyroOffset = offset;
|
|
||||||
}
|
|
||||||
|
|
||||||
public boolean isFieldRelativeControl() {
|
|
||||||
return fieldRelativeControl;
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command toggleFieldRelativeControl() {
|
|
||||||
return runOnce(() -> {
|
|
||||||
fieldRelativeControl = !fieldRelativeControl;
|
|
||||||
});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -188,14 +116,6 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
return m_poseEstimator.getEstimatedPosition();
|
return m_poseEstimator.getEstimatedPosition();
|
||||||
}
|
}
|
||||||
|
|
||||||
public double getPoseX(){
|
|
||||||
return m_poseEstimator.getEstimatedPosition().getX();
|
|
||||||
}
|
|
||||||
|
|
||||||
public double getPoseY(){
|
|
||||||
return m_poseEstimator.getEstimatedPosition().getY();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Resets the odometry to the specified pose.
|
* Resets the odometry to the specified pose.
|
||||||
*
|
*
|
||||||
@ -233,7 +153,7 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
* field.
|
* field.
|
||||||
* @param rateLimit Whether to enable rate limiting for smoother control.
|
* @param rateLimit Whether to enable rate limiting for smoother control.
|
||||||
*/
|
*/
|
||||||
public void drive(double xSpeed, double ySpeed, double rot, BooleanSupplier fieldRelative, boolean rateLimit) {
|
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) {
|
||||||
double xSpeedCommanded;
|
double xSpeedCommanded;
|
||||||
double ySpeedCommanded;
|
double ySpeedCommanded;
|
||||||
|
|
||||||
@ -287,8 +207,8 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed;
|
double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed;
|
||||||
|
|
||||||
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
|
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
|
||||||
fieldRelative.getAsBoolean()
|
fieldRelative
|
||||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation())
|
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
|
||||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||||
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
||||||
@ -313,88 +233,12 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
|
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
|
||||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
|
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
|
||||||
-MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
|
-MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
|
||||||
() -> fieldRelativeControl,
|
true,
|
||||||
true
|
true
|
||||||
);
|
);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command driveCardinal(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier pov, double deadband) {
|
|
||||||
PIDController controller = new PIDController(
|
|
||||||
AutoConstants.kPHeadingController,
|
|
||||||
0,
|
|
||||||
AutoConstants.kDHeadingController
|
|
||||||
);
|
|
||||||
controller.enableContinuousInput(-180, 180);
|
|
||||||
|
|
||||||
return new PIDCommand(
|
|
||||||
controller,
|
|
||||||
this::getHeading,
|
|
||||||
pov::getAsDouble,
|
|
||||||
(output) -> {
|
|
||||||
this.drive(
|
|
||||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
|
|
||||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
|
|
||||||
output,
|
|
||||||
() -> fieldRelativeControl,
|
|
||||||
true
|
|
||||||
);
|
|
||||||
},
|
|
||||||
this);
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
|
|
||||||
if (m_aprilTagProvider == null) {
|
|
||||||
return new PrintCommand("No AprilTag Provider Available");
|
|
||||||
}
|
|
||||||
|
|
||||||
// TODO The process variable is different here than what these constants are used for, may need to use something different
|
|
||||||
PIDController controller = new PIDController(
|
|
||||||
AutoConstants.kPHeadingController,
|
|
||||||
0,
|
|
||||||
AutoConstants.kDHeadingController
|
|
||||||
);
|
|
||||||
|
|
||||||
return new PIDCommand(
|
|
||||||
controller,
|
|
||||||
() -> {
|
|
||||||
OptionalDouble tagYaw = m_aprilTagProvider.getTagYawByID(tagID);
|
|
||||||
|
|
||||||
if (tagYaw.isEmpty()) {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
return tagYaw.getAsDouble();
|
|
||||||
},
|
|
||||||
() -> { return 0; },
|
|
||||||
(output) -> {
|
|
||||||
// TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field, it shouldn't be affected by the drivers mode choice I don't think.
|
|
||||||
this.drive(
|
|
||||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
|
|
||||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
|
|
||||||
output,
|
|
||||||
() -> fieldRelativeControl,
|
|
||||||
true
|
|
||||||
);
|
|
||||||
},
|
|
||||||
this
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets the wheels into an X formation to prevent movement.
|
|
||||||
*/
|
|
||||||
public void setX() {
|
|
||||||
m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
|
|
||||||
m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
|
|
||||||
m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
|
|
||||||
m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command setXCommand() {
|
|
||||||
return runOnce(this::setX);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Sets the swerve ModuleStates.
|
* Sets the swerve ModuleStates.
|
||||||
*
|
*
|
||||||
@ -426,10 +270,6 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
return runOnce(this::zeroHeading);
|
return runOnce(this::zeroHeading);
|
||||||
}
|
}
|
||||||
|
|
||||||
public void offsetZero(Pose2d angle){
|
|
||||||
resetOdometry(angle);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the heading of the robot.
|
* Returns the heading of the robot.
|
||||||
*
|
*
|
||||||
|
|||||||
@ -1,60 +0,0 @@
|
|||||||
package frc.robot.subsystems;
|
|
||||||
|
|
||||||
import com.revrobotics.CANSparkMax;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import com.revrobotics.CANSparkBase.IdleMode;
|
|
||||||
import com.revrobotics.CANSparkLowLevel.MotorType;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import frc.robot.constants.IndexerConstants;
|
|
||||||
|
|
||||||
public class Indexer extends SubsystemBase{
|
|
||||||
|
|
||||||
private CANSparkMax indexerMotor;
|
|
||||||
private DigitalInput indexerBeamBreak;
|
|
||||||
|
|
||||||
public Indexer(){
|
|
||||||
indexerMotor = new CANSparkMax(IndexerConstants.kIndexerID, MotorType.kBrushed);
|
|
||||||
|
|
||||||
indexerMotor.setSmartCurrentLimit(40);
|
|
||||||
indexerMotor.setIdleMode(IdleMode.kBrake);
|
|
||||||
indexerMotor.burnFlash();
|
|
||||||
|
|
||||||
indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel);
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command autoIndexing(){
|
|
||||||
return run(() -> {
|
|
||||||
if(!indexerBeamBreak.get()){
|
|
||||||
indexerMotor.set(0.75);
|
|
||||||
}else if(indexerBeamBreak.get()){
|
|
||||||
indexerMotor.set(0.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command advanceNote(){
|
|
||||||
return run(() -> {
|
|
||||||
if(indexerBeamBreak.get()){
|
|
||||||
indexerMotor.set(0.75);
|
|
||||||
}else{
|
|
||||||
indexerMotor.set(0.75);
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command shootNote(DoubleSupplier indexerSpeed){
|
|
||||||
return run(() -> {
|
|
||||||
indexerMotor.set(indexerSpeed.getAsDouble());
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public boolean getBeamBreak(){
|
|
||||||
return indexerBeamBreak.get();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,115 +0,0 @@
|
|||||||
package frc.robot.subsystems;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import com.revrobotics.CANSparkMax;
|
|
||||||
import com.revrobotics.RelativeEncoder;
|
|
||||||
import com.revrobotics.CANSparkBase.IdleMode;
|
|
||||||
import com.revrobotics.CANSparkLowLevel.MotorType;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.controller.ArmFeedforward;
|
|
||||||
import edu.wpi.first.math.controller.PIDController;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import frc.robot.constants.IntakeConstants;
|
|
||||||
|
|
||||||
public class Intake extends SubsystemBase{
|
|
||||||
private PIDController intakeAnglePID;
|
|
||||||
|
|
||||||
private CANSparkMax intakeRoller;
|
|
||||||
private CANSparkMax intakePivot;
|
|
||||||
|
|
||||||
private RelativeEncoder intakeEncoder;
|
|
||||||
|
|
||||||
private ArmFeedforward intakeFeedForward;
|
|
||||||
|
|
||||||
public Intake(){
|
|
||||||
intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
|
|
||||||
|
|
||||||
intakeRoller.setSmartCurrentLimit(60);
|
|
||||||
intakeRoller.setIdleMode(IdleMode.kBrake);
|
|
||||||
intakeRoller.burnFlash();
|
|
||||||
|
|
||||||
intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
|
|
||||||
|
|
||||||
intakePivot.setIdleMode(IdleMode.kBrake);
|
|
||||||
intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit);
|
|
||||||
intakePivot.burnFlash();
|
|
||||||
|
|
||||||
intakeFeedForward = new ArmFeedforward(
|
|
||||||
IntakeConstants.kSFeedForward,
|
|
||||||
IntakeConstants.kGFeedForward,
|
|
||||||
IntakeConstants.kVFeedForward
|
|
||||||
);
|
|
||||||
|
|
||||||
intakeEncoder = intakePivot.getEncoder();
|
|
||||||
intakeEncoder.setPosition(IntakeConstants.kStartingAngle);
|
|
||||||
intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
|
|
||||||
|
|
||||||
intakeAnglePID = new PIDController(
|
|
||||||
IntakeConstants.kPIntake,
|
|
||||||
IntakeConstants.kIIntake,
|
|
||||||
IntakeConstants.kDIntake
|
|
||||||
);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command intakeDownCommand() {
|
|
||||||
return run(() -> {
|
|
||||||
intakeRoller.set(1.0);
|
|
||||||
|
|
||||||
intakePivot.setVoltage(
|
|
||||||
intakeAnglePID.calculate(
|
|
||||||
intakeEncoder.getPosition(),
|
|
||||||
IntakeConstants.kDownAngle
|
|
||||||
) + intakeFeedForward.calculate(
|
|
||||||
IntakeConstants.kDownAngle,
|
|
||||||
intakeEncoder.getVelocity()
|
|
||||||
)
|
|
||||||
);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){
|
|
||||||
return run(() ->{
|
|
||||||
intakePivot.set(pivotPower.getAsDouble());
|
|
||||||
intakeRoller.set(rollerSpinny.getAsDouble());
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command climbingStateCommand() {
|
|
||||||
return run(() -> {
|
|
||||||
intakeRoller.set(0.0);
|
|
||||||
|
|
||||||
intakePivot.setVoltage(
|
|
||||||
intakeAnglePID.calculate(
|
|
||||||
intakeEncoder.getPosition(),
|
|
||||||
IntakeConstants.kDownAngle
|
|
||||||
) + intakeFeedForward.calculate(
|
|
||||||
IntakeConstants.kDownAngle,
|
|
||||||
intakeEncoder.getVelocity()
|
|
||||||
)
|
|
||||||
);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command intakeUpCommand() {
|
|
||||||
return run(() -> {
|
|
||||||
intakeRoller.set(0.0);
|
|
||||||
|
|
||||||
intakePivot.setVoltage(
|
|
||||||
intakeAnglePID.calculate(
|
|
||||||
intakeEncoder.getPosition(),
|
|
||||||
IntakeConstants.kUpAngle
|
|
||||||
) + intakeFeedForward.calculate(
|
|
||||||
IntakeConstants.kUpAngle,
|
|
||||||
intakeEncoder.getVelocity()
|
|
||||||
)
|
|
||||||
);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public double getIntakeAngle(){
|
|
||||||
return intakeEncoder.getPosition();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,151 +0,0 @@
|
|||||||
package frc.robot.subsystems;
|
|
||||||
|
|
||||||
import java.util.function.BooleanSupplier;
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import com.revrobotics.CANSparkMax;
|
|
||||||
import com.revrobotics.CANSparkBase.IdleMode;
|
|
||||||
import com.revrobotics.CANSparkLowLevel.MotorType;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.controller.ArmFeedforward;
|
|
||||||
import edu.wpi.first.math.controller.PIDController;
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
|
||||||
import edu.wpi.first.wpilibj.Encoder;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import frc.robot.constants.ShooterConstants;
|
|
||||||
|
|
||||||
public class Shooter extends SubsystemBase{
|
|
||||||
private CANSparkMax topShooter;
|
|
||||||
private CANSparkMax bottomShooter;
|
|
||||||
|
|
||||||
private CANSparkMax shooterPivot;
|
|
||||||
|
|
||||||
private Encoder pivotEncoder;
|
|
||||||
|
|
||||||
private PIDController shooterPivotPID;
|
|
||||||
private ArmFeedforward shooterPivotFF;
|
|
||||||
|
|
||||||
private DigitalInput shooterBeamBreak;
|
|
||||||
|
|
||||||
private boolean indexerBeamBreak;
|
|
||||||
|
|
||||||
public Shooter(BooleanSupplier indexerBeamBreak){
|
|
||||||
topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
|
|
||||||
bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
|
|
||||||
|
|
||||||
shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
|
|
||||||
shooterPivot.setInverted(true);
|
|
||||||
|
|
||||||
/*
|
|
||||||
topPID = topShooter.getPIDController();
|
|
||||||
topPID.setFeedbackDevice(topEncoder);
|
|
||||||
bottomPID = bottomShooter.getPIDController();
|
|
||||||
bottomPID.setFeedbackDevice(bottomEncoder);
|
|
||||||
*/
|
|
||||||
|
|
||||||
shooterPivot.setSmartCurrentLimit(50);
|
|
||||||
topShooter.setSmartCurrentLimit(40);
|
|
||||||
bottomShooter.setSmartCurrentLimit(40);
|
|
||||||
|
|
||||||
pivotEncoder = new Encoder(0, 1);
|
|
||||||
pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
|
|
||||||
|
|
||||||
shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
|
|
||||||
|
|
||||||
topShooter.setIdleMode(IdleMode.kCoast);
|
|
||||||
bottomShooter.setIdleMode(IdleMode.kCoast);
|
|
||||||
|
|
||||||
bottomShooter.burnFlash();
|
|
||||||
shooterPivot.burnFlash();
|
|
||||||
topShooter.burnFlash();
|
|
||||||
|
|
||||||
shooterPivotPID = new PIDController(
|
|
||||||
ShooterConstants.kShooterPivotP,
|
|
||||||
ShooterConstants.kShooterPivotI,
|
|
||||||
ShooterConstants.kShooterPivotD
|
|
||||||
);
|
|
||||||
shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
|
|
||||||
return run(()-> {
|
|
||||||
angleAndSpeedControl(setpointAngle, topRPM, bottomRPM);
|
|
||||||
});
|
|
||||||
}*/
|
|
||||||
|
|
||||||
public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){
|
|
||||||
return run(()-> {
|
|
||||||
angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble());
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){
|
|
||||||
return run(()-> {
|
|
||||||
angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){
|
|
||||||
shooterPivot.setIdleMode(IdleMode.kBrake);
|
|
||||||
|
|
||||||
shooterPivot.setVoltage(
|
|
||||||
shooterPivotPID.calculate(getShooterAngle(), setpointAngle) +
|
|
||||||
shooterPivotFF.calculate(setpointAngle, 0.0));
|
|
||||||
|
|
||||||
//topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
|
|
||||||
//bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
|
|
||||||
topShooter.set(topRPM);
|
|
||||||
bottomShooter.set(bottomRPM);
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command ampHandoff(DoubleSupplier shootNoteSpeed, BooleanSupplier ampAngleOrNah){
|
|
||||||
return run(() -> {
|
|
||||||
|
|
||||||
shooterPivot.setIdleMode(IdleMode.kBrake);
|
|
||||||
|
|
||||||
shooterPivot.setVoltage(
|
|
||||||
shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) +
|
|
||||||
shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
|
|
||||||
|
|
||||||
if(shooterBeamBreak.get() || indexerBeamBreak){
|
|
||||||
angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.25, 0.25);
|
|
||||||
}else{
|
|
||||||
angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, shootNoteSpeed.getAsDouble(), shootNoteSpeed.getAsDouble());
|
|
||||||
}
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command climbState(){
|
|
||||||
return run(() -> {
|
|
||||||
shooterPivot.setIdleMode(IdleMode.kCoast);
|
|
||||||
shooterPivot.set(0.0);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public double getShooterAngle(){
|
|
||||||
return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle;
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command zeroEncoder(){
|
|
||||||
return run(() -> {
|
|
||||||
pivotEncoder.reset();
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Boolean getBeamBreak(){
|
|
||||||
return shooterBeamBreak.get();
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){
|
|
||||||
return run(() ->{
|
|
||||||
shooterPivot.set(pivotPower.getAsDouble());
|
|
||||||
topShooter.set(spinny.getAsDouble());
|
|
||||||
bottomShooter.set(spinny.getAsDouble());
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,135 +0,0 @@
|
|||||||
package frc.robot.utilities;
|
|
||||||
|
|
||||||
import java.io.IOException;
|
|
||||||
import java.util.List;
|
|
||||||
import java.util.Optional;
|
|
||||||
import java.util.OptionalDouble;
|
|
||||||
|
|
||||||
import org.photonvision.EstimatedRobotPose;
|
|
||||||
import org.photonvision.PhotonCamera;
|
|
||||||
import org.photonvision.PhotonPoseEstimator;
|
|
||||||
import org.photonvision.PhotonUtils;
|
|
||||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
|
||||||
import org.photonvision.targeting.PhotonPipelineResult;
|
|
||||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
|
||||||
|
|
||||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
|
||||||
import edu.wpi.first.apriltag.AprilTagFields;
|
|
||||||
import edu.wpi.first.math.geometry.Transform3d;
|
|
||||||
import edu.wpi.first.math.util.Units;
|
|
||||||
import frc.robot.interfaces.IAprilTagProvider;
|
|
||||||
import frc.robot.interfaces.IVisualPoseProvider;
|
|
||||||
|
|
||||||
public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
|
|
||||||
|
|
||||||
private final PhotonCamera camera;
|
|
||||||
|
|
||||||
private final PhotonPoseEstimator photonPoseEstimator;
|
|
||||||
|
|
||||||
private final double cameraHeightMeters;
|
|
||||||
private final double cameraPitchRadians;
|
|
||||||
|
|
||||||
public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
|
|
||||||
camera = new PhotonCamera(cameraName);
|
|
||||||
|
|
||||||
photonPoseEstimator = new PhotonPoseEstimator(
|
|
||||||
AprilTagFieldLayout.loadFromResource(
|
|
||||||
AprilTagFields.k2024Crescendo.m_resourceFile
|
|
||||||
),
|
|
||||||
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
|
|
||||||
camera,
|
|
||||||
robotToCam
|
|
||||||
);
|
|
||||||
|
|
||||||
this.cameraHeightMeters = cameraHeightMeters;
|
|
||||||
this.cameraPitchRadians = cameraPitchRadians;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public Optional<VisualPose> getVisualPose() {
|
|
||||||
Optional<EstimatedRobotPose> pose = photonPoseEstimator.update();
|
|
||||||
|
|
||||||
if (pose.isEmpty()) {
|
|
||||||
return Optional.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
return Optional.of(
|
|
||||||
new VisualPose(
|
|
||||||
pose.get().estimatedPose.toPose2d(),
|
|
||||||
pose.get().timestampSeconds
|
|
||||||
)
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
|
|
||||||
PhotonPipelineResult result = camera.getLatestResult();
|
|
||||||
|
|
||||||
if (!result.hasTargets()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
|
|
||||||
|
|
||||||
if (desiredTarget.isEmpty()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
return OptionalDouble.of(
|
|
||||||
PhotonUtils.calculateDistanceToTargetMeters(
|
|
||||||
cameraHeightMeters,
|
|
||||||
targetHeightMeters,
|
|
||||||
cameraPitchRadians,
|
|
||||||
Units.degreesToRadians(desiredTarget.get().getPitch()))
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public OptionalDouble getTagPitchByID(int id) {
|
|
||||||
PhotonPipelineResult result = camera.getLatestResult();
|
|
||||||
|
|
||||||
if (!result.hasTargets()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
|
|
||||||
|
|
||||||
if (desiredTarget.isEmpty()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
return OptionalDouble.of(
|
|
||||||
desiredTarget.get().getPitch()
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public OptionalDouble getTagYawByID(int id) {
|
|
||||||
PhotonPipelineResult result = camera.getLatestResult();
|
|
||||||
|
|
||||||
if (!result.hasTargets()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
|
|
||||||
|
|
||||||
if (desiredTarget.isEmpty()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
return OptionalDouble.of(
|
|
||||||
desiredTarget.get().getYaw()
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
|
|
||||||
for (PhotonTrackedTarget target : targets) {
|
|
||||||
if (target.getFiducialId() == id) {
|
|
||||||
return Optional.of(target);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return Optional.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
Loading…
Reference in New Issue
Block a user