Compare commits
9 Commits
GreysonRew
...
RPM
| Author | SHA1 | Date | |
|---|---|---|---|
|
38db87047f | ||
|
|
4bb240138d | ||
|
|
0e5561dba0 | ||
|
|
cc41ea3f1b | ||
|
|
420fd90b12 | ||
|
|
670de7dd90 | ||
|
|
1b84701184 | ||
|
|
69279812f3 | ||
|
|
76e55fc5ec |
1
.DataLogTool/datalogtool.json
Normal file
1
.DataLogTool/datalogtool.json
Normal file
@@ -0,0 +1 @@
|
||||
{}
|
||||
1
.SysId/sysid.json
Normal file
1
.SysId/sysid.json
Normal file
@@ -0,0 +1 @@
|
||||
{}
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "test_90Degree_Turn"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
79
src/main/deploy/pathplanner/autos/Center Auto.auto
Normal file
79
src/main/deploy/pathplanner/autos/Center Auto.auto
Normal file
@@ -0,0 +1,79 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Deploy_Intake_Command"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Start_Intake_Wheels"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center to Floor Intake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Floor Intake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Floor Intake to Left Shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot_Fuel_Command"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Assist_Shooter"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Left Shoot to Offsetted-Left Climb"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Offsetted-Left to Left Climb"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift_Robot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
79
src/main/deploy/pathplanner/autos/Human Player Auto.auto
Normal file
79
src/main/deploy/pathplanner/autos/Human Player Auto.auto
Normal file
@@ -0,0 +1,79 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Deploy_Intake_Command"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Starting to HP"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.25
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HP to Hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot_Fuel_Command"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Assist_Shooter"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Stop_Shooter_Command"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Hub to side of Climb"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Side into Climb"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift_Robot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Start to Over Left Ramp"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Deploy_Intake_Command"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Start_Intake_Wheels"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Collect Left-Center Fuel"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Collect Left-Center Fuel to Shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot_Fuel_Command"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Assist_Shooter"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Left Shoot to Offsetted-Left Climb"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Offsetted-Left to Left Climb"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift_Robot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.528863636363636,
|
||||
"y": 5.086295454545454
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.539647727272727,
|
||||
"y": 5.818079545454545
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Middle Start Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1789090909090907,
|
||||
"y": 5.941761363636363
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.9929090909090905,
|
||||
"y": 5.261511363636364
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Left Intake Balls Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Middle Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.734045454548223,
|
||||
"y": 4.6843295454545455
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.147034090909091,
|
||||
"y": 5.282125000000001
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Collect Fuel Left Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.6837045454545447,
|
||||
"y": 4.591568181818182
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.6837045454545447,
|
||||
"y": 6.805519750430038
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Floor Intake to Shoot Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.6572327044025181,
|
||||
"rotationDegrees": -91.02718603450556
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -18.80579011375007
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Left Auto",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.734045454548223,
|
||||
"y": 7.054897727274547
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.734045454545454,
|
||||
"y": 5.075988636363636
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Over Left Ramp Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.734045454548223,
|
||||
"y": 4.6843295454545455
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.734045454548223,
|
||||
"y": 6.8982811140664015
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Collect Fuel Left Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Left Auto",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.5192727272727271,
|
||||
"y": 5.941761363636363
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.6630909090909087,
|
||||
"y": 4.591568181818182
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Floor Intake Fuel Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.6837045454545447,
|
||||
"y": 4.591568181818182
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.8490909090909089,
|
||||
"y": 5.745931818181819
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Floor Intake to Shoot Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -18.80579011375007
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Middle Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/Floor Intake.path
Normal file
54
src/main/deploy/pathplanner/paths/Floor Intake.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1789090909090907,
|
||||
"y": 5.941761363636363
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 0.5089659090909093,
|
||||
"y": 5.931454545454544
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Left Intake Balls Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.5192727272727271,
|
||||
"y": 5.941761363636363
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.9521590909090447,
|
||||
"y": 5.9314545454499195
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Floor Intake Fuel Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Middle Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/HP to Hub.path
Normal file
54
src/main/deploy/pathplanner/paths/HP to Hub.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.46,
|
||||
"y": 0.634
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 0.8135533905932739,
|
||||
"y": 0.9875533905932734
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.5,
|
||||
"y": 2.5
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.146446609406726,
|
||||
"y": 2.146446609406726
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Right Shoot Position Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 36.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "HP to Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/Hub to side of Climb.path
Normal file
54
src/main/deploy/pathplanner/paths/Hub to side of Climb.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.5,
|
||||
"y": 2.5
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.030153689607046,
|
||||
"y": 2.3289899283371653
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Right Shoot Position Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.14,
|
||||
"y": 2.046
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.6098463103929541,
|
||||
"y": 2.2170100716628345
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Offsetted-Climb Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "HP to Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 36.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.6837045454545447,
|
||||
"y": 4.591568181818182
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.4468863636391924,
|
||||
"y": 4.911079545452217
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Floor Intake to Shoot Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.14,
|
||||
"y": 5.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.467261363639192,
|
||||
"y": 4.653409090906762
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Ofsetted-Left Climb Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Middle Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -18.80579011375007
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/Left to Floor Intake.path
Normal file
54
src/main/deploy/pathplanner/paths/Left to Floor Intake.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.5185568181818176,
|
||||
"y": 7.219806818181819
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.5190340909090907,
|
||||
"y": 6.12728409090909
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1789090909090907,
|
||||
"y": 5.941761363636363
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.9929090909090905,
|
||||
"y": 6.910602272727273
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Left Intake Balls Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Middle Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.0,
|
||||
"y": 7.0
|
||||
"x": 1.14,
|
||||
"y": 5.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.75,
|
||||
"y": 7.0
|
||||
"x": 1.14,
|
||||
"y": 4.292625343835472
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "Ofsetted-Left Climb Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.0,
|
||||
"y": 7.0
|
||||
"x": 1.14,
|
||||
"y": 4.25
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.48768579117494,
|
||||
"y": 7.0
|
||||
"x": 1.14,
|
||||
"y": 4.70494908797556
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,7 +33,7 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 2.0,
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
"rotation": 90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Middle Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
"rotation": 90.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/Side into Climb.path
Normal file
54
src/main/deploy/pathplanner/paths/Side into Climb.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.14,
|
||||
"y": 2.046
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.1561466905187832,
|
||||
"y": 2.7126296958855094
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Offsetted-Right Climb Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.14,
|
||||
"y": 3.25
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.146147727272727,
|
||||
"y": 2.940795454545455
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Right Climb Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "HP to Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.5185568181818176,
|
||||
"y": 7.2095
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.1273750000027687,
|
||||
"y": 5.385193181820003
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.672920454548223,
|
||||
"y": 5.477954545456365
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.3539345896947985,
|
||||
"y": 5.471848129415376
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 6.899193181820951,
|
||||
"y": 5.488261363638184
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.734045454548223,
|
||||
"y": 7.054897727274547
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.971579545454545,
|
||||
"y": 7.271340909090909
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Over Left Ramp Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Left Auto",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/Starting to HP.path
Normal file
54
src/main/deploy/pathplanner/paths/Starting to HP.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.5159570661896247,
|
||||
"y": 0.8
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.6424772727272727,
|
||||
"y": 0.7471249999999998
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Start Right Trench Waypoint"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.46,
|
||||
"y": 0.634
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.0119022424324617,
|
||||
"y": 0.6340000000000003
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Waypoint"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "HP to Shoot to Climb",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -4,24 +4,24 @@
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.0,
|
||||
"y": 7.0
|
||||
"y": 1.87
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.0,
|
||||
"y": 7.0
|
||||
"x": 1.9999999999999858,
|
||||
"y": 1.87
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.0,
|
||||
"y": 7.0
|
||||
"x": 3.0,
|
||||
"y": 1.87
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.8572206587126847,
|
||||
"y": 7.0
|
||||
"x": 1.8572206587126847,
|
||||
"y": 1.8700000000000003
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
|
||||
@@ -2,7 +2,11 @@
|
||||
"robotWidth": 0.9,
|
||||
"robotLength": 0.9,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [],
|
||||
"pathFolders": [
|
||||
"HP to Shoot to Climb",
|
||||
"Middle Shoot to Climb",
|
||||
"Left Auto"
|
||||
],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 3.0,
|
||||
"defaultMaxAccel": 3.0,
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
"front": -10.9,
|
||||
"left": -10.9
|
||||
},
|
||||
"absoluteEncoderOffset": 287.578125,
|
||||
"absoluteEncoderOffset": 283.7109375,
|
||||
"drive": {
|
||||
"type": "sparkflex_neo",
|
||||
"id": 11,
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
"front": 10.9,
|
||||
"left": 10.9
|
||||
},
|
||||
"absoluteEncoderOffset": 36.03515625,
|
||||
"absoluteEncoderOffset": 34.716796875,
|
||||
"drive": {
|
||||
"type": "sparkflex_neo",
|
||||
"id": 13,
|
||||
|
||||
@@ -4,13 +4,19 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.Vector;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
|
||||
import com.pathplanner.lib.path.PathConstraints;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
@@ -37,7 +43,7 @@ public final class Constants {
|
||||
public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound
|
||||
public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
|
||||
public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag
|
||||
public static final double MAX_SPEED_MPS = Units.feetToMeters(14.5);
|
||||
public static final double MAX_SPEED = Units.feetToMeters(14.5);
|
||||
// Maximum speed of the robot in meters per second, used to limit acceleration.
|
||||
|
||||
// public static final class AutonConstants
|
||||
@@ -64,129 +70,162 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static class ShooterConstants {
|
||||
private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000)
|
||||
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
|
||||
// private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000)
|
||||
// .withWidget(BuiltInWidgets.kNumberBar).getEntry();
|
||||
|
||||
public static double SHOOTER_RPM;
|
||||
public static double SHOOTER_RPM = -2700;
|
||||
public static final double IDLE_SHOOTER_RPM = -1800;
|
||||
public static final double AUTO_SHOOTER_RPM = -3100; //always negative to shoot
|
||||
|
||||
public static void updateShooterRPM() {
|
||||
SHOOTER_RPM = shooterRPM.getDouble(-4000);
|
||||
}
|
||||
// public static void updateShooterRPM() {
|
||||
// SHOOTER_RPM = shooterRPM.getDouble(-4000);
|
||||
// }
|
||||
|
||||
public static final int CENTER_SHOOTER_MOTOR_ID = 42;
|
||||
public static final int LEFT_SHOOTER_MOTOR_ID = 41;
|
||||
public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
|
||||
public static final int INDEXER_MOTOR_ID = 43;
|
||||
|
||||
public static final int SHOOTER_MOTOR_CURRENT_LIMIT = 80;
|
||||
|
||||
public static final double SHOOTER_MOTOR_P = 0.0018;
|
||||
public static final double SHOOTER_MOTOR_I = 0;
|
||||
public static final double SHOOTER_MOTOR_D = 0;
|
||||
public static double SHOOTER_MOTOR_P = 0.001;
|
||||
public static double SHOOTER_MOTOR_I = 0;
|
||||
public static double SHOOTER_MOTOR_D = 0.001;
|
||||
public static double SHOOTER_MOTOR_S = 0.2;
|
||||
public static double SHOOTER_MOTOR_V = 0.0015;
|
||||
|
||||
public static final double INDEXER_MOTOR_P = 0.0001;
|
||||
public static final double INDEXER_MOTOR_I = 0;
|
||||
public static final double INDEXER_MOTOR_D = 0;
|
||||
public static double INDEXER_MOTOR_P = 0.0001;
|
||||
public static double INDEXER_MOTOR_I = 0;
|
||||
public static double INDEXER_MOTOR_D = 0;
|
||||
|
||||
public static final double CENTER_MOTOR_S = 0.0;
|
||||
public static final double CENTER_MOTOR_V = 0.0;
|
||||
/*
|
||||
* private static GenericEntry indexerAndRampRPM =
|
||||
* programmingTab.add("Desired Ramp + Indexer RPM", 2000)
|
||||
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
|
||||
*/
|
||||
|
||||
public static final double LEFT_MOTOR_S = 0.0;
|
||||
public static final double LEFT_MOTOR_V = 0.0;
|
||||
|
||||
public static final double RIGHT_MOTOR_S = 0.0;
|
||||
public static final double RIGHT_MOTOR_V = 0.0;
|
||||
|
||||
|
||||
/*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000)
|
||||
.withWidget(BuiltInWidgets.kNumberBar).getEntry();*/
|
||||
|
||||
|
||||
public static double INDEXER_AND_RAMP_MOTOR_RPM = 10000;
|
||||
public static double INDEXER_AND_RAMP_MOTOR_RPM = 15000; //always positive when feeding into shooter
|
||||
|
||||
// this method called in robot periodic so values updated in elastic are
|
||||
// constantly read and applied to RAMP_MOTOR_SPEED
|
||||
/*public static void updateIndexerAndRampMotorRPM() {
|
||||
INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000);
|
||||
}*/
|
||||
/*
|
||||
* public static void updateIndexerAndRampMotorRPM() {
|
||||
* INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000);
|
||||
* }
|
||||
*/
|
||||
}
|
||||
|
||||
public static class IntakeConstants {
|
||||
|
||||
/* private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000)
|
||||
.withWidget(BuiltInWidgets.kNumberBar).getEntry(); */
|
||||
public static double INTAKE_WHEELS_MOTOR_RPM = -7000;
|
||||
/*
|
||||
* private static GenericEntry intakeRPM =
|
||||
* programmingTab.add("Desired Intake RPM", -1000)
|
||||
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
|
||||
*/
|
||||
public static double INTAKE_WHEELS_MOTOR_RPM_FAST = -8000; // always negative when intaking
|
||||
public static double INTAKE_WHEELS_MOTOR_RPM_SLOW = -5000; // always negative when intaking
|
||||
|
||||
/*public static void updateIntakeWheelsRPM() {
|
||||
INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
|
||||
}*/
|
||||
|
||||
/*
|
||||
* public static void updateIntakeWheelsRPM() {
|
||||
* INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
|
||||
* }
|
||||
*/
|
||||
|
||||
public static final int INTAKE_WHEELS_MOTOR_ID = 50;
|
||||
public static final int INTAKE_WHEELS_CURRENT_LIMIT = 60;
|
||||
public static final double INTAKE_WHEELS_POSITION_CONVERSION_FACTOR = 2 * Math.PI; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians
|
||||
public static final double INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR = INTAKE_WHEELS_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second
|
||||
|
||||
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
|
||||
public static final int INTAKE_ROTATOR_CURRENT_LIMIT = 40;
|
||||
public static final double INTAKE_ROTATOR_INITIAL_ENCODER_VALUE = Units.degreesToRadians(-90);
|
||||
public static final double INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR = (2 * Math.PI) / 12.0; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians. Gear ratio is 12:1, so 12 Rotations = 1 full intake Rotation.
|
||||
public static final double INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR = INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second
|
||||
|
||||
public static final double INTAKE_WHEELS_MOTOR_P = 0.0001; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_WHEELS_MOTOR_I = 0.0; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_WHEELS_MOTOR_D = 0.0; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_WHEELS_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia
|
||||
public static final double INTAKE_WHEELS_MOTOR_V = 0.0; // Voltage per Rads/Second gain
|
||||
public static class IntakeRotatorPID {
|
||||
public static final double INTAKE_ROTATOR_P = .07;
|
||||
public static final double INTAKE_ROTATOR_I = 0;
|
||||
public static final double INTAKE_ROTATOR_D = 0.03;
|
||||
}
|
||||
|
||||
public static final double INTAKE_ROTATOR_DOWN_P = 0.03; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_ROTATOR_DOWN_I = 0.0; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_ROTATOR_DOWN_D = 0.0; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_MOTOR_P = 0.0001;
|
||||
public static final double INTAKE_MOTOR_I = 0;
|
||||
public static final double INTAKE_MOTOR_D = 0.00005;
|
||||
|
||||
public static final double INTAKE_ROTATOR_UP_P = 0.06; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_ROTATOR_UP_I = 0.0; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_ROTATOR_UP_D = 0.0; // Radians -> Radians Per Second
|
||||
public static final double INTAKE_COLLECT_ENCODER_VALUE = 1;
|
||||
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -4;
|
||||
public static final double INTAKE_RETRACT_ENCODER_VALUE = -6.2;
|
||||
|
||||
public static final double INTAKE_ROTATOR_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia. V
|
||||
public static final double INTAKE_ROTATOR_MOTOR_G = 0.0; // Voltage to overcome Gravity. V
|
||||
public static final double INTAKE_ROTATOR_MOTOR_V = 0.0; // Voltage per Radians/Second. V/(Rad/s)
|
||||
public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .13;
|
||||
public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .49;
|
||||
public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .389;
|
||||
|
||||
public static final double INTAKE_COLLECT_POSITION_RADS = 0.0;
|
||||
public static final double INTAKE_MIDDLE_POSITION_RADS = -Units.degreesToRadians(45);
|
||||
public static final double INTAKE_RETRACT_POSITION_RADS = -Units.degreesToRadians(90);
|
||||
public static final double INTAKE_MANUAL_SPEED = .15;
|
||||
}
|
||||
|
||||
|
||||
// create object and a new widget under programming tab in Elastic where object
|
||||
// retrieves value from widget
|
||||
|
||||
|
||||
// create object and a new widget under programming tab in Elastic where object
|
||||
// retrieves value from widget
|
||||
|
||||
public static class TargetingConstants {
|
||||
public static final Pose2d RIGHT_CLIMB_POSE_METERS = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
|
||||
public static final Pose2d LEFT_CLIMB_POSE_METERS = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
|
||||
public static final Pose2d BLUE_LEFT_CLIMB_POSE = new Pose2d(1.14, 4.25, Rotation2d.fromDegrees(90));
|
||||
public static final Pose2d BLUE_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(1.14, 5.0,
|
||||
Rotation2d.fromDegrees(90));
|
||||
|
||||
public static final Vector<N3> SINGLE_TAG_STD_DEVS = VecBuilder.fill(4, 4, 8);
|
||||
public static final Vector<N3> MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1);
|
||||
public static final Pose2d BLUE_RIGHT_CLIMB_POSE = new Pose2d(1.14, 2.046, Rotation2d.fromDegrees(-90));
|
||||
public static final Pose2d BLUE_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(1.14, 3.25,
|
||||
Rotation2d.fromDegrees(-90));
|
||||
|
||||
public static final Pose2d HUB_POSE_METERS = new Pose2d(4.625, 4.03, new Rotation2d());
|
||||
public static final Pose2d RED_RIGHT_CLIMB_POSE = new Pose2d(15.474, 5.22, Rotation2d.fromDegrees(90));
|
||||
public static final Pose2d RED_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 5.72,
|
||||
Rotation2d.fromDegrees(90));
|
||||
|
||||
public static final Translation3d FRONT_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
|
||||
public static final Translation3d FRONT_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
|
||||
public static final Translation3d REAR_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
|
||||
public static final Translation3d REAR_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
|
||||
public static final Pose2d RED_LEFT_CLIMB_POSE = new Pose2d(15.474, 3.437, Rotation2d.fromDegrees(-90));
|
||||
public static final Pose2d RED_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 2.937,
|
||||
Rotation2d.fromDegrees(-90));
|
||||
|
||||
public static final Rotation3d FRONT_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
|
||||
public static final Rotation3d FRONT_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
|
||||
public static final Rotation3d REAR_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
|
||||
public static final Rotation3d REAR_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
|
||||
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Back Left Camera");
|
||||
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Back Right Camera");
|
||||
public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera");
|
||||
public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera");
|
||||
|
||||
public static Rotation2d HUB_ROTATION_BLUE;
|
||||
public static Rotation2d HUB_ROTATION_RED;
|
||||
public static Pose2d allianceHubPose = new Pose2d();
|
||||
|
||||
|
||||
public static final double HUB_X_POSE_BLUE = 4.625;
|
||||
public static final double HUB_Y_POSE_BLUE = 4.03;
|
||||
|
||||
public static final double HUB_X_POSE_RED = 11.915;
|
||||
public static final double HUB_Y_POSE_RED = 4.03;
|
||||
|
||||
public static PathConstraints DRIVE_INTO_CLIMB_CONSTRAINTS;
|
||||
|
||||
public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d(
|
||||
new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25),
|
||||
Units.inchesToMeters(13.1875)),
|
||||
new Rotation3d(0, 0, Math.toRadians(195)));
|
||||
|
||||
public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d(
|
||||
new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)),
|
||||
new Rotation3d(0, 0, Math.toRadians(-200)));
|
||||
|
||||
public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d(
|
||||
new Translation3d(Units.inchesToMeters(1.3), Units.inchesToMeters(11.25), Units.inchesToMeters(26)),
|
||||
new Rotation3d(0, Math.toRadians(10), 0));
|
||||
|
||||
public static final Transform3d ROBOT_TO_FRONT_RIGHT_CAM = new Transform3d(
|
||||
new Translation3d(Units.inchesToMeters(1.5), Units.inchesToMeters(-11.25), Units.inchesToMeters(26)),
|
||||
new Rotation3d(0, Math.toRadians(-10), 0));
|
||||
}
|
||||
|
||||
public static class ClimberConstants {
|
||||
public static final int CLIMB_MOTOR_ID = 60;
|
||||
public static final int RATCHET_PWM_PORT = 9;
|
||||
public static final int CLIMB_MOTOR_ID = 52;
|
||||
|
||||
public static final double CLIMBER_SPEED = .5;
|
||||
|
||||
public static final double CLIMBER_PID_P = 5;
|
||||
public static final double CLIMBER_PID_I = 0;
|
||||
public static final double CLIMBER_PID_D = 0;
|
||||
|
||||
public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 65;
|
||||
public static final double CLIMBER_LOWERED_SETPOINT_VALUE = 0;
|
||||
|
||||
}
|
||||
|
||||
public static class LEDConstants {
|
||||
public static final int LED_PWM_PORT = 6;
|
||||
|
||||
public static final double RATCHET_UNLOCK_ANGLE_DEGREES = 0;
|
||||
public static final double RATCHET_LOCK_ANGLE_DEGREES = 180;
|
||||
public static final double CLIMBER_SPEED_DUTY_CYCLE = .5;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,11 +4,32 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import org.photonvision.PhotonUtils;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.networktables.StructPublisher;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
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.CommandScheduler;
|
||||
import frc.robot.Constants.IntakeConstants;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
import frc.robot.subsystems.IntakeSubsystem;
|
||||
import frc.robot.subsystems.LEDSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.subsystems.TargetingSubsystems;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
@@ -22,9 +43,14 @@ public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private RobotContainer m_robotContainer;
|
||||
private LEDSubsystem m_LedSubsystem;
|
||||
private ClimberSubsystem m_ClimberSubsystem;
|
||||
private ShooterSubsystem m_ShooterSubsystem;
|
||||
|
||||
private Timer disabledTimer;
|
||||
private static NetworkTable table;
|
||||
|
||||
private static StructPublisher<Pose2d> advantageScopePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose hahaha", Pose2d.struct).publish();
|
||||
|
||||
public Robot() {
|
||||
instance = this;
|
||||
@@ -43,8 +69,11 @@ public class Robot extends TimedRobot {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings,
|
||||
// and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
m_LedSubsystem = new LEDSubsystem();
|
||||
m_robotContainer = new RobotContainer();
|
||||
m_ClimberSubsystem = new ClimberSubsystem();
|
||||
m_ShooterSubsystem = new ShooterSubsystem();
|
||||
// Create a timer to disable motor brake a few seconds after disable. This will
|
||||
// let the robot stop
|
||||
// immediately when disabled, but then also let it be pushed more
|
||||
@@ -55,6 +84,7 @@ public class Robot extends TimedRobot {
|
||||
}
|
||||
|
||||
m_robotContainer.getSwerveDrive().zeroGyroWithAlliance();
|
||||
IntakeSubsystem.resetIntakeRotationEncoder();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -77,11 +107,16 @@ public class Robot extends TimedRobot {
|
||||
// robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
TargetingSubsystems.updateShooterRPM(m_robotContainer.getSwerveDrive().getPose());
|
||||
|
||||
SmartDashboard.putNumber("Estimated Shooter RPM", Constants.ShooterConstants.SHOOTER_RPM);
|
||||
SmartDashboard.putNumber("Distance From Hub: ", PhotonUtils.getDistanceToPose(m_robotContainer.getSwerveDrive().getPose(), Constants.TargetingConstants.allianceHubPose));
|
||||
//Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
|
||||
//Constants.IntakeConstants.updateIntakeWheelsRPM();
|
||||
Constants.ShooterConstants.updateShooterRPM();
|
||||
//Constants.ShooterConstants.updateShooterRPM();
|
||||
TargetingSubsystems.getHubPoseTheta(m_robotContainer.getSwerveDrive());
|
||||
//Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
|
||||
advantageScopePublisher.set(m_robotContainer.getSwerveDrive().getPose());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -109,8 +144,10 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
// IntakeSubsystem.resetIntakeRotationEncoder();
|
||||
m_robotContainer.setMotorBrake(true);
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
//ShooterSubsystem.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM);
|
||||
|
||||
// Print the selected autonomous command upon autonomous init
|
||||
System.out.println("Auto selected: " + m_autonomousCommand);
|
||||
@@ -119,6 +156,9 @@ public class Robot extends TimedRobot {
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -130,6 +170,8 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
ClimberSubsystem.lowerRobot();
|
||||
//ShooterSubsystem.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM);
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
|
||||
@@ -4,9 +4,12 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import com.fasterxml.jackson.databind.util.Named;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
@@ -16,21 +19,34 @@ import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
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.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import frc.robot.Constants.OperatorConstants;
|
||||
import frc.robot.subsystems.IntakeSubsystem;
|
||||
import frc.robot.subsystems.LEDSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
|
||||
import java.io.File;
|
||||
import java.lang.annotation.Target;
|
||||
import java.util.concurrent.atomic.AtomicBoolean;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import javax.lang.model.util.ElementScanner14;
|
||||
|
||||
import frc.robot.subsystems.TargetingSubsystems;
|
||||
import swervelib.SwerveInputStream;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
import frc.robot.subsystems.swervedrive.Vision;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
@@ -42,256 +58,334 @@ import frc.robot.subsystems.ClimberSubsystem;
|
||||
*/
|
||||
public class RobotContainer {
|
||||
|
||||
// Replace with CommandPS4Controller or CommandJoystick if needed
|
||||
final CommandXboxController driverXbox = new CommandXboxController(0);
|
||||
final CommandXboxController operatorXbox = new CommandXboxController(1);
|
||||
// Replace with CommandPS4Controller or CommandJoystick if needed
|
||||
final CommandXboxController driverXbox = new CommandXboxController(0);
|
||||
final CommandXboxController operatorXbox = new CommandXboxController(1);
|
||||
private final static CommandJoystick topButtons = new CommandJoystick(2);
|
||||
final CommandJoystick bottomButtons = new CommandJoystick(3);
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
|
||||
"swerve/neo"));
|
||||
// The robot's subsystems and commands are defined here...
|
||||
private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
|
||||
"swerve/neo"));
|
||||
|
||||
// Establish a Sendable Chooser that will be able to be sent to the
|
||||
// SmartDashboard, allowing selection of desired auto
|
||||
private final SendableChooser<Command> autoChooser;
|
||||
// Establish a Sendable Chooser that will be able to be sent to the
|
||||
// SmartDashboard, allowing selection of desired auto
|
||||
private final SendableChooser<Command> autoChooser;
|
||||
|
||||
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
|
||||
// private final TargetingSubsystems m_TargetingSubsystems = new
|
||||
// TargetingSubsystems();
|
||||
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
|
||||
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
|
||||
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
|
||||
private final TargetingSubsystems m_TargetingSubsystems = new TargetingSubsystems();
|
||||
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
|
||||
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
|
||||
/**
|
||||
* Converts driver input into a field-relative ChassisSpeeds that is controlled
|
||||
* by angular velocity.
|
||||
*/
|
||||
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
|
||||
() -> driverXbox.getLeftY() * -1,
|
||||
() -> driverXbox.getLeftX() * -1)
|
||||
.withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
|
||||
.deadband(OperatorConstants.DEADBAND)
|
||||
.scaleTranslation(0.8)
|
||||
.allianceRelativeControl(true);
|
||||
|
||||
/**
|
||||
* Converts driver input into a field-relative ChassisSpeeds that is controlled
|
||||
* by angular velocity.
|
||||
*/
|
||||
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
|
||||
() -> driverXbox.getLeftY() * -1,
|
||||
() -> driverXbox.getLeftX() * -1)
|
||||
.withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
|
||||
.deadband(OperatorConstants.DEADBAND)
|
||||
.scaleTranslation(0.8)
|
||||
.allianceRelativeControl(true);
|
||||
/**
|
||||
* Clone's the angular velocity input stream and converts it to a fieldRelative
|
||||
* input stream.
|
||||
*/
|
||||
SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()
|
||||
.withControllerHeadingAxis(driverXbox::getRightX,
|
||||
driverXbox::getRightY)
|
||||
.headingWhile(true);
|
||||
|
||||
/**
|
||||
* Clone's the angular velocity input stream and converts it to a fieldRelative
|
||||
* input stream.
|
||||
*/
|
||||
SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()
|
||||
.withControllerHeadingAxis(driverXbox::getRightX,
|
||||
driverXbox::getRightY)
|
||||
.headingWhile(true);
|
||||
/**
|
||||
* Clone's the angular velocity input stream and converts it to a robotRelative
|
||||
* input stream.
|
||||
*/
|
||||
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
|
||||
.allianceRelativeControl(true);
|
||||
|
||||
/**
|
||||
* Clone's the angular velocity input stream and converts it to a robotRelative
|
||||
* input stream.
|
||||
*/
|
||||
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
|
||||
.allianceRelativeControl(true);
|
||||
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
|
||||
() -> -driverXbox.getLeftY(),
|
||||
() -> -driverXbox.getLeftX())
|
||||
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
|
||||
2))
|
||||
.deadband(OperatorConstants.DEADBAND)
|
||||
.scaleTranslation(0.8)
|
||||
.allianceRelativeControl(true);
|
||||
// Derive the heading axis with math!
|
||||
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
|
||||
.withControllerHeadingAxis(() -> Math.sin(
|
||||
driverXbox.getRawAxis(
|
||||
2) *
|
||||
Math.PI)
|
||||
*
|
||||
(Math.PI *
|
||||
2),
|
||||
() -> Math.cos(
|
||||
driverXbox.getRawAxis(
|
||||
2) *
|
||||
Math.PI)
|
||||
*
|
||||
(Math.PI *
|
||||
2))
|
||||
.headingWhile(true)
|
||||
.translationHeadingOffset(true)
|
||||
.translationHeadingOffset(Rotation2d.fromDegrees(
|
||||
0));
|
||||
|
||||
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
|
||||
() -> -driverXbox.getLeftY(),
|
||||
() -> -driverXbox.getLeftX())
|
||||
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
|
||||
2))
|
||||
.deadband(OperatorConstants.DEADBAND)
|
||||
.scaleTranslation(0.8)
|
||||
.allianceRelativeControl(true);
|
||||
// Derive the heading axis with math!
|
||||
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
|
||||
.withControllerHeadingAxis(() -> Math.sin(
|
||||
driverXbox.getRawAxis(
|
||||
2) *
|
||||
Math.PI)
|
||||
*
|
||||
(Math.PI *
|
||||
2),
|
||||
() -> Math.cos(
|
||||
driverXbox.getRawAxis(
|
||||
2) *
|
||||
Math.PI)
|
||||
*
|
||||
(Math.PI *
|
||||
2))
|
||||
.headingWhile(true)
|
||||
.translationHeadingOffset(true)
|
||||
.translationHeadingOffset(Rotation2d.fromDegrees(
|
||||
0));
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
// Configure the trigger bindings
|
||||
configureBindings();
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
// Configure the trigger bindings
|
||||
configureBindings();
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// Create the NamedCommands that will be used in PathPlanner
|
||||
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
|
||||
NamedCommands.registerCommand("Start_Intake_Wheels", m_IntakeSubsystem.startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST));
|
||||
NamedCommands.registerCommand("Shoot_Fuel_Command",
|
||||
m_ShooterSubsystem.setShooterMotorsRPMAutoCommand());
|
||||
NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand()
|
||||
.andThen(m_IntakeSubsystem.startIntakeMotorCommand(
|
||||
Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_SLOW)));
|
||||
NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()
|
||||
.andThen(m_IntakeSubsystem.deployIntakeCommand()));
|
||||
NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand());
|
||||
NamedCommands.registerCommand("Assist_Shooter",
|
||||
m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(5));
|
||||
NamedCommands.registerCommand("Lift_Robot", m_ClimberSubsystem.liftRobotCommand());
|
||||
NamedCommands.registerCommand("Kill_All", killAllCommand());
|
||||
NamedCommands.registerCommand("Auto_Aim_To_Hub",
|
||||
m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
|
||||
|
||||
|
||||
/*NamedCommands.registerCommand("PathPlan_To_Climb_Right_Offsetted",
|
||||
drivebase.driveToClimbPoseOffsetted(
|
||||
Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED,
|
||||
Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
|
||||
NamedCommands.registerCommand("PathPlan_To_Climb_Left_Offsetted",
|
||||
drivebase.driveToClimbPoseOffsetted(
|
||||
Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED,
|
||||
Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
|
||||
NamedCommands.registerCommand("PathPlan_Into_Climb_Right",
|
||||
drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE,
|
||||
Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE));
|
||||
NamedCommands.registerCommand("PathPlan_Into_Climb_Left",
|
||||
drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE,
|
||||
Constants.TargetingConstants.RED_LEFT_CLIMB_POSE));*/
|
||||
|
||||
// Create the NamedCommands that will be used in PathPlanner
|
||||
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
|
||||
// Have the autoChooser pull in all PathPlanner autos as options
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
|
||||
// Have the autoChooser pull in all PathPlanner autos as options
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
// Set the default auto (do nothing)
|
||||
autoChooser.setDefaultOption("Do Nothing", Commands.none());
|
||||
|
||||
// Set the default auto (do nothing)
|
||||
autoChooser.setDefaultOption("Do Nothing", Commands.none());
|
||||
// Add a simple auto option to have the robot drive forward for 1 second then
|
||||
// stop
|
||||
autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
|
||||
|
||||
// Add a simple auto option to have the robot drive forward for 1 second then
|
||||
// stop
|
||||
autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
|
||||
|
||||
// Put the autoChooser on the SmartDashboard
|
||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your trigger->command mappings. Triggers can be
|
||||
* created via the
|
||||
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
|
||||
* an arbitrary predicate, or via the
|
||||
* named factories in
|
||||
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
|
||||
* for
|
||||
* {@link CommandXboxController
|
||||
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
|
||||
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
|
||||
* Flight joysticks}.
|
||||
*/
|
||||
private void configureBindings() {
|
||||
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
|
||||
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
|
||||
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
|
||||
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
|
||||
driveDirectAngle);
|
||||
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
|
||||
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
|
||||
.driveFieldOriented(driveAngularVelocityKeyboard);
|
||||
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
|
||||
driveDirectAngleKeyboard);
|
||||
|
||||
driverXbox.leftTrigger().whileTrue(m_IntakeSubsystem.startIntakeMotorCommand())
|
||||
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
|
||||
driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand().andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand()))
|
||||
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()));
|
||||
// command for
|
||||
// full shooting system including linear actuators
|
||||
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
|
||||
// .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
|
||||
|
||||
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
|
||||
|
||||
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand())
|
||||
.onFalse(m_ClimberSubsystem.stopClimberCommand());
|
||||
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand())
|
||||
.onFalse(m_ClimberSubsystem.stopClimberCommand());
|
||||
driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
|
||||
driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
|
||||
driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
|
||||
driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
|
||||
driverXbox.rightStick().onTrue(climbCommand());
|
||||
|
||||
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
|
||||
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand()
|
||||
.andThen(m_IntakeSubsystem.deployintakeCommand()));
|
||||
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
|
||||
// () -> -driverXbox.getLeftX()));
|
||||
|
||||
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
|
||||
|
||||
operatorXbox.x().whileTrue(m_ShooterSubsystem.testLeftShooterCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopLeftShooterCommand());
|
||||
operatorXbox.y().whileTrue(m_ShooterSubsystem.testCenterShooterCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopCenterShooterCommand());
|
||||
operatorXbox.b().whileTrue(m_ShooterSubsystem.testRightShooterCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopRightShooterCommand());
|
||||
operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
|
||||
|
||||
if (RobotBase.isSimulation()) {
|
||||
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
|
||||
} else {
|
||||
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
|
||||
}
|
||||
|
||||
if (Robot.isSimulation()) {
|
||||
Pose2d target = new Pose2d(new Translation2d(1, 4),
|
||||
Rotation2d.fromDegrees(90));
|
||||
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
|
||||
driveDirectAngleKeyboard.driveToPose(() -> target,
|
||||
new ProfiledPIDController(5,
|
||||
0,
|
||||
0,
|
||||
new Constraints(5, 2)),
|
||||
new ProfiledPIDController(5,
|
||||
0,
|
||||
0,
|
||||
new Constraints(Units.degreesToRadians(360),
|
||||
Units.degreesToRadians(180))));
|
||||
driverXbox.start()
|
||||
.onTrue(Commands.runOnce(() -> drivebase
|
||||
.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
|
||||
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
|
||||
driverXbox.button(2)
|
||||
.whileTrue(Commands.runEnd(
|
||||
() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
|
||||
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
|
||||
|
||||
// driverXbox.b().whileTrue(
|
||||
// drivebase.driveToPose(
|
||||
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
|
||||
// );
|
||||
// Put the autoChooser on the SmartDashboard
|
||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||
|
||||
}
|
||||
if (DriverStation.isTest()) {
|
||||
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
|
||||
// above!
|
||||
|
||||
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
|
||||
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
|
||||
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
|
||||
driverXbox.leftBumper().onTrue(Commands.none());
|
||||
driverXbox.rightBumper().onTrue(Commands.none());
|
||||
} else {
|
||||
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
|
||||
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
|
||||
driverXbox.start().whileTrue(Commands.none());
|
||||
driverXbox.back().whileTrue(Commands.none());
|
||||
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
|
||||
driverXbox.rightBumper().onTrue(Commands.none());
|
||||
/**
|
||||
* Use this method to define your trigger->command mappings. Triggers can be
|
||||
* created via the
|
||||
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
|
||||
* an arbitrary predicate, or via the
|
||||
* named factories in
|
||||
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
|
||||
* for
|
||||
* {@link CommandXboxController
|
||||
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
|
||||
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
|
||||
* Flight joysticks}.
|
||||
*/
|
||||
private void configureBindings() {
|
||||
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
|
||||
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
|
||||
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
|
||||
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
|
||||
driveDirectAngle);
|
||||
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
|
||||
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
|
||||
.driveFieldOriented(driveAngularVelocityKeyboard);
|
||||
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
|
||||
driveDirectAngleKeyboard);
|
||||
|
||||
driverXbox.start().onTrue(m_IntakeSubsystem.stopIntakeMotorCommand());
|
||||
driverXbox.back().onTrue((Commands.runOnce(drivebase::zeroGyro)));
|
||||
|
||||
driverXbox.leftTrigger().onTrue(m_IntakeSubsystem
|
||||
.startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST));
|
||||
driverXbox.rightBumper()
|
||||
.whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand()
|
||||
.andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand())
|
||||
.andThen(m_ShooterSubsystem.reverseShooterCommand()))
|
||||
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()
|
||||
.andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand())
|
||||
.andThen(m_ShooterSubsystem.setShooterMotorsRPMCommand(
|
||||
Constants.ShooterConstants.IDLE_SHOOTER_RPM)));
|
||||
// command for
|
||||
// full shooting system including linear actuators
|
||||
//driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand().andThen(new WaitCommand(1.5)));
|
||||
//.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
|
||||
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
|
||||
driverXbox.leftBumper().onTrue(m_IntakeSubsystem
|
||||
.startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_SLOW));
|
||||
// driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE,
|
||||
// Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly());
|
||||
|
||||
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand());
|
||||
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand());
|
||||
// driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
|
||||
// driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
|
||||
// driverXbox.povLeft().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE));
|
||||
|
||||
driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand());
|
||||
driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand());
|
||||
// driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase,
|
||||
// driverXbox));
|
||||
|
||||
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
|
||||
driverXbox.x().onTrue(setIdleShooterRPMCommand().andThen(m_IntakeSubsystem.deployIntakeCommand()));
|
||||
// .andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)));
|
||||
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
|
||||
// () -> -driverXbox.getLeftX()));
|
||||
|
||||
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
|
||||
|
||||
|
||||
topButtons.button(12).whileTrue(m_ShooterSubsystem.testLeftShooterCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopLeftShooterCommand());
|
||||
topButtons.button(11).whileTrue(m_ShooterSubsystem.testCenterShooterCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopCenterShooterCommand());
|
||||
topButtons.button(10).whileTrue(m_ShooterSubsystem.testRightShooterCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopRightShooterCommand());
|
||||
|
||||
|
||||
topButtons.axisGreaterThan(1, 0.3)
|
||||
.toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(
|
||||
Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -5))
|
||||
.toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
|
||||
topButtons.axisGreaterThan(1, -0.3)
|
||||
.toggleOnTrue(m_IntakeSubsystem
|
||||
.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED))
|
||||
.toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
|
||||
topButtons.axisGreaterThan(0, 0.5).onTrue(m_ClimberSubsystem.setClimberSpeedCommand(0.4))
|
||||
.onFalse(m_ClimberSubsystem.setClimberSpeedCommand(0));
|
||||
topButtons.axisLessThan(0, -0.8).onTrue(m_ClimberSubsystem.setClimberSpeedCommand(-0.4))
|
||||
.onFalse(m_ClimberSubsystem.setClimberSpeedCommand(0));
|
||||
topButtons.button(3).onTrue(killAllCommand());
|
||||
topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
|
||||
topButtons.button(1)
|
||||
.onTrue(drivebase.driveToClimbPoseOffsetted(
|
||||
Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED,
|
||||
Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
|
||||
topButtons.button(2)
|
||||
.onTrue(drivebase.driveToClimbPoseOffsetted(
|
||||
Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED,
|
||||
Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
|
||||
topButtons.button(4).whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
|
||||
.onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
|
||||
|
||||
bottomButtons.button(9)
|
||||
.onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(1.5));
|
||||
bottomButtons.button(3).whileTrue(m_IntakeSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
|
||||
bottomButtons.button(7).whileTrue(m_IntakeSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
|
||||
bottomButtons.button(4).whileTrue(m_IntakeSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
|
||||
bottomButtons.button(8).whileTrue(m_IntakeSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
|
||||
|
||||
// topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))
|
||||
|
||||
if (RobotBase.isSimulation()) {
|
||||
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
|
||||
} else {
|
||||
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
|
||||
}
|
||||
|
||||
if (Robot.isSimulation()) {
|
||||
Pose2d target = new Pose2d(new Translation2d(1, 4),
|
||||
Rotation2d.fromDegrees(90));
|
||||
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
|
||||
driveDirectAngleKeyboard.driveToPose(() -> target,
|
||||
new ProfiledPIDController(5,
|
||||
0,
|
||||
0,
|
||||
new Constraints(5, 2)),
|
||||
new ProfiledPIDController(5,
|
||||
0,
|
||||
0,
|
||||
new Constraints(Units.degreesToRadians(360),
|
||||
Units.degreesToRadians(180))));
|
||||
driverXbox.start()
|
||||
.onTrue(Commands.runOnce(() -> drivebase
|
||||
.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
|
||||
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
|
||||
driverXbox.button(2)
|
||||
.whileTrue(Commands.runEnd(
|
||||
() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
|
||||
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
|
||||
|
||||
// driverXbox.b().whileTrue(
|
||||
// drivebase.driveToPose(
|
||||
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
|
||||
// );
|
||||
|
||||
}
|
||||
if (DriverStation.isTest()) {
|
||||
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
|
||||
// above!
|
||||
|
||||
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
|
||||
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
|
||||
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
|
||||
driverXbox.leftBumper().onTrue(Commands.none());
|
||||
driverXbox.rightBumper().onTrue(Commands.none());
|
||||
} else {
|
||||
// driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
|
||||
// driverXbox.start().whileTrue(Commands.none());
|
||||
// driverXbox.back().whileTrue(Commands.none());
|
||||
// driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock,
|
||||
// drivebase).repeatedly());
|
||||
// driverXbox.rightBumper().onTrue(Commands.none());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// Pass in the selected auto from the SmartDashboard as our desired autnomous
|
||||
// commmand
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// Pass in the selected auto from the SmartDashboard as our desired autnomous
|
||||
// commmand
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
public void setMotorBrake(boolean brake) {
|
||||
drivebase.setMotorBrake(brake);
|
||||
}
|
||||
|
||||
public void setMotorBrake(boolean brake) {
|
||||
drivebase.setMotorBrake(brake);
|
||||
}
|
||||
public SwerveSubsystem getSwerveDrive() {
|
||||
return drivebase;
|
||||
}
|
||||
|
||||
public SwerveSubsystem getSwerveDrive() {
|
||||
return drivebase;
|
||||
}
|
||||
public void killAll() {
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
public Command climbCommand() {
|
||||
if (driverXbox.getRightY() > -0.5) {
|
||||
return m_ClimberSubsystem.lowerRobotCommand();
|
||||
} else if (driverXbox.getRightX() < 0.5) {
|
||||
return m_ClimberSubsystem.liftRobotCommand();
|
||||
} else
|
||||
return m_ClimberSubsystem.stopClimberCommand();
|
||||
public Command killAllCommand() {
|
||||
return Commands.runOnce(() -> killAll());
|
||||
}
|
||||
|
||||
}
|
||||
public Command setIdleShooterRPMCommand() {
|
||||
return Commands.runOnce(() -> ShooterSubsystem
|
||||
.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM))
|
||||
.andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
|
||||
}
|
||||
|
||||
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
|
||||
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
|
||||
m_ShooterSubsystem.shootFuelCommand(),
|
||||
m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,12 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.SparkFlex;
|
||||
import com.revrobotics.spark.SparkBase.ControlType;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import com.revrobotics.spark.config.ClosedLoopConfig;
|
||||
import com.revrobotics.spark.config.SparkFlexConfig;
|
||||
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
@@ -9,47 +15,60 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class ClimberSubsystem extends SubsystemBase {
|
||||
private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
|
||||
private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT);
|
||||
// private static TalonFX climberMotor = new
|
||||
// TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
|
||||
private static SparkFlex climberMotor = new SparkFlex(Constants.ClimberConstants.CLIMB_MOTOR_ID,
|
||||
MotorType.kBrushless);
|
||||
private static SparkClosedLoopController climberMotorPIDController;
|
||||
public static SparkFlexConfig climberMotorConfig = new SparkFlexConfig();
|
||||
|
||||
public void liftRobot() {
|
||||
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE);
|
||||
}
|
||||
public ClimberSubsystem() {
|
||||
climberMotorConfig.closedLoop.pid(Constants.ClimberConstants.CLIMBER_PID_P,
|
||||
Constants.ClimberConstants.CLIMBER_PID_I,
|
||||
Constants.ClimberConstants.CLIMBER_PID_D);
|
||||
climberMotorConfig.smartCurrentLimit(80);
|
||||
climberMotorConfig.openLoopRampRate(0);
|
||||
climberMotorConfig.closedLoopRampRate(0);
|
||||
climberMotor.configure(climberMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
climberMotorPIDController = climberMotor.getClosedLoopController();
|
||||
}
|
||||
|
||||
public void lowerRobot() {
|
||||
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE * -1);
|
||||
}
|
||||
public void setClimberSpeed(double speed) {
|
||||
climberMotor.set(speed);
|
||||
}
|
||||
|
||||
public void stopClimber() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
public Command setClimberSpeedCommand(double speed) {
|
||||
return runOnce(() -> setClimberSpeed(speed));
|
||||
}
|
||||
public void liftRobot() {
|
||||
climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LIFTED_SETPOINT_VALUE, ControlType.kPosition);
|
||||
}
|
||||
|
||||
public Command liftRobotCommand() {
|
||||
return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot());
|
||||
}
|
||||
public static void lowerRobot() {
|
||||
climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LOWERED_SETPOINT_VALUE, ControlType.kPosition);
|
||||
}
|
||||
|
||||
public Command lowerRobotCommand() {
|
||||
return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot());
|
||||
}
|
||||
|
||||
public Command stopClimberCommand() {
|
||||
return runOnce(() -> stopClimber());
|
||||
}
|
||||
public void stopClimber() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
|
||||
public static void toggleRatchet(boolean toggle) {
|
||||
if (toggle == true) {
|
||||
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE_DEGREES);
|
||||
} else
|
||||
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE_DEGREES);
|
||||
}
|
||||
public Command liftRobotCommand() {
|
||||
return runOnce(() -> liftRobot());
|
||||
}
|
||||
|
||||
public Command toggleRatchetCommand(boolean toggle) {
|
||||
return runOnce(() -> toggleRatchet(toggle));
|
||||
public Command lowerRobotCommand() {
|
||||
return runOnce(() -> lowerRobot());
|
||||
}
|
||||
|
||||
public Command stopClimberCommand() {
|
||||
return runOnce(() -> stopClimber());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic()
|
||||
{
|
||||
SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getAngle());
|
||||
public void periodic() {
|
||||
SmartDashboard.putNumber("Climber Motor Encoder", climberMotor.getEncoder().getPosition());
|
||||
SmartDashboard.putNumber("Climber motor power", climberMotor.get());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,69 +1,142 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.controller.ArmFeedforward;
|
||||
import edu.wpi.first.math.controller.BangBangController;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.units.Units;
|
||||
import edu.wpi.first.units.measure.Voltage;
|
||||
import edu.wpi.first.wpilibj.DutyCycleEncoder;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import frc.robot.Constants;
|
||||
|
||||
import com.revrobotics.spark.ClosedLoopSlot;
|
||||
import com.revrobotics.spark.FeedbackSensor;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.SparkFlex;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import com.revrobotics.spark.config.SparkFlexConfig;
|
||||
|
||||
import static edu.wpi.first.units.Units.Radians;
|
||||
import static edu.wpi.first.units.Units.RadiansPerSecond;
|
||||
import static edu.wpi.first.units.Units.Volts;
|
||||
import static edu.wpi.first.units.Units.VoltsPerRadianPerSecond;
|
||||
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.SparkBase.ControlType;
|
||||
|
||||
public class IntakeSubsystem extends SubsystemBase {
|
||||
|
||||
private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, MotorType.kBrushless);
|
||||
private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
|
||||
MotorType.kBrushless);
|
||||
|
||||
private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, MotorType.kBrushless);
|
||||
private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
|
||||
MotorType.kBrushless);
|
||||
|
||||
public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
|
||||
private static BangBangController intakeWheelsMotorBBController = new BangBangController();
|
||||
private static SimpleMotorFeedforward intakeWheelsMotorFeedforward;
|
||||
private final TrapezoidProfile.Constraints m_Constraints = new TrapezoidProfile.Constraints(0.3, .5);
|
||||
private final ProfiledPIDController intakeRotatorProfiledPIDController;
|
||||
private static TrapezoidProfile.State goalState = new TrapezoidProfile.State(
|
||||
Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT, 0);
|
||||
|
||||
SysIdRoutine routine = new SysIdRoutine(new SysIdRoutine.Config(),
|
||||
new SysIdRoutine.Mechanism(intakeRotatorMotor::setVoltage,
|
||||
log -> log.motor("arm").voltage(Volts.of(intakeRotatorMotor.getAppliedOutput() * 12))
|
||||
.angularPosition(Radians.of(intakeRotatorEncoder.get() * 2 * Math.PI))
|
||||
.angularVelocity(RadiansPerSecond.of(0)),
|
||||
this, "armSysId"));
|
||||
|
||||
private static SparkClosedLoopController intakeRotatorPIDController;
|
||||
public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
|
||||
private static PIDController intakeRotatorMotorPIDController;
|
||||
private static ArmFeedforward intakeRotatorMotorFeedforward;
|
||||
|
||||
private static SparkClosedLoopController intakeWheelsMotorPIDController;
|
||||
public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
|
||||
|
||||
public static DutyCycleEncoder intakeRotatorEncoder = new DutyCycleEncoder(1);
|
||||
|
||||
private static double encoderValue = intakeRotatorEncoder.get();
|
||||
|
||||
public IntakeSubsystem() {
|
||||
intakeWheelsMotorConfig
|
||||
.smartCurrentLimit(Constants.IntakeConstants.INTAKE_WHEELS_CURRENT_LIMIT)
|
||||
.encoder
|
||||
.positionConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_POSITION_CONVERSION_FACTOR)
|
||||
.velocityConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR);
|
||||
intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
intakeRotatorProfiledPIDController = new ProfiledPIDController(
|
||||
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
|
||||
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
|
||||
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D,
|
||||
m_Constraints,
|
||||
0.02);
|
||||
intakeRotatorProfiledPIDController.setTolerance(0.02);
|
||||
intakeRotatorProfiledPIDController.setGoal(goalState);
|
||||
|
||||
intakeWheelsMotorFeedforward = new SimpleMotorFeedforward(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_S, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
|
||||
|
||||
intakeRotatorConfig
|
||||
.smartCurrentLimit(Constants.IntakeConstants.INTAKE_ROTATOR_CURRENT_LIMIT)
|
||||
.encoder
|
||||
.positionConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR)
|
||||
.velocityConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR);
|
||||
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_ROTATOR_INITIAL_ENCODER_VALUE);
|
||||
intakeRotatorConfig.closedLoop
|
||||
// Slot 0
|
||||
.p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
|
||||
.i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
|
||||
.d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
|
||||
|
||||
intakeRotatorMotorPIDController = new PIDController(Constants.IntakeConstants.INTAKE_ROTATOR_UP_P, Constants.IntakeConstants.INTAKE_ROTATOR_UP_I, Constants.IntakeConstants.INTAKE_ROTATOR_UP_D);
|
||||
intakeRotatorMotorFeedforward = new ArmFeedforward(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_S, Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_G, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
|
||||
// Slot 1
|
||||
.p(.05, ClosedLoopSlot.kSlot1)
|
||||
.i(0, ClosedLoopSlot.kSlot1)
|
||||
.d(.05, ClosedLoopSlot.kSlot1)
|
||||
|
||||
// Slot 2
|
||||
.p(.8, ClosedLoopSlot.kSlot2)
|
||||
.i(.0, ClosedLoopSlot.kSlot2)
|
||||
.d(0.8, ClosedLoopSlot.kSlot2);
|
||||
|
||||
intakeRotatorConfig.smartCurrentLimit(80);
|
||||
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
|
||||
intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
|
||||
|
||||
intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
|
||||
Constants.IntakeConstants.INTAKE_MOTOR_I,
|
||||
Constants.IntakeConstants.INTAKE_MOTOR_D);
|
||||
intakeWheelsMotorConfig.smartCurrentLimit(70);
|
||||
intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
|
||||
intakeRotatorMotor.getEncoder().setPosition(-6);
|
||||
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
|
||||
}
|
||||
|
||||
public void startIntakeMotor() {
|
||||
intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM);
|
||||
public void goToPosition(double goalPosition) {
|
||||
goalState = new TrapezoidProfile.State(goalPosition, 0);
|
||||
}
|
||||
|
||||
public Command goToPositionCommand(double goalPosition) {
|
||||
return runOnce(() -> goToPosition(goalPosition));
|
||||
}
|
||||
|
||||
public void rotateIntake(double speed) {
|
||||
intakeRotatorMotor.set(speed);
|
||||
}
|
||||
|
||||
public Command rotateIntakeCommand(double speed) {
|
||||
return runOnce(() -> intakeRotatorMotor.set(speed));
|
||||
}
|
||||
|
||||
public void startIntakeMotor(double speed) {
|
||||
intakeWheelsMotorPIDController.setSetpoint(speed,
|
||||
ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public void reverseIntakeMotor() {
|
||||
intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1);
|
||||
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST * -1,
|
||||
ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public void stopIntakeMotor() {
|
||||
intakeWheelsMotorBBController.setSetpoint(0.0);
|
||||
intakeWheelsMotor.set(0);
|
||||
}
|
||||
|
||||
public Command startIntakeMotorCommand() {
|
||||
return runOnce(() -> startIntakeMotor());
|
||||
public Command startIntakeMotorCommand(double speed) {
|
||||
return runOnce(() -> startIntakeMotor(speed));
|
||||
}
|
||||
|
||||
public Command reverseIntakeMotorCommand() {
|
||||
@@ -74,16 +147,37 @@ public class IntakeSubsystem extends SubsystemBase {
|
||||
return runOnce(() -> stopIntakeMotor());
|
||||
}
|
||||
|
||||
public void deployIntake() {
|
||||
intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_POSITION_RADS);
|
||||
/*
|
||||
* public Command assistFuelIntakeCommand(double deployedPosition, double
|
||||
* assistPosition) {
|
||||
* return runOnce(() -> goToPositionCommand(assistPosition).andThen(new
|
||||
* WaitCommand(1.5))
|
||||
* .andThen(goToPositionCommand(deployedPosition)).andThen(new
|
||||
* WaitCommand(1.5)));
|
||||
* }
|
||||
*/
|
||||
|
||||
public static void resetIntakeRotationEncoder() {
|
||||
intakeRotatorMotor.getEncoder().setPosition(-5);
|
||||
}
|
||||
|
||||
public Command deployintakeCommand() {
|
||||
public void deployIntake() {
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
|
||||
ControlType.kPosition, ClosedLoopSlot.kSlot0);
|
||||
}
|
||||
|
||||
public void deployIntakeAssist() {
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
|
||||
ControlType.kPosition, ClosedLoopSlot.kSlot2);
|
||||
}
|
||||
|
||||
public Command deployIntakeCommand() {
|
||||
return runOnce(() -> deployIntake());
|
||||
}
|
||||
|
||||
public void retractIntake() {
|
||||
intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_POSITION_RADS);
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE,
|
||||
ControlType.kPosition, ClosedLoopSlot.kSlot1);
|
||||
}
|
||||
|
||||
public Command retractIntakeCommand() {
|
||||
@@ -91,23 +185,30 @@ public class IntakeSubsystem extends SubsystemBase {
|
||||
}
|
||||
|
||||
public void assistFuelIntake() {
|
||||
intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_POSITION_RADS);
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
|
||||
ControlType.kPosition, ClosedLoopSlot.kSlot2);
|
||||
}
|
||||
|
||||
public Command assistFuelIntakeCommand() {
|
||||
return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
|
||||
.andThen(new WaitCommand(2));
|
||||
return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(.5)).andThen(deployIntakeCommand())
|
||||
.andThen(new WaitCommand(.5));
|
||||
}
|
||||
|
||||
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
|
||||
return routine.quasistatic(direction);
|
||||
}
|
||||
|
||||
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
|
||||
return routine.dynamic(direction);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
intakeWheelsMotor.setVoltage(
|
||||
12.0 * intakeWheelsMotorBBController.calculate(intakeWheelsMotor.getEncoder().getVelocity())
|
||||
+ 0.9 * intakeWheelsMotorFeedforward.calculate(intakeWheelsMotorBBController.getSetpoint()));
|
||||
intakeRotatorMotor.setVoltage(
|
||||
12.0 * intakeRotatorMotorPIDController.calculate(intakeRotatorMotor.getEncoder().getPosition())
|
||||
+ intakeRotatorMotorFeedforward.calculate(intakeRotatorMotorPIDController.getSetpoint(), 0.0));
|
||||
|
||||
SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
|
||||
encoderValue = intakeRotatorEncoder.get();
|
||||
//intakeRotatorMotor.setVoltage(intakeRotatorProfiledPIDController.calculate(encoderValue, goalState));
|
||||
SmartDashboard.putNumber("Intake Rotator Motor Encoder", intakeRotatorMotor.getEncoder().getPosition());
|
||||
SmartDashboard.putNumber("Intake Rotator Throughbore Encoder Value", intakeRotatorEncoder.get());
|
||||
SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue,
|
||||
goalState));
|
||||
}
|
||||
}
|
||||
|
||||
195
src/main/java/frc/robot/subsystems/LEDSubsystem.java
Normal file
195
src/main/java/frc/robot/subsystems/LEDSubsystem.java
Normal file
@@ -0,0 +1,195 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import static edu.wpi.first.units.Units.Percent;
|
||||
import static edu.wpi.first.units.Units.Second;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import edu.wpi.first.wpilibj.AddressableLED;
|
||||
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
|
||||
import edu.wpi.first.wpilibj.AddressableLEDBufferView;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.LEDPattern;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.util.Color;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class LEDSubsystem extends SubsystemBase {
|
||||
|
||||
AddressableLED m_LED;
|
||||
|
||||
AddressableLEDBuffer m_Buffer;
|
||||
AddressableLEDBufferView m_Left;
|
||||
AddressableLEDBufferView m_Right;
|
||||
|
||||
LEDPattern hubActiveColor = LEDPattern.solid(Color.kGreen);
|
||||
LEDPattern hubActiveBlinkPattern = hubActiveColor.blink(Second.of(0.5));
|
||||
|
||||
LEDPattern hubInactiveColor = LEDPattern.solid(Color.kRed);
|
||||
LEDPattern hubInactiveBlinkPattern = hubInactiveColor.blink(Second.of(0.1));
|
||||
|
||||
LEDPattern allianceShiftColor = LEDPattern.gradient(LEDPattern.GradientType.kContinuous, Color.kTeal, Color.kMagenta);
|
||||
LEDPattern allianceShiftPattern = allianceShiftColor.scrollAtRelativeSpeed(Percent.per(Second).of(100));
|
||||
|
||||
LEDPattern transitionColor = LEDPattern.solid(Color.kYellow);
|
||||
LEDPattern transitionBlinkPattern = transitionColor.blink(Second.of(0.2));
|
||||
|
||||
LEDPattern endGameColor = LEDPattern.solid(Color.kOrangeRed);
|
||||
LEDPattern endGameBlinkPattern = endGameColor.blink(Second.of(0.2));
|
||||
|
||||
LEDPattern rainbow = LEDPattern.rainbow(255, 128);
|
||||
LEDPattern rainbowScroll = rainbow.scrollAtRelativeSpeed(Percent.per(Second).of(100));
|
||||
|
||||
public LEDSubsystem() {
|
||||
m_LED = new AddressableLED(Constants.LEDConstants.LED_PWM_PORT);
|
||||
m_Buffer = new AddressableLEDBuffer(44);
|
||||
m_LED.setLength(m_Buffer.getLength());
|
||||
|
||||
m_Left = m_Buffer.createView(0, 21);
|
||||
m_Right = m_Buffer.createView(22, 43);
|
||||
|
||||
hubInactiveBlinkPattern.applyTo(m_Left);
|
||||
hubActiveBlinkPattern.applyTo(m_Right);
|
||||
|
||||
m_LED.setData(m_Buffer);
|
||||
m_LED.start();
|
||||
}
|
||||
|
||||
private double matchTime;
|
||||
private boolean isHubActive;
|
||||
|
||||
public void setLEDPeriod() {
|
||||
if(DriverStation.isAutonomous())
|
||||
{
|
||||
allianceShiftPattern.applyTo(m_Buffer);
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
if (matchTime <= 140 && matchTime > 132) // transition
|
||||
{
|
||||
transitionBlinkPattern.applyTo(m_Left);
|
||||
}
|
||||
|
||||
if(matchTime <= 132 && matchTime > 130)
|
||||
{
|
||||
hubInactiveBlinkPattern.applyTo(m_Buffer);
|
||||
}
|
||||
|
||||
if(matchTime <= 107 && matchTime > 105 && isHubActive == false)
|
||||
{
|
||||
hubInactiveBlinkPattern.applyTo(m_Buffer);
|
||||
}
|
||||
|
||||
if(matchTime <= 82 && matchTime > 80 && isHubActive == false)
|
||||
{
|
||||
hubInactiveBlinkPattern.applyTo(m_Buffer);
|
||||
}
|
||||
|
||||
if(matchTime <= 57 && matchTime > 55 && isHubActive == false)
|
||||
{
|
||||
hubInactiveBlinkPattern.applyTo(m_Buffer);
|
||||
}
|
||||
|
||||
if(matchTime <= 32 && matchTime > 30 && isHubActive == false)
|
||||
{
|
||||
hubInactiveBlinkPattern.applyTo(m_Buffer);
|
||||
}
|
||||
|
||||
if (matchTime <= 30) // endgame
|
||||
{
|
||||
endGameBlinkPattern.applyTo(m_Left);
|
||||
}
|
||||
|
||||
|
||||
allianceShiftPattern.applyTo(m_Buffer);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void setLEDHubActive() {
|
||||
if (isHubActive) {
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public boolean isHubActive() {
|
||||
Optional<Alliance> alliance = DriverStation.getAlliance();
|
||||
// If we have no alliance, we cannot be enabled, therefore no hub.
|
||||
if (alliance.isEmpty()) {
|
||||
return false;
|
||||
}
|
||||
// Hub is always enabled in autonomous.
|
||||
if (DriverStation.isAutonomousEnabled()) {
|
||||
return true;
|
||||
}
|
||||
// At this point, if we're not teleop enabled, there is no hub.
|
||||
if (!DriverStation.isTeleopEnabled()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// We're teleop enabled, compute.
|
||||
double matchTime = DriverStation.getMatchTime();
|
||||
String gameData = DriverStation.getGameSpecificMessage();
|
||||
// If we have no game data, we cannot compute, assume hub is active, as its
|
||||
// likely early in teleop.
|
||||
if (gameData.isEmpty()) {
|
||||
return true;
|
||||
}
|
||||
boolean redInactiveFirst = false;
|
||||
switch (gameData.charAt(0)) {
|
||||
case 'R' -> redInactiveFirst = true;
|
||||
case 'B' -> redInactiveFirst = false;
|
||||
default -> {
|
||||
// If we have invalid game data, assume hub is active.
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// Shift was is active for blue if red won auto, or red if blue won auto.
|
||||
boolean shift1Active = switch (alliance.get()) {
|
||||
case Red -> !redInactiveFirst;
|
||||
case Blue -> redInactiveFirst;
|
||||
};
|
||||
|
||||
if (matchTime > 130) {
|
||||
// Transition shift, hub is active.
|
||||
return true;
|
||||
} else if (matchTime > 105) {
|
||||
// Shift 1
|
||||
return shift1Active;
|
||||
} else if (matchTime > 80) {
|
||||
// Shift 2
|
||||
return !shift1Active;
|
||||
} else if (matchTime > 55) {
|
||||
// Shift 3
|
||||
return shift1Active;
|
||||
} else if (matchTime > 30) {
|
||||
// Shift 4
|
||||
return !shift1Active;
|
||||
} else {
|
||||
// End game, hub always active.
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
matchTime = DriverStation.getMatchTime();
|
||||
isHubActive = isHubActive();
|
||||
setLEDPeriod();
|
||||
|
||||
m_LED.setData(m_Buffer);
|
||||
}
|
||||
}
|
||||
@@ -1,17 +1,31 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
import edu.wpi.first.wpilibj.PneumaticsModuleType;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.swerve.utility.WheelForceCalculator.Feedforwards;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.SparkBase.ControlType;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.FeedbackSensor;
|
||||
import com.revrobotics.spark.SparkBase;
|
||||
import com.revrobotics.spark.SparkFlex;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import com.revrobotics.spark.config.SparkFlexConfig;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.math.controller.BangBangController;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||
|
||||
public class ShooterSubsystem extends SubsystemBase {
|
||||
private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
|
||||
@@ -26,115 +40,162 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
|
||||
MotorType.kBrushless);
|
||||
|
||||
private static SparkClosedLoopController centerShooterMotorPIDController;
|
||||
public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
|
||||
private static BangBangController centerShooterMotorBBController = new BangBangController();
|
||||
private static SimpleMotorFeedforward centerShooterMotorFeedforward;
|
||||
|
||||
private static SparkClosedLoopController leftShooterMotorPIDController;
|
||||
public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
|
||||
private static BangBangController leftShooterMotorBBController = new BangBangController();
|
||||
private static SimpleMotorFeedforward leftShooterMotorFeedforward;
|
||||
|
||||
private static SparkClosedLoopController rightShooterMotorPIDController;
|
||||
public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
|
||||
private static BangBangController rightShooterMotorBBController = new BangBangController();
|
||||
private static SimpleMotorFeedforward rightShooterMotorFeedforward;
|
||||
|
||||
private static SparkClosedLoopController indexerAndRampMotorPIDController;
|
||||
public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
|
||||
private static PIDController indexerAndRampMotorPIDController;
|
||||
|
||||
public ShooterSubsystem() {
|
||||
centerShooterMotorConfig.smartCurrentLimit(60);
|
||||
centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
centerShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.CENTER_MOTOR_S, Constants.ShooterConstants.CENTER_MOTOR_V);
|
||||
|
||||
leftShooterMotorConfig.smartCurrentLimit(60);
|
||||
leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
leftShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.LEFT_MOTOR_S, Constants.ShooterConstants.LEFT_MOTOR_V);
|
||||
|
||||
rightShooterMotorConfig.smartCurrentLimit(60);
|
||||
rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
rightShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.RIGHT_MOTOR_S, Constants.ShooterConstants.RIGHT_MOTOR_V);
|
||||
centerShooterMotorConfig
|
||||
.voltageCompensation(12.0)
|
||||
.closedLoop
|
||||
.outputRange(-1, 0)
|
||||
.p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
|
||||
.i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
|
||||
.d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.feedForward
|
||||
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
|
||||
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
|
||||
centerShooterMotorConfig.smartCurrentLimit(45);
|
||||
|
||||
indexerAndRampMotorConfig.smartCurrentLimit(60);
|
||||
centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
|
||||
|
||||
leftShooterMotorConfig
|
||||
.voltageCompensation(12.0)
|
||||
.closedLoop
|
||||
.outputRange(-1, 0)
|
||||
.p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
|
||||
.i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
|
||||
.d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.feedForward
|
||||
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
|
||||
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
|
||||
leftShooterMotorConfig.smartCurrentLimit(45);
|
||||
leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController();
|
||||
|
||||
rightShooterMotorConfig
|
||||
.voltageCompensation(12.0)
|
||||
.closedLoop
|
||||
.outputRange(-1, 0)
|
||||
.p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
|
||||
.i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
|
||||
.d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.feedForward
|
||||
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
|
||||
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
|
||||
rightShooterMotorConfig.smartCurrentLimit(45);
|
||||
rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
|
||||
|
||||
indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.INDEXER_MOTOR_P,
|
||||
0,
|
||||
0);
|
||||
indexerAndRampMotorConfig.smartCurrentLimit(50);
|
||||
indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
indexerAndRampMotorPIDController = new PIDController(Constants.ShooterConstants.INDEXER_MOTOR_P, Constants.ShooterConstants.INDEXER_MOTOR_I, Constants.ShooterConstants.INDEXER_MOTOR_D);
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
|
||||
}
|
||||
|
||||
|
||||
//private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
|
||||
// MotorType.kBrushless);
|
||||
|
||||
//private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
|
||||
//MotorType.kBrushless);
|
||||
|
||||
//private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
|
||||
//private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
|
||||
|
||||
private static double currentPotentiometerPosition; // might need second value for the right potentiometer
|
||||
|
||||
public void setShooterMotorsRPM() {
|
||||
centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
|
||||
leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
|
||||
rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
|
||||
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
|
||||
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
|
||||
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
|
||||
}
|
||||
|
||||
//test individual motor code
|
||||
public void setLeftShooterMotorRPM() {
|
||||
leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
|
||||
public static void setShooterMotorsRPM(double RPM){
|
||||
centerShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
|
||||
leftShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
|
||||
rightShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command setShooterMotorsRPMCommand(double RPM) {
|
||||
return runOnce(() -> setShooterMotorsRPM(RPM));
|
||||
}
|
||||
|
||||
public Command setShooterMotorsRPMAutoCommand()
|
||||
{
|
||||
return runOnce(()-> setShooterMotorsRPM(Constants.ShooterConstants.AUTO_SHOOTER_RPM)).andThen(new WaitCommand(1))
|
||||
.andThen(() -> setIndexerAndRampMotorRPM());
|
||||
}
|
||||
|
||||
// test individual motor code
|
||||
public void setLeftShooterMotorRPM() {
|
||||
leftShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command testLeftShooterCommand() {
|
||||
return runOnce(() -> setLeftShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void setRightShooterMotorRPM() {
|
||||
rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
|
||||
rightShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command testRightShooterCommand() {
|
||||
return runOnce(() -> setRightShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void setCenterShooterMotorRPM() {
|
||||
centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
|
||||
centerShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command testCenterShooterCommand() {
|
||||
return runOnce(() -> setCenterShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void stopLeftShooterMotorRPM() {
|
||||
leftShooterMotorBBController.setSetpoint(0.0);
|
||||
leftShooterMotor.set(0);
|
||||
}
|
||||
|
||||
public Command stopLeftShooterCommand() {
|
||||
return runOnce(() -> stopLeftShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void stopCenterShooterMotorRPM() {
|
||||
centerShooterMotorBBController.setSetpoint(0.0);
|
||||
centerShooterMotor.set(0);
|
||||
}
|
||||
|
||||
public Command stopCenterShooterCommand() {
|
||||
return runOnce(() -> stopCenterShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void stopRightShooterMotorRPM() {
|
||||
rightShooterMotorBBController.setSetpoint(0.0);
|
||||
rightShooterMotor.set(0);
|
||||
}
|
||||
|
||||
public Command stopRightShooterCommand() {
|
||||
return runOnce(() -> stopRightShooterMotorRPM());
|
||||
}
|
||||
|
||||
|
||||
public double getShooterMotorRPM() {
|
||||
return rightShooterMotor.getEncoder().getVelocity();
|
||||
}
|
||||
|
||||
public void setIndexerAndRampMotorRPM() {
|
||||
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM);
|
||||
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM,
|
||||
ControlType.kVelocity);
|
||||
}
|
||||
|
||||
|
||||
public void reverseIndexerAndRampMotorRPM() {
|
||||
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1);
|
||||
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1,
|
||||
ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command reverseIndexerAndRampMotorRPMCommand() {
|
||||
@@ -146,72 +207,63 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command stopIndexerAndRampMotorCommand() {
|
||||
return runOnce(()-> indexerAndRampMotorPIDController.setSetpoint(0.0));
|
||||
return runOnce(() -> indexerAndRampMotor.set(0));
|
||||
}
|
||||
|
||||
/*public Command shootFuelCommand() {
|
||||
return runOnce(() -> setShooterMotorsRPM())
|
||||
.until(() -> {return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM * 0.9);})
|
||||
.andThen(() -> setIndexerAndRampMotorRPM());
|
||||
} */
|
||||
.until(() -> {
|
||||
return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM);
|
||||
})
|
||||
.andThen(() -> setIndexerAndRampMotorRPM());
|
||||
}*/
|
||||
|
||||
|
||||
public Command shootFuelCommand() {
|
||||
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2))
|
||||
public Command shootFuelCommand() {
|
||||
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1))
|
||||
.andThen(() -> setIndexerAndRampMotorRPM());
|
||||
};
|
||||
};
|
||||
|
||||
/* public Command shootFuelCommand() {
|
||||
return runOnce(() -> setShooterMotorsRPM())
|
||||
.andThen(new WaitUntilCommand(() -> {
|
||||
return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM);
|
||||
}))
|
||||
.andThen(() -> setIndexerAndRampMotorRPM());
|
||||
};*/
|
||||
|
||||
|
||||
public void stopShooters() {
|
||||
centerShooterMotorBBController.setSetpoint(0.0);
|
||||
leftShooterMotorBBController.setSetpoint(0.0);
|
||||
rightShooterMotorBBController.setSetpoint(0.0);
|
||||
indexerAndRampMotorPIDController.setSetpoint(0.0);
|
||||
|
||||
centerShooterMotor.set(0);
|
||||
leftShooterMotor.set(0);
|
||||
rightShooterMotor.set(0);
|
||||
indexerAndRampMotor.set(0);
|
||||
|
||||
}
|
||||
|
||||
public Command stopShooterCommand() {
|
||||
return runOnce(() -> stopShooters());
|
||||
}
|
||||
|
||||
public void moveActuator(double desiredPotentiometerPosition) {
|
||||
if (desiredPotentiometerPosition > currentPotentiometerPosition) {
|
||||
//TODO: Test for positive or negative power
|
||||
//leftActuatorMotor.set(0.1);
|
||||
//rightActuatorMotor.set(0.1);
|
||||
} else {
|
||||
//leftActuatorMotor.set(-0.1);
|
||||
//rightActuatorMotor.set(-0.1);
|
||||
}
|
||||
public void reverseShooter()
|
||||
{
|
||||
centerShooterMotor.set(.4);
|
||||
leftShooterMotor.set(0.4);
|
||||
rightShooterMotor.set(0.4);
|
||||
}
|
||||
|
||||
public void stopActuator() {
|
||||
//leftActuatorMotor.set(0);
|
||||
//rightActuatorMotor.set(0);
|
||||
}
|
||||
|
||||
public Command moveActuatorCommand(double desiredPotentiometerPosition) {
|
||||
return run(() -> moveActuator(desiredPotentiometerPosition))
|
||||
.until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
|
||||
.andThen(() -> stopActuator());
|
||||
public Command reverseShooterCommand()
|
||||
{
|
||||
return runOnce(()-> reverseShooter());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
centerShooterMotor.setVoltage(
|
||||
12.0 * centerShooterMotorBBController.calculate(centerShooterMotor.getEncoder().getVelocity())
|
||||
+ 0.9 * centerShooterMotorFeedforward.calculate(centerShooterMotorBBController.getSetpoint()));
|
||||
leftShooterMotor.setVoltage(
|
||||
12.0 * leftShooterMotorBBController.calculate(leftShooterMotor.getEncoder().getVelocity())
|
||||
+ 0.9 * leftShooterMotorFeedforward.calculate(leftShooterMotorBBController.getSetpoint()));
|
||||
rightShooterMotor.setVoltage(
|
||||
12.0 * rightShooterMotorBBController.calculate(rightShooterMotor.getEncoder().getVelocity())
|
||||
+ 0.9 * rightShooterMotorFeedforward.calculate(rightShooterMotorBBController.getSetpoint()));
|
||||
indexerAndRampMotor.setVoltage(
|
||||
12.0 * indexerAndRampMotorPIDController.calculate(indexerAndRampMotor.getEncoder().getVelocity()));
|
||||
|
||||
SmartDashboard.putNumber("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity());
|
||||
/* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
|
||||
SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
|
||||
currentPotentiometerPosition = leftPotentiometer.get(); */
|
||||
SmartDashboard.putString("Shooter Velocity", "Left: "
|
||||
+ leftShooterMotor.getEncoder().getVelocity()
|
||||
+ " Center:" + centerShooterMotor.getEncoder().getVelocity()
|
||||
+ " Right: " + rightShooterMotor.getEncoder().getVelocity());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
180
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
Normal file
180
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
Normal file
@@ -0,0 +1,180 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.lang.StackWalker.Option;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.dyn4j.geometry.Rotation;
|
||||
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.PhotonTrackedTarget;
|
||||
|
||||
import com.pathplanner.lib.path.GoalEndState;
|
||||
import com.pathplanner.lib.path.PathConstraints;
|
||||
import com.pathplanner.lib.path.PathPlannerPath;
|
||||
import com.pathplanner.lib.path.PathPoint;
|
||||
import com.pathplanner.lib.path.RotationTarget;
|
||||
import com.pathplanner.lib.path.Waypoint;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import frc.robot.RobotContainer;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class TargetingSubsystems extends SubsystemBase {
|
||||
|
||||
PIDController photonAimPIDController = new PIDController(5, 0.01, 0);
|
||||
|
||||
public static Rotation2d hubThetaPose = new Rotation2d();
|
||||
public static Optional<Alliance> alliance = DriverStation.getAlliance();
|
||||
|
||||
public TargetingSubsystems() {
|
||||
photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
}
|
||||
|
||||
Pose2d currentRobotPose;
|
||||
|
||||
public List<Waypoint> pathWaypoints;
|
||||
|
||||
public Command pathPlanToPoseCommand(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
|
||||
GoalEndState goalEndState = new GoalEndState(0, desiredPose.getRotation());
|
||||
PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
|
||||
currentRobotPose = swerveDrive.getPose();
|
||||
pathWaypoints = PathPlannerPath.waypointsFromPoses(
|
||||
currentRobotPose, desiredPose);
|
||||
|
||||
PathPlannerPath goToDesiredPose = new PathPlannerPath(pathWaypoints, pathConstraints, null,
|
||||
goalEndState);
|
||||
goToDesiredPose.preventFlipping = true;
|
||||
|
||||
return swerveDrive.getAutonomousCommand("goToDesiredPose");
|
||||
}
|
||||
|
||||
public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
|
||||
return new RunCommand(() -> {
|
||||
currentRobotPose = swerveDrive.getPose();
|
||||
|
||||
Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
|
||||
|
||||
double xError = errorFromDesiredPose.getX();
|
||||
double yError = errorFromDesiredPose.getY();
|
||||
double angleError = errorFromDesiredPose.getRotation().getRadians();
|
||||
|
||||
PIDController xController = new PIDController(1.5, 0, 0);
|
||||
PIDController yController = new PIDController(1.5, 0, 0);
|
||||
|
||||
double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
|
||||
double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
|
||||
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
|
||||
desiredPose.getRotation().getRadians());
|
||||
|
||||
swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, false);
|
||||
}, swerveDrive);
|
||||
}
|
||||
|
||||
public Command aimAtHubPose(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
|
||||
return new RunCommand(() -> {
|
||||
currentRobotPose = swerveDrive.getPose();
|
||||
|
||||
// Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
|
||||
|
||||
Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose,
|
||||
Constants.TargetingConstants.allianceHubPose);
|
||||
|
||||
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), Constants.TargetingConstants.allianceHubPose.getRotation().getRadians());
|
||||
|
||||
angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0);
|
||||
|
||||
swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, -driverXbox.getLeftY() * -1), angleSpeed,
|
||||
true);
|
||||
}, swerveDrive);
|
||||
}
|
||||
|
||||
Command photonAimAtAprilTag(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
|
||||
return new RunCommand(() -> {
|
||||
double rot = 0.0;
|
||||
var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult();
|
||||
if (result.hasTargets()) {
|
||||
double yawError = result.getBestTarget().getYaw();
|
||||
rot = photonAimPIDController.calculate(yawError, 0);
|
||||
}
|
||||
|
||||
rot = MathUtil.clamp(rot, -3.0, 3.0);
|
||||
|
||||
swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
|
||||
driverXbox.getLeftX() * -1), rot, false);
|
||||
}, swerveDrive);
|
||||
}
|
||||
|
||||
public static void getHubPoseTheta(SwerveSubsystem swerveDrive) {
|
||||
if (alliance.isPresent()) {
|
||||
if (alliance.get() == Alliance.Blue) {
|
||||
hubThetaPose = new Rotation2d(
|
||||
Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX()));
|
||||
|
||||
Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE,
|
||||
Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose);
|
||||
}
|
||||
|
||||
else {
|
||||
hubThetaPose = new Rotation2d(
|
||||
Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(),
|
||||
Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX()));
|
||||
Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED,
|
||||
Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
public static void updateShooterRPM(Pose2d currentRobotPose) {
|
||||
double distance = PhotonUtils.getDistanceToPose(currentRobotPose,
|
||||
Constants.TargetingConstants.allianceHubPose);
|
||||
Constants.ShooterConstants.SHOOTER_RPM = Math.max((-293.84123 * Math.pow(distance, 3))
|
||||
+ (1360.01497 * Math.pow(distance, 2))
|
||||
- (2391.17127 * distance)
|
||||
- 1249.22704, -6000);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
alliance = DriverStation.getAlliance();
|
||||
SmartDashboard.putString("Target Hub Pose",
|
||||
Constants.TargetingConstants.allianceHubPose.getX() + " " + Constants.TargetingConstants.allianceHubPose.getY() + " " + Constants.TargetingConstants.allianceHubPose.getRotation());
|
||||
|
||||
SmartDashboard.putString("Hub Pose", "x: " + Constants.TargetingConstants.allianceHubPose.getMeasureX() + " y: " + Constants.TargetingConstants.allianceHubPose.getY()
|
||||
+ " angle: " + Constants.TargetingConstants.allianceHubPose.getRotation());
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -16,12 +16,12 @@ import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.networktables.NetworkTablesJNI;
|
||||
import edu.wpi.first.wpilibj.Alert;
|
||||
import edu.wpi.first.wpilibj.Alert.AlertType;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import frc.robot.Robot;
|
||||
import frc.robot.Constants;
|
||||
import java.awt.Desktop;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
@@ -39,60 +39,59 @@ import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
import swervelib.SwerveDrive;
|
||||
import swervelib.telemetry.SwerveDriveTelemetry;
|
||||
|
||||
import frc.robot.Constants;
|
||||
|
||||
/**
|
||||
* Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from
|
||||
* Example PhotonVision class to aid in the pursuit of accurate odometry. Taken
|
||||
* from
|
||||
* https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads
|
||||
*/
|
||||
public class Vision
|
||||
{
|
||||
public class Vision {
|
||||
|
||||
/**
|
||||
* April Tag Field Layout of the year.
|
||||
*/
|
||||
public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
|
||||
AprilTagFields.k2025ReefscapeAndyMark);
|
||||
public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
|
||||
AprilTagFields.k2026RebuiltAndymark);
|
||||
/**
|
||||
* Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
|
||||
* Ambiguity defined as a value between (0,1). Used in
|
||||
* {@link Vision#filterPose}.
|
||||
*/
|
||||
private final double maximumAmbiguity = 0.25;
|
||||
private final double maximumAmbiguity = 0.25;
|
||||
/**
|
||||
* Photon Vision Simulation
|
||||
*/
|
||||
public VisionSystemSim visionSim;
|
||||
public VisionSystemSim visionSim;
|
||||
/**
|
||||
* Count of times that the odom thinks we're more than 10meters away from the april tag.
|
||||
* Count of times that the odom thinks we're more than 10meters away from the
|
||||
* april tag.
|
||||
*/
|
||||
private double longDistangePoseEstimationCount = 0;
|
||||
private double longDistangePoseEstimationCount = 0;
|
||||
/**
|
||||
* Current pose from the pose estimator using wheel odometry.
|
||||
*/
|
||||
private Supplier<Pose2d> currentPose;
|
||||
private Supplier<Pose2d> currentPose;
|
||||
/**
|
||||
* Field from {@link swervelib.SwerveDrive#field}
|
||||
*/
|
||||
private Field2d field2d;
|
||||
|
||||
private Field2d field2d;
|
||||
|
||||
/**
|
||||
* Constructor for the Vision class.
|
||||
*
|
||||
* @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()}
|
||||
* @param currentPose Current pose supplier, should reference
|
||||
* {@link SwerveDrive#getPose()}
|
||||
* @param field Current field, should be {@link SwerveDrive#field}
|
||||
*/
|
||||
public Vision(Supplier<Pose2d> currentPose, Field2d field)
|
||||
{
|
||||
public Vision(Supplier<Pose2d> currentPose, Field2d field) {
|
||||
this.currentPose = currentPose;
|
||||
this.field2d = field;
|
||||
|
||||
if (Robot.isSimulation())
|
||||
{
|
||||
if (Robot.isSimulation()) {
|
||||
visionSim = new VisionSystemSim("Vision");
|
||||
visionSim.addAprilTags(fieldLayout);
|
||||
|
||||
for (Cameras c : Cameras.values())
|
||||
{
|
||||
for (Cameras c : Cameras.values()) {
|
||||
c.addToVisionSim(visionSim);
|
||||
}
|
||||
|
||||
@@ -104,50 +103,48 @@ public class Vision
|
||||
* Calculates a target pose relative to an AprilTag on the field.
|
||||
*
|
||||
* @param aprilTag The ID of the AprilTag.
|
||||
* @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position
|
||||
* @param robotOffset The offset {@link Transform2d} of the robot to apply to
|
||||
* the pose for the robot to position
|
||||
* itself correctly.
|
||||
* @return The target pose of the AprilTag.
|
||||
*/
|
||||
public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
|
||||
{
|
||||
public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) {
|
||||
Optional<Pose3d> aprilTagPose3d = fieldLayout.getTagPose(aprilTag);
|
||||
if (aprilTagPose3d.isPresent())
|
||||
{
|
||||
if (aprilTagPose3d.isPresent()) {
|
||||
return aprilTagPose3d.get().toPose2d().transformBy(robotOffset);
|
||||
} else
|
||||
{
|
||||
} else {
|
||||
throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the pose estimation inside of {@link SwerveDrive} with all of the given poses.
|
||||
* Update the pose estimation inside of {@link SwerveDrive} with all of the
|
||||
* given poses.
|
||||
*
|
||||
* @param swerveDrive {@link SwerveDrive} instance.
|
||||
*/
|
||||
public void updatePoseEstimation(SwerveDrive swerveDrive)
|
||||
{
|
||||
if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent())
|
||||
{
|
||||
public void updatePoseEstimation(SwerveDrive swerveDrive) {
|
||||
if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) {
|
||||
/*
|
||||
* In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting.
|
||||
* In the maple-sim, odometry is simulated using encoder values, accounting for
|
||||
* factors like skidding and drifting.
|
||||
* As a result, the odometry may not always be 100% accurate.
|
||||
* However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect.
|
||||
* However, the vision system should be able to provide a reasonably accurate
|
||||
* pose estimation, even when odometry is incorrect.
|
||||
* (This is why teams implement vision system to correct odometry.)
|
||||
* Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation.
|
||||
* Therefore, we must ensure that the actual robot pose is provided in the
|
||||
* simulator when updating the vision simulation during the simulation.
|
||||
*/
|
||||
visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
|
||||
}
|
||||
for (Cameras camera : Cameras.values())
|
||||
{
|
||||
for (Cameras camera : Cameras.values()) {
|
||||
Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
|
||||
if (poseEst.isPresent())
|
||||
{
|
||||
if (poseEst.isPresent()) {
|
||||
var pose = poseEst.get();
|
||||
swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(),
|
||||
pose.timestampSeconds,
|
||||
camera.curStdDevs);
|
||||
pose.timestampSeconds,
|
||||
camera.curStdDevs);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -156,24 +153,22 @@ public class Vision
|
||||
/**
|
||||
* Generates the estimated robot pose. Returns empty if:
|
||||
* <ul>
|
||||
* <li> No Pose Estimates could be generated</li>
|
||||
* <li> The generated pose estimate was considered not accurate</li>
|
||||
* <li>No Pose Estimates could be generated</li>
|
||||
* <li>The generated pose estimate was considered not accurate</li>
|
||||
* </ul>
|
||||
*
|
||||
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate
|
||||
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and
|
||||
* targets used to create the estimate
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
|
||||
{
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera) {
|
||||
Optional<EstimatedRobotPose> poseEst = camera.getEstimatedGlobalPose();
|
||||
if (Robot.isSimulation())
|
||||
{
|
||||
if (Robot.isSimulation()) {
|
||||
Field2d debugField = visionSim.getDebugField();
|
||||
// Uncomment to enable outputting of vision targets in sim.
|
||||
poseEst.ifPresentOrElse(
|
||||
est ->
|
||||
debugField
|
||||
.getObject("VisionEstimation")
|
||||
.setPose(est.estimatedPose.toPose2d()),
|
||||
est -> debugField
|
||||
.getObject("VisionEstimation")
|
||||
.setPose(est.estimatedPose.toPose2d()),
|
||||
() -> {
|
||||
debugField.getObject("VisionEstimation").setPoses();
|
||||
});
|
||||
@@ -181,46 +176,39 @@ public class Vision
|
||||
return poseEst;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than
|
||||
* Filter pose via the ambiguity and find best estimate between all of the
|
||||
* camera's throwing out distances more than
|
||||
* 10m for a short amount of time.
|
||||
*
|
||||
* @param pose Estimated robot pose.
|
||||
* @return Could be empty if there isn't a good reading.
|
||||
*/
|
||||
@Deprecated(since = "2024", forRemoval = true)
|
||||
private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose)
|
||||
{
|
||||
if (pose.isPresent())
|
||||
{
|
||||
private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose) {
|
||||
if (pose.isPresent()) {
|
||||
double bestTargetAmbiguity = 1; // 1 is max ambiguity
|
||||
for (PhotonTrackedTarget target : pose.get().targetsUsed)
|
||||
{
|
||||
for (PhotonTrackedTarget target : pose.get().targetsUsed) {
|
||||
double ambiguity = target.getPoseAmbiguity();
|
||||
if (ambiguity != -1 && ambiguity < bestTargetAmbiguity)
|
||||
{
|
||||
if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) {
|
||||
bestTargetAmbiguity = ambiguity;
|
||||
}
|
||||
}
|
||||
//ambiguity to high dont use estimate
|
||||
if (bestTargetAmbiguity > maximumAmbiguity)
|
||||
{
|
||||
// ambiguity to high dont use estimate
|
||||
if (bestTargetAmbiguity > maximumAmbiguity) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
//est pose is very far from recorded robot pose
|
||||
if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1)
|
||||
{
|
||||
// est pose is very far from recorded robot pose
|
||||
if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) {
|
||||
longDistangePoseEstimationCount++;
|
||||
|
||||
//if it calculates that were 10 meter away for more than 10 times in a row its probably right
|
||||
if (longDistangePoseEstimationCount < 10)
|
||||
{
|
||||
// if it calculates that were 10 meter away for more than 10 times in a row its
|
||||
// probably right
|
||||
if (longDistangePoseEstimationCount < 10) {
|
||||
return Optional.empty();
|
||||
}
|
||||
} else
|
||||
{
|
||||
} else {
|
||||
longDistangePoseEstimationCount = 0;
|
||||
}
|
||||
return pose;
|
||||
@@ -228,15 +216,13 @@ public class Vision
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Get distance of the robot from the AprilTag pose.
|
||||
*
|
||||
* @param id AprilTag ID
|
||||
* @return Distance
|
||||
*/
|
||||
public double getDistanceFromAprilTag(int id)
|
||||
{
|
||||
public double getDistanceFromAprilTag(int id) {
|
||||
Optional<Pose3d> tag = fieldLayout.getTagPose(id);
|
||||
return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
|
||||
}
|
||||
@@ -248,17 +234,12 @@ public class Vision
|
||||
* @param camera Camera to check.
|
||||
* @return Tracked target.
|
||||
*/
|
||||
public PhotonTrackedTarget getTargetFromId(int id, Cameras camera)
|
||||
{
|
||||
public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) {
|
||||
PhotonTrackedTarget target = null;
|
||||
for (PhotonPipelineResult result : camera.resultsList)
|
||||
{
|
||||
if (result.hasTargets())
|
||||
{
|
||||
for (PhotonTrackedTarget i : result.getTargets())
|
||||
{
|
||||
if (i.getFiducialId() == id)
|
||||
{
|
||||
for (PhotonPipelineResult result : camera.resultsList) {
|
||||
if (result.hasTargets()) {
|
||||
for (PhotonTrackedTarget i : result.getTargets()) {
|
||||
if (i.getFiducialId() == id) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
@@ -273,54 +254,46 @@ public class Vision
|
||||
*
|
||||
* @return Vision Simulation
|
||||
*/
|
||||
public VisionSystemSim getVisionSim()
|
||||
{
|
||||
public VisionSystemSim getVisionSim() {
|
||||
return visionSim;
|
||||
}
|
||||
|
||||
/**
|
||||
* Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost.
|
||||
* Open up the photon vision camera streams on the localhost, assumes running
|
||||
* photon vision on localhost.
|
||||
*/
|
||||
private void openSimCameraViews()
|
||||
{
|
||||
if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
|
||||
{
|
||||
// try
|
||||
// {
|
||||
// Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
|
||||
// Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
|
||||
// Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
|
||||
// } catch (IOException | URISyntaxException e)
|
||||
// {
|
||||
// e.printStackTrace();
|
||||
// }
|
||||
private void openSimCameraViews() {
|
||||
if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) {
|
||||
// try
|
||||
// {
|
||||
// Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
|
||||
// Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
|
||||
// Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
|
||||
// } catch (IOException | URISyntaxException e)
|
||||
// {
|
||||
// e.printStackTrace();
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the {@link Field2d} to include tracked targets/
|
||||
*/
|
||||
public void updateVisionField()
|
||||
{
|
||||
public void updateVisionField() {
|
||||
|
||||
List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
|
||||
for (Cameras c : Cameras.values())
|
||||
{
|
||||
if (!c.resultsList.isEmpty())
|
||||
{
|
||||
for (Cameras c : Cameras.values()) {
|
||||
if (!c.resultsList.isEmpty()) {
|
||||
PhotonPipelineResult latest = c.resultsList.get(0);
|
||||
if (latest.hasTargets())
|
||||
{
|
||||
if (latest.hasTargets()) {
|
||||
targets.addAll(latest.targets);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
List<Pose2d> poses = new ArrayList<>();
|
||||
for (PhotonTrackedTarget target : targets)
|
||||
{
|
||||
if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
|
||||
{
|
||||
for (PhotonTrackedTarget target : targets) {
|
||||
if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) {
|
||||
Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
|
||||
poses.add(targetPose);
|
||||
}
|
||||
@@ -332,98 +305,103 @@ public class Vision
|
||||
/**
|
||||
* Camera Enum to select each camera
|
||||
*/
|
||||
enum Cameras
|
||||
{
|
||||
enum Cameras {
|
||||
/**
|
||||
* Left Camera
|
||||
* Back Left Camera
|
||||
*/
|
||||
FRONT_LEFT("Front Left Camera",
|
||||
Constants.TargetingConstants.FRONT_LEFT_CAMERA_ANGLE_RADIANS,
|
||||
Constants.TargetingConstants.FRONT_LEFT_CAMERA_LOCATION_METERS,
|
||||
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
|
||||
Constants.TargetingConstants.MULTI_TAG_STD_DEVS),
|
||||
BACK_LEFT_CAMERA("Rear Left Camera",
|
||||
Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getRotation(),
|
||||
Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getTranslation(),
|
||||
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
|
||||
/**
|
||||
* Right Camera
|
||||
* Back Right Camera
|
||||
*/
|
||||
FRONT_RIGHT("Front Right Camera",
|
||||
Constants.TargetingConstants.FRONT_RIGHT_CAMERA_ANGLE_RADIANS,
|
||||
Constants.TargetingConstants.FRONT_RIGHT_CAMERA_LOCATION_METERS,
|
||||
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
|
||||
Constants.TargetingConstants.MULTI_TAG_STD_DEVS),
|
||||
BACK_RIGHT_CAM("Rear Right Camera",
|
||||
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(),
|
||||
Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(),
|
||||
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
|
||||
/**
|
||||
* Center Camera
|
||||
* Front Left Camera
|
||||
*/
|
||||
REAR_LEFT("Rear Left Camera",
|
||||
Constants.TargetingConstants.REAR_LEFT_CAMERA_ANGLE_RADIANS,
|
||||
Constants.TargetingConstants.REAR_LEFT_CAMERA_LOCATION_METERS,
|
||||
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
|
||||
Constants.TargetingConstants.MULTI_TAG_STD_DEVS),
|
||||
FRONT_LEFT_CAM("Front Left Camera",
|
||||
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(),
|
||||
Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(),
|
||||
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
|
||||
|
||||
|
||||
REAR_RIGHT("Rear Right Camera",
|
||||
Constants.TargetingConstants.REAR_RIGHT_CAMERA_ANGLE_RADIANS,
|
||||
Constants.TargetingConstants.REAR_RIGHT_CAMERA_LOCATION_METERS,
|
||||
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
|
||||
Constants.TargetingConstants.MULTI_TAG_STD_DEVS);
|
||||
/**
|
||||
* Front Right Camera
|
||||
*/
|
||||
/*PURPLE_PHOTON_CAM("Front Right Camera",
|
||||
Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getRotation(),
|
||||
Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getTranslation(),
|
||||
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/
|
||||
|
||||
/**
|
||||
* Latency alert to use when high latency is detected.
|
||||
*/
|
||||
public final Alert latencyAlert;
|
||||
public final Alert latencyAlert;
|
||||
/**
|
||||
* Camera instance for comms.
|
||||
*/
|
||||
public final PhotonCamera camera;
|
||||
public final PhotonCamera camera;
|
||||
/**
|
||||
* Pose estimator for camera.
|
||||
*/
|
||||
public final PhotonPoseEstimator poseEstimator;
|
||||
public final PhotonPoseEstimator poseEstimator;
|
||||
/**
|
||||
* Standard Deviation for single tag readings for pose estimation.
|
||||
*/
|
||||
private final Matrix<N3, N1> singleTagStdDevs;
|
||||
private final Matrix<N3, N1> singleTagStdDevs;
|
||||
/**
|
||||
* Standard deviation for multi-tag readings for pose estimation.
|
||||
*/
|
||||
private final Matrix<N3, N1> multiTagStdDevs;
|
||||
private final Matrix<N3, N1> multiTagStdDevs;
|
||||
/**
|
||||
* Transform of the camera rotation and translation relative to the center of the robot
|
||||
* Transform of the camera rotation and translation relative to the center of
|
||||
* the robot
|
||||
*/
|
||||
private final Transform3d robotToCamTransform;
|
||||
private final Transform3d robotToCamTransform;
|
||||
/**
|
||||
* Current standard deviations used.
|
||||
*/
|
||||
public Matrix<N3, N1> curStdDevs;
|
||||
public Matrix<N3, N1> curStdDevs;
|
||||
/**
|
||||
* Estimated robot pose.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
|
||||
public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
|
||||
|
||||
/**
|
||||
* Simulated camera instance which only exists during simulations.
|
||||
*/
|
||||
public PhotonCameraSim cameraSim;
|
||||
public PhotonCameraSim cameraSim;
|
||||
/**
|
||||
* Results list to be updated periodically and cached to avoid unnecessary queries.
|
||||
* Results list to be updated periodically and cached to avoid unnecessary
|
||||
* queries.
|
||||
*/
|
||||
public List<PhotonPipelineResult> resultsList = new ArrayList<>();
|
||||
public List<PhotonPipelineResult> resultsList = new ArrayList<>();
|
||||
/**
|
||||
* Last read from the camera timestamp to prevent lag due to slow data fetches.
|
||||
*/
|
||||
private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
|
||||
private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
|
||||
|
||||
/**
|
||||
* Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine
|
||||
* Construct a Photon Camera class with help. Standard deviations are fake
|
||||
* values, experiment and determine
|
||||
* estimation noise on an actual robot.
|
||||
*
|
||||
* @param name Name of the PhotonVision camera found in the PV UI.
|
||||
* @param name Name of the PhotonVision camera found in the PV
|
||||
* UI.
|
||||
* @param robotToCamRotation {@link Rotation3d} of the camera.
|
||||
* @param robotToCamTranslation {@link Translation3d} relative to the center of the robot.
|
||||
* @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera.
|
||||
* @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera.
|
||||
* @param robotToCamTranslation {@link Translation3d} relative to the center of
|
||||
* the robot.
|
||||
* @param singleTagStdDevs Single AprilTag standard deviations of estimated
|
||||
* poses from the camera.
|
||||
* @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated
|
||||
* poses from the camera.
|
||||
*/
|
||||
Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation,
|
||||
Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix)
|
||||
{
|
||||
Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix) {
|
||||
latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning);
|
||||
|
||||
camera = new PhotonCamera(name);
|
||||
@@ -432,21 +410,22 @@ public class Vision
|
||||
robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation);
|
||||
|
||||
poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout,
|
||||
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
|
||||
robotToCamTransform);
|
||||
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
|
||||
robotToCamTransform);
|
||||
poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
|
||||
this.singleTagStdDevs = singleTagStdDevs;
|
||||
this.multiTagStdDevs = multiTagStdDevsMatrix;
|
||||
|
||||
if (Robot.isSimulation())
|
||||
{
|
||||
if (Robot.isSimulation()) {
|
||||
SimCameraProperties cameraProp = new SimCameraProperties();
|
||||
// A 640 x 480 camera with a 100 degree diagonal FOV.
|
||||
cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100));
|
||||
// Approximate detection noise with average and standard deviation error in pixels.
|
||||
// Approximate detection noise with average and standard deviation error in
|
||||
// pixels.
|
||||
cameraProp.setCalibError(0.25, 0.08);
|
||||
// Set the camera image capture framerate (Note: this is limited by robot loop rate).
|
||||
// Set the camera image capture framerate (Note: this is limited by robot loop
|
||||
// rate).
|
||||
cameraProp.setFPS(30);
|
||||
// The average and standard deviation in milliseconds of image data latency.
|
||||
cameraProp.setAvgLatencyMs(35);
|
||||
@@ -462,35 +441,31 @@ public class Vision
|
||||
*
|
||||
* @param systemSim {@link VisionSystemSim} to use.
|
||||
*/
|
||||
public void addToVisionSim(VisionSystemSim systemSim)
|
||||
{
|
||||
if (Robot.isSimulation())
|
||||
{
|
||||
public void addToVisionSim(VisionSystemSim systemSim) {
|
||||
if (Robot.isSimulation()) {
|
||||
systemSim.addCamera(cameraSim, robotToCamTransform);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most
|
||||
* Get the result with the least ambiguity from the best tracked target within
|
||||
* the Cache. This may not be the most
|
||||
* recent result!
|
||||
*
|
||||
* @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result!
|
||||
* @return The result in the cache with the least ambiguous best tracked target.
|
||||
* This is not the most recent result!
|
||||
*/
|
||||
public Optional<PhotonPipelineResult> getBestResult()
|
||||
{
|
||||
if (resultsList.isEmpty())
|
||||
{
|
||||
public Optional<PhotonPipelineResult> getBestResult() {
|
||||
if (resultsList.isEmpty()) {
|
||||
return Optional.empty();
|
||||
}
|
||||
|
||||
PhotonPipelineResult bestResult = resultsList.get(0);
|
||||
double amiguity = bestResult.getBestTarget().getPoseAmbiguity();
|
||||
double currentAmbiguity = 0;
|
||||
for (PhotonPipelineResult result : resultsList)
|
||||
{
|
||||
PhotonPipelineResult bestResult = resultsList.get(0);
|
||||
double amiguity = bestResult.getBestTarget().getPoseAmbiguity();
|
||||
double currentAmbiguity = 0;
|
||||
for (PhotonPipelineResult result : resultsList) {
|
||||
currentAmbiguity = result.getBestTarget().getPoseAmbiguity();
|
||||
if (currentAmbiguity < amiguity && currentAmbiguity > 0)
|
||||
{
|
||||
if (currentAmbiguity < amiguity && currentAmbiguity > 0) {
|
||||
bestResult = result;
|
||||
amiguity = currentAmbiguity;
|
||||
}
|
||||
@@ -501,63 +476,63 @@ public class Vision
|
||||
/**
|
||||
* Get the latest result from the current cache.
|
||||
*
|
||||
* @return Empty optional if nothing is found. Latest result if something is there.
|
||||
* @return Empty optional if nothing is found. Latest result if something is
|
||||
* there.
|
||||
*/
|
||||
public Optional<PhotonPipelineResult> getLatestResult()
|
||||
{
|
||||
public Optional<PhotonPipelineResult> getLatestResult() {
|
||||
return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the
|
||||
* Get the estimated robot pose. Updates the current robot pose estimation,
|
||||
* standard deviations, and flushes the
|
||||
* cache of results.
|
||||
*
|
||||
* @return Estimated pose.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose()
|
||||
{
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
|
||||
updateUnreadResults();
|
||||
return estimatedRobotPose;
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
|
||||
* Update the latest results, cached with a maximum refresh rate of 1req/15ms.
|
||||
* Sorts the list by timestamp.
|
||||
*/
|
||||
private void updateUnreadResults()
|
||||
{
|
||||
private void updateUnreadResults() {
|
||||
double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
|
||||
|
||||
for (PhotonPipelineResult result : resultsList)
|
||||
{
|
||||
|
||||
for (PhotonPipelineResult result : resultsList) {
|
||||
mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
|
||||
}
|
||||
|
||||
resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
|
||||
resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
|
||||
return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
|
||||
});
|
||||
if (!resultsList.isEmpty())
|
||||
{
|
||||
updateEstimatedGlobalPose();
|
||||
}
|
||||
resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
|
||||
resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
|
||||
return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
|
||||
});
|
||||
if (!resultsList.isEmpty()) {
|
||||
updateEstimatedGlobalPose();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once
|
||||
* The latest estimated robot pose on the field from vision data. This may be
|
||||
* empty. This should only be called once
|
||||
* per loop.
|
||||
*
|
||||
* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
|
||||
* <p>
|
||||
* Also includes updates for the standard deviations, which can (optionally) be
|
||||
* retrieved with
|
||||
* {@link Cameras#updateEstimationStdDevs}
|
||||
*
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
|
||||
* estimation.
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate
|
||||
* timestamp, and targets used for
|
||||
* estimation.
|
||||
*/
|
||||
private void updateEstimatedGlobalPose()
|
||||
{
|
||||
private void updateEstimatedGlobalPose() {
|
||||
Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
for (var change : resultsList)
|
||||
{
|
||||
for (var change : resultsList) {
|
||||
visionEst = poseEstimator.update(change);
|
||||
updateEstimationStdDevs(visionEst, change.getTargets());
|
||||
}
|
||||
@@ -565,63 +540,54 @@ public class Vision
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based
|
||||
* Calculates new standard deviations This algorithm is a heuristic that creates
|
||||
* dynamic standard deviations based
|
||||
* on number of tags, estimation strategy, and distance from the tags.
|
||||
*
|
||||
* @param estimatedPose The estimated pose to guess standard deviations for.
|
||||
* @param targets All targets in this camera frame
|
||||
*/
|
||||
private void updateEstimationStdDevs(
|
||||
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets)
|
||||
{
|
||||
if (estimatedPose.isEmpty())
|
||||
{
|
||||
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
|
||||
if (estimatedPose.isEmpty()) {
|
||||
// No pose input. Default to single-tag std devs
|
||||
curStdDevs = singleTagStdDevs;
|
||||
|
||||
} else
|
||||
{
|
||||
} else {
|
||||
// Pose present. Start running Heuristic
|
||||
var estStdDevs = singleTagStdDevs;
|
||||
int numTags = 0;
|
||||
double avgDist = 0;
|
||||
var estStdDevs = singleTagStdDevs;
|
||||
int numTags = 0;
|
||||
double avgDist = 0;
|
||||
|
||||
// Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
for (var tgt : targets)
|
||||
{
|
||||
// Precalculation - see how many tags we found, and calculate an
|
||||
// average-distance metric
|
||||
for (var tgt : targets) {
|
||||
var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
if (tagPose.isEmpty())
|
||||
{
|
||||
if (tagPose.isEmpty()) {
|
||||
continue;
|
||||
}
|
||||
numTags++;
|
||||
avgDist +=
|
||||
tagPose
|
||||
.get()
|
||||
.toPose2d()
|
||||
.getTranslation()
|
||||
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
avgDist += tagPose
|
||||
.get()
|
||||
.toPose2d()
|
||||
.getTranslation()
|
||||
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
}
|
||||
|
||||
if (numTags == 0)
|
||||
{
|
||||
if (numTags == 0) {
|
||||
// No tags visible. Default to single-tag std devs
|
||||
curStdDevs = singleTagStdDevs;
|
||||
} else
|
||||
{
|
||||
} else {
|
||||
// One or more tags visible, run the full heuristic.
|
||||
avgDist /= numTags;
|
||||
// Decrease std devs if multiple targets are visible
|
||||
if (numTags > 1)
|
||||
{
|
||||
if (numTags > 1) {
|
||||
estStdDevs = multiTagStdDevs;
|
||||
}
|
||||
// Increase std devs based on (average) distance
|
||||
if (numTags == 1 && avgDist > 4)
|
||||
{
|
||||
if (numTags == 1 && avgDist > 4) {
|
||||
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
|
||||
} else
|
||||
{
|
||||
} else {
|
||||
estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
|
||||
}
|
||||
curStdDevs = estStdDevs;
|
||||
|
||||
Reference in New Issue
Block a user