Compare commits
7 Commits
master
...
swerve_tes
| Author | SHA1 | Date | |
|---|---|---|---|
| 213194cf7b | |||
| c02b8b0a3c | |||
| e0fd9dab1d | |||
| 40e53305e0 | |||
| cbb54706bf | |||
| a0a27008db | |||
| 626a241fff |
1
networktables.json
Normal file
1
networktables.json
Normal file
@ -0,0 +1 @@
|
||||
[]
|
||||
97
simgui-ds.json
Normal file
97
simgui-ds.json
Normal file
@ -0,0 +1,97 @@
|
||||
{
|
||||
"keyboardJoysticks": [
|
||||
{
|
||||
"axisConfig": [
|
||||
{
|
||||
"decKey": 65,
|
||||
"incKey": 68
|
||||
},
|
||||
{
|
||||
"decKey": 87,
|
||||
"incKey": 83
|
||||
},
|
||||
{
|
||||
"decKey": 69,
|
||||
"decayRate": 0.0,
|
||||
"incKey": 82,
|
||||
"keyRate": 0.009999999776482582
|
||||
}
|
||||
],
|
||||
"axisCount": 3,
|
||||
"buttonCount": 4,
|
||||
"buttonKeys": [
|
||||
90,
|
||||
88,
|
||||
67,
|
||||
86
|
||||
],
|
||||
"povConfig": [
|
||||
{
|
||||
"key0": 328,
|
||||
"key135": 323,
|
||||
"key180": 322,
|
||||
"key225": 321,
|
||||
"key270": 324,
|
||||
"key315": 327,
|
||||
"key45": 329,
|
||||
"key90": 326
|
||||
}
|
||||
],
|
||||
"povCount": 1
|
||||
},
|
||||
{
|
||||
"axisConfig": [
|
||||
{
|
||||
"decKey": 74,
|
||||
"incKey": 76
|
||||
},
|
||||
{
|
||||
"decKey": 73,
|
||||
"incKey": 75
|
||||
}
|
||||
],
|
||||
"axisCount": 2,
|
||||
"buttonCount": 4,
|
||||
"buttonKeys": [
|
||||
77,
|
||||
44,
|
||||
46,
|
||||
47
|
||||
],
|
||||
"povCount": 0
|
||||
},
|
||||
{
|
||||
"axisConfig": [
|
||||
{
|
||||
"decKey": 263,
|
||||
"incKey": 262
|
||||
},
|
||||
{
|
||||
"decKey": 265,
|
||||
"incKey": 264
|
||||
}
|
||||
],
|
||||
"axisCount": 2,
|
||||
"buttonCount": 6,
|
||||
"buttonKeys": [
|
||||
260,
|
||||
268,
|
||||
266,
|
||||
261,
|
||||
269,
|
||||
267
|
||||
],
|
||||
"povCount": 0
|
||||
},
|
||||
{
|
||||
"axisCount": 0,
|
||||
"buttonCount": 0,
|
||||
"povCount": 0
|
||||
}
|
||||
],
|
||||
"robotJoysticks": [
|
||||
{
|
||||
"guid": "Keyboard0"
|
||||
}
|
||||
]
|
||||
}
|
||||
17
simgui.json
Normal file
17
simgui.json
Normal file
@ -0,0 +1,17 @@
|
||||
{
|
||||
"NTProvider": {
|
||||
"types": {
|
||||
"/FMSInfo": "FMSInfo"
|
||||
}
|
||||
},
|
||||
"NetworkTables": {
|
||||
"transitory": {
|
||||
"Shuffleboard": {
|
||||
"open": true
|
||||
}
|
||||
}
|
||||
},
|
||||
"NetworkTables Info": {
|
||||
"visible": true
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -0,0 +1,25 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"startingPose": {
|
||||
"position": {
|
||||
"x": 0.7046435911053639,
|
||||
"y": 4.3751818412727745
|
||||
},
|
||||
"rotation": 120.0
|
||||
},
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "From Subwoofer Far Edge to Amp"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@ -2,8 +2,8 @@
|
||||
"version": 1.0,
|
||||
"startingPose": {
|
||||
"position": {
|
||||
"x": 0.9847983228729987,
|
||||
"y": 1.9646179463299098
|
||||
"x": 1.29,
|
||||
"y": 5.58
|
||||
},
|
||||
"rotation": 0
|
||||
},
|
||||
@ -14,7 +14,7 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Just Backup"
|
||||
"pathName": "From Subwoofer Front to Amp"
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.0,
|
||||
"y": 7.0
|
||||
"x": 0.7046435911053639,
|
||||
"y": 4.3751818412727745
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.0,
|
||||
"y": 7.0
|
||||
"x": 2.4368920788482864,
|
||||
"y": 3.1622483921028106
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.2654580294915574,
|
||||
"y": 6.291455090032687
|
||||
"x": 1.82409310273166,
|
||||
"y": 7.565612949407717
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.7098358983042754,
|
||||
"y": 6.525338178881486
|
||||
"x": 1.8176568333482528,
|
||||
"y": 6.336387277909188
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@ -39,13 +39,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -145.88552705465875,
|
||||
"rotation": 89.21368742867472,
|
||||
"rotateFast": false
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"previewStartingState": {
|
||||
"rotation": 179.55484341368705,
|
||||
"rotation": 122.73522627210761,
|
||||
"velocity": 0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.2888463383764373,
|
||||
"y": 6.642279723305886
|
||||
"x": 1.2945574370576298,
|
||||
"y": 5.580559282764361
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.2888463383764366,
|
||||
"y": 6.642279723305886
|
||||
"x": 2.2786077412442953,
|
||||
"y": 5.580559282764361
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.159911049166827,
|
||||
"y": 6.864468657712245
|
||||
"x": 1.8232420006879377,
|
||||
"y": 7.533755031731888
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.159911049166827,
|
||||
"y": 6.864468657712245
|
||||
"x": 1.8232420006879377,
|
||||
"y": 6.694612522270728
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@ -39,13 +39,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0,
|
||||
"rotation": 90.0,
|
||||
"rotateFast": false
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"previewStartingState": {
|
||||
"rotation": 21.801409486351982,
|
||||
"rotation": 0,
|
||||
"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.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,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,43 @@
|
||||
|
||||
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.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.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
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.PhotonConstants;
|
||||
import frc.robot.constants.ShooterConstants;
|
||||
import frc.robot.subsystems.Climber;
|
||||
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;
|
||||
import frc.robot.utilities.PoseSendable;
|
||||
|
||||
public class RobotContainer {
|
||||
private static final String kAutoTabName = "Autonomous";
|
||||
private static final String kTeleopTabName = "Teloperated";
|
||||
|
||||
//private PhotonVision photonvision;
|
||||
|
||||
private Drivetrain drivetrain;
|
||||
private Intake intake;
|
||||
private Shooter shooter;
|
||||
private Climber climber;
|
||||
private Indexer indexer;
|
||||
|
||||
private CommandXboxController driver;
|
||||
private CommandXboxController operator;
|
||||
|
||||
private Trigger isTeleopTrigger;
|
||||
private PoseSendable poseSendable;
|
||||
|
||||
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() {
|
||||
/*try {
|
||||
|
||||
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();
|
||||
drivetrain = new Drivetrain(new Pose2d());
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
||||
operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
|
||||
|
||||
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
|
||||
poseSendable = new PoseSendable();
|
||||
|
||||
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;
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
|
||||
configureBindings();
|
||||
shuffleboardSetup();
|
||||
shuffleboardConfig();
|
||||
}
|
||||
|
||||
// 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() {
|
||||
isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.teleopCommand(
|
||||
driver::getLeftY,
|
||||
@ -141,161 +50,36 @@ public class RobotContainer {
|
||||
)
|
||||
);
|
||||
|
||||
//intake.setDefaultCommand(intake.intakeUpCommand());
|
||||
intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, () -> 0.0));
|
||||
driver.a().onTrue(drivetrain.zeroHeadingCommand());
|
||||
|
||||
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;
|
||||
new InstantCommand(() -> {
|
||||
System.out.println("X: " + poseSendable.getX());
|
||||
System.out.println("Y: " + poseSendable.getY());
|
||||
System.out.println("Rotation: " + poseSendable.getRotation());
|
||||
drivetrain.resetOdometry(poseSendable.getPose());
|
||||
}
|
||||
})));
|
||||
|
||||
|
||||
//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));
|
||||
private void shuffleboardConfig() {
|
||||
ShuffleboardTab tab = Shuffleboard.getTab("Testing");
|
||||
tab.add("Drivetrain Pose", poseSendable)
|
||||
.withPosition(0, 0)
|
||||
.withSize(2, 3);
|
||||
|
||||
operator.start().onTrue(shooter.zeroEncoder());
|
||||
tab.add("Auto Selector", autoChooser)
|
||||
.withPosition(2, 0)
|
||||
.withSize(2, 1);
|
||||
|
||||
tab.addDouble("Current Pose Rotation", () -> { return drivetrain.getPose().getRotation().getDegrees(); })
|
||||
.withPosition(2, 1)
|
||||
.withSize(2, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return drivetrain.zeroHeadingCommand().andThen(autoChooser.getSelected());
|
||||
|
||||
}
|
||||
|
||||
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() {
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -24,11 +24,11 @@ public final class AutoConstants {
|
||||
new PIDConstants(kPXController),
|
||||
new PIDConstants(kPThetaController),
|
||||
kMaxSpeedMetersPerSecond,
|
||||
Units.inchesToMeters(Math.sqrt(Math.pow(14-17.5, 2)+ Math.pow(14-1.75, 2))),
|
||||
Units.inchesToMeters(Math.sqrt(Math.pow(14-1.75, 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;
|
||||
|
||||
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.filter.SlewRateLimiter;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
@ -13,8 +12,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
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 com.kauailabs.navx.frc.AHRS;
|
||||
@ -26,12 +23,7 @@ import frc.robot.constants.AutoConstants;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
import frc.robot.utilities.MAXSwerveModule;
|
||||
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.PIDCommand;
|
||||
import edu.wpi.first.wpilibj2.command.PrintCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Drivetrain extends SubsystemBase {
|
||||
@ -47,15 +39,9 @@ public class Drivetrain extends SubsystemBase {
|
||||
private final SlewRateLimiter m_magLimiter;
|
||||
private final SlewRateLimiter m_rotLimiter;
|
||||
|
||||
private final IAprilTagProvider m_aprilTagProvider;
|
||||
|
||||
private final IVisualPoseProvider m_visualPoseProvider;
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
private final SwerveDrivePoseEstimator m_poseEstimator;
|
||||
|
||||
private boolean fieldRelativeControl;
|
||||
|
||||
// Slew rate filter variables for controlling lateral acceleration
|
||||
private double m_currentRotation;
|
||||
private double m_currentTranslationDir;
|
||||
@ -63,10 +49,8 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
private double m_prevTime;
|
||||
|
||||
private double gyroOffset;
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public Drivetrain(Pose2d initialPose, IAprilTagProvider aprilTagProvider, IVisualPoseProvider visualPoseProvider) {
|
||||
public Drivetrain(Pose2d initialPose) {
|
||||
m_frontLeft = new MAXSwerveModule(
|
||||
DrivetrainConstants.kFrontLeftDrivingCanId,
|
||||
DrivetrainConstants.kFrontLeftTurningCanId,
|
||||
@ -108,12 +92,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
initialPose
|
||||
);
|
||||
|
||||
m_aprilTagProvider = aprilTagProvider;
|
||||
|
||||
m_visualPoseProvider = visualPoseProvider;
|
||||
|
||||
fieldRelativeControl = true;
|
||||
|
||||
m_currentRotation = 0.0;
|
||||
m_currentTranslationDir = 0.0;
|
||||
m_currentTranslationMag = 0.0;
|
||||
@ -135,8 +113,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
this
|
||||
);
|
||||
|
||||
gyroOffset = DrivetrainConstants.kRobotStartOffset;
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
@ -151,32 +127,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
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 +138,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
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.
|
||||
*
|
||||
@ -233,7 +175,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
* field.
|
||||
* @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 ySpeedCommanded;
|
||||
|
||||
@ -287,7 +229,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed;
|
||||
|
||||
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
fieldRelative.getAsBoolean()
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation())
|
||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
@ -313,88 +255,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
|
||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
|
||||
-MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
|
||||
() -> fieldRelativeControl,
|
||||
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.
|
||||
*
|
||||
@ -426,10 +292,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
return runOnce(this::zeroHeading);
|
||||
}
|
||||
|
||||
public void offsetZero(Pose2d angle){
|
||||
resetOdometry(angle);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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();
|
||||
}
|
||||
|
||||
}
|
||||
62
src/main/java/frc/robot/utilities/PoseSendable.java
Normal file
62
src/main/java/frc/robot/utilities/PoseSendable.java
Normal file
@ -0,0 +1,62 @@
|
||||
package frc.robot.utilities;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
|
||||
public class PoseSendable implements Sendable {
|
||||
|
||||
private double x;
|
||||
private double y;
|
||||
private double rot;
|
||||
|
||||
public PoseSendable() {
|
||||
this(0, 0, 0);
|
||||
}
|
||||
|
||||
public PoseSendable(double x, double y, double rot) {
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
this.rot = rot;
|
||||
}
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public double getRotation() {
|
||||
return rot;
|
||||
}
|
||||
|
||||
public Rotation2d getRotation2d() {
|
||||
return Rotation2d.fromDegrees(rot);
|
||||
}
|
||||
|
||||
public Pose2d getPose() {
|
||||
return new Pose2d(x, y, getRotation2d());
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public void setRotation(double rot) {
|
||||
this.rot = rot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.addDoubleProperty("X", this::getX, this::setX);
|
||||
builder.addDoubleProperty("Y", this::getY, this::setY);
|
||||
builder.addDoubleProperty("Rotation", this::getRotation, this::setRotation);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user