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,
|
||||
|
||||
@@ -7,6 +7,8 @@ package frc.robot;
|
||||
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;
|
||||
@@ -68,115 +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 double SHOOTER_MOTOR_P = 0.0018;
|
||||
public static double SHOOTER_MOTOR_P = 0.001;
|
||||
public static double SHOOTER_MOTOR_I = 0;
|
||||
public static double SHOOTER_MOTOR_D = 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 double INDEXER_MOTOR_P = 0.0001;
|
||||
public static double INDEXER_MOTOR_I = 0;
|
||||
public static double INDEXER_MOTOR_D = 0;
|
||||
|
||||
/*
|
||||
* private static GenericEntry indexerAndRampRPM =
|
||||
* programmingTab.add("Desired Ramp + Indexer RPM", 2000)
|
||||
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
|
||||
*/
|
||||
|
||||
/*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_ROTATOR_MOTOR_ID = 51;
|
||||
|
||||
public static class IntakeRotatorPID {
|
||||
public static final double INTAKE_ROTATOR_P = 0.03;
|
||||
public static final double INTAKE_ROTATOR_P = .07;
|
||||
public static final double INTAKE_ROTATOR_I = 0;
|
||||
public static final double INTAKE_ROTATOR_D = 0;
|
||||
public static final double INTAKE_ROTATOR_D = 0.03;
|
||||
}
|
||||
|
||||
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;
|
||||
public static final double INTAKE_MOTOR_D = 0.00005;
|
||||
|
||||
public static final double INTAKE_COLLECT_ENCODER_VALUE = 5;
|
||||
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5;
|
||||
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
|
||||
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_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_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 = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
|
||||
public static final Pose2d LEFT_CLIMB_POSE = 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 PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera");
|
||||
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera");
|
||||
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 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 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 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 final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d());
|
||||
public static Rotation2d HUB_ROTATION_BLUE;
|
||||
public static Rotation2d HUB_ROTATION_RED;
|
||||
public static Pose2d allianceHubPose = new Pose2d();
|
||||
|
||||
public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
|
||||
new Rotation3d(0, 0, 0));
|
||||
public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
|
||||
new Rotation3d(0, 0, 0));
|
||||
public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
|
||||
new Rotation3d(0, 0, 0));
|
||||
public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
|
||||
new Rotation3d(0, 0, 0));
|
||||
|
||||
public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
|
||||
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
|
||||
ORANGE_ROBOT_TO_CAM);
|
||||
public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
|
||||
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
|
||||
BLACK_ROBOT_TO_CAM);
|
||||
public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
|
||||
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
|
||||
RED_ROBOT_TO_CAM);
|
||||
public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
|
||||
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
|
||||
PURPLE_ROBOT_TO_CAM);
|
||||
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 RATCHET_UNLOCK_ANGLE = 0;
|
||||
public static final double RATCHET_LOCK_ANGLE = 180;
|
||||
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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -4,13 +4,18 @@
|
||||
|
||||
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;
|
||||
@@ -19,6 +24,11 @@ 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;
|
||||
|
||||
/**
|
||||
@@ -33,11 +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 GenericEntry distanceFromLimelight;
|
||||
|
||||
private static StructPublisher<Pose2d> advantageScopePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose hahaha", Pose2d.struct).publish();
|
||||
|
||||
public Robot() {
|
||||
instance = this;
|
||||
@@ -56,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
|
||||
@@ -68,6 +84,7 @@ public class Robot extends TimedRobot {
|
||||
}
|
||||
|
||||
m_robotContainer.getSwerveDrive().zeroGyroWithAlliance();
|
||||
IntakeSubsystem.resetIntakeRotationEncoder();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -90,21 +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();
|
||||
|
||||
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM,
|
||||
Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
|
||||
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM,
|
||||
Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
|
||||
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM,
|
||||
Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
|
||||
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM,
|
||||
Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
|
||||
|
||||
advantageScopePublisher.set(m_robotContainer.getSwerveDrive().getPose());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -132,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);
|
||||
@@ -142,6 +156,9 @@ public class Robot extends TimedRobot {
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -153,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,6 +4,7 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import com.fasterxml.jackson.databind.util.Named;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
|
||||
@@ -22,11 +23,15 @@ 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;
|
||||
@@ -39,7 +44,9 @@ 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
|
||||
@@ -51,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;
|
||||
@@ -8,48 +14,61 @@ import edu.wpi.first.wpilibj2.command.Command;
|
||||
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);
|
||||
public class ClimberSubsystem extends SubsystemBase {
|
||||
// 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);
|
||||
}
|
||||
|
||||
public void lowerRobot() {
|
||||
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
|
||||
}
|
||||
|
||||
public void stopClimber() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
|
||||
public Command liftRobotCommand() {
|
||||
return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot());
|
||||
}
|
||||
|
||||
public Command lowerRobotCommand() {
|
||||
return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot());
|
||||
}
|
||||
|
||||
public Command stopClimberCommand() {
|
||||
return runOnce(() -> stopClimber());
|
||||
}
|
||||
|
||||
public static void toggleRatchet(boolean toggle) {
|
||||
if (toggle == true) {
|
||||
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
|
||||
} else
|
||||
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
|
||||
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 Command toggleRatchetCommand(boolean toggle) {
|
||||
return runOnce(() -> toggleRatchet(toggle));
|
||||
public void setClimberSpeed(double speed) {
|
||||
climberMotor.set(speed);
|
||||
}
|
||||
|
||||
public Command setClimberSpeedCommand(double speed) {
|
||||
return runOnce(() -> setClimberSpeed(speed));
|
||||
}
|
||||
public void liftRobot() {
|
||||
climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LIFTED_SETPOINT_VALUE, ControlType.kPosition);
|
||||
}
|
||||
|
||||
public static void lowerRobot() {
|
||||
climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LOWERED_SETPOINT_VALUE, ControlType.kPosition);
|
||||
}
|
||||
|
||||
|
||||
public void stopClimber() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
|
||||
public Command liftRobotCommand() {
|
||||
return runOnce(() -> liftRobot());
|
||||
}
|
||||
|
||||
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,19 +1,34 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
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;
|
||||
|
||||
@@ -24,47 +39,104 @@ public class IntakeSubsystem extends SubsystemBase {
|
||||
|
||||
private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
|
||||
MotorType.kBrushless);
|
||||
|
||||
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 SparkClosedLoopController intakeWheelsMotorPIDController;
|
||||
public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
|
||||
|
||||
public IntakeSubsystem() {
|
||||
intakeRotatorConfig.closedLoop
|
||||
.p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
|
||||
.i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
|
||||
.d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
|
||||
public static DutyCycleEncoder intakeRotatorEncoder = new DutyCycleEncoder(1);
|
||||
|
||||
.p(.06, ClosedLoopSlot.kSlot1)
|
||||
.i(0, ClosedLoopSlot.kSlot1)
|
||||
.d(0, ClosedLoopSlot.kSlot1);
|
||||
private static double encoderValue = intakeRotatorEncoder.get();
|
||||
|
||||
public IntakeSubsystem() {
|
||||
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);
|
||||
|
||||
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)
|
||||
|
||||
// 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() {
|
||||
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity);
|
||||
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() {
|
||||
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity);
|
||||
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST * -1,
|
||||
ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public void stopIntakeMotor() {
|
||||
intakeWheelsMotor.set(0);
|
||||
}
|
||||
|
||||
public Command startIntakeMotorCommand() {
|
||||
return runOnce(() -> startIntakeMotor());
|
||||
public Command startIntakeMotorCommand(double speed) {
|
||||
return runOnce(() -> startIntakeMotor(speed));
|
||||
}
|
||||
|
||||
public Command reverseIntakeMotorCommand() {
|
||||
@@ -75,16 +147,37 @@ public class IntakeSubsystem extends SubsystemBase {
|
||||
return runOnce(() -> stopIntakeMotor());
|
||||
}
|
||||
|
||||
public void deployIntake() {
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0);
|
||||
/*
|
||||
* 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() {
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1);
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE,
|
||||
ControlType.kPosition, ClosedLoopSlot.kSlot1);
|
||||
}
|
||||
|
||||
public Command retractIntakeCommand() {
|
||||
@@ -93,16 +186,29 @@ public class IntakeSubsystem extends SubsystemBase {
|
||||
|
||||
public void assistFuelIntake() {
|
||||
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
|
||||
ControlType.kPosition);
|
||||
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() {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -11,11 +11,13 @@ 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;
|
||||
@@ -23,8 +25,7 @@ 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 frc.robot.LimelightHelpers;
|
||||
import edu.wpi.first.wpilibj.AnalogPotentiometer;
|
||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||
|
||||
public class ShooterSubsystem extends SubsystemBase {
|
||||
private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
|
||||
@@ -52,107 +53,149 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
|
||||
|
||||
public ShooterSubsystem() {
|
||||
centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
|
||||
Constants.ShooterConstants.SHOOTER_MOTOR_I,
|
||||
Constants.ShooterConstants.SHOOTER_MOTOR_D);
|
||||
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);
|
||||
|
||||
centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
|
||||
com.revrobotics.PersistMode.kNoPersistParameters);
|
||||
centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
|
||||
|
||||
leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
|
||||
Constants.ShooterConstants.SHOOTER_MOTOR_I,
|
||||
Constants.ShooterConstants.SHOOTER_MOTOR_D);
|
||||
|
||||
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.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
|
||||
Constants.ShooterConstants.SHOOTER_MOTOR_I,
|
||||
Constants.ShooterConstants.SHOOTER_MOTOR_D);
|
||||
|
||||
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(0.0001,
|
||||
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 = 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() {
|
||||
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
|
||||
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
|
||||
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
|
||||
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() {
|
||||
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
|
||||
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() {
|
||||
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
|
||||
rightShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command testRightShooterCommand() {
|
||||
return runOnce(() -> setRightShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void setCenterShooterMotorRPM() {
|
||||
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
|
||||
centerShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command testCenterShooterCommand() {
|
||||
return runOnce(() -> setCenterShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void stopLeftShooterMotorRPM() {
|
||||
leftShooterMotor.set(0);
|
||||
leftShooterMotor.set(0);
|
||||
}
|
||||
|
||||
public Command stopLeftShooterCommand() {
|
||||
return runOnce(() -> stopLeftShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void stopCenterShooterMotorRPM() {
|
||||
centerShooterMotor.set(0);
|
||||
centerShooterMotor.set(0);
|
||||
}
|
||||
|
||||
public Command stopCenterShooterCommand() {
|
||||
return runOnce(() -> stopCenterShooterMotorRPM());
|
||||
}
|
||||
|
||||
public void stopRightShooterMotorRPM() {
|
||||
rightShooterMotor.set(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, ControlType.kVelocity);
|
||||
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM,
|
||||
ControlType.kVelocity);
|
||||
}
|
||||
|
||||
|
||||
public void reverseIndexerAndRampMotorRPM() {
|
||||
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity);
|
||||
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1,
|
||||
ControlType.kVelocity);
|
||||
}
|
||||
|
||||
public Command reverseIndexerAndRampMotorRPMCommand() {
|
||||
@@ -164,20 +207,30 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command stopIndexerAndRampMotorCommand() {
|
||||
return runOnce(()-> indexerAndRampMotor.set(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() {
|
||||
@@ -193,34 +246,24 @@ public class ShooterSubsystem extends SubsystemBase {
|
||||
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() {
|
||||
|
||||
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());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,9 +1,11 @@
|
||||
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;
|
||||
@@ -33,8 +35,9 @@ 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.LimelightHelpers;
|
||||
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;
|
||||
@@ -49,10 +52,14 @@ import frc.robot.Constants;
|
||||
|
||||
public class TargetingSubsystems extends SubsystemBase {
|
||||
|
||||
PIDController photonAimPIDController = new PIDController(3, 0, 0.001);
|
||||
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;
|
||||
@@ -95,26 +102,25 @@ public class TargetingSubsystems extends SubsystemBase {
|
||||
}, swerveDrive);
|
||||
}
|
||||
|
||||
public Command aimAtHub(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
|
||||
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.HUB_POSE);
|
||||
Constants.TargetingConstants.allianceHubPose);
|
||||
|
||||
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
|
||||
angleDifference.getRadians());
|
||||
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,
|
||||
false);
|
||||
true);
|
||||
}, swerveDrive);
|
||||
}
|
||||
|
||||
Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
|
||||
Command photonAimAtAprilTag(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
|
||||
return new RunCommand(() -> {
|
||||
double rot = 0.0;
|
||||
var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult();
|
||||
@@ -125,83 +131,50 @@ public class TargetingSubsystems extends SubsystemBase {
|
||||
|
||||
rot = MathUtil.clamp(rot, -3.0, 3.0);
|
||||
|
||||
swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1,
|
||||
driverXbox.getLeftY() * -1), rot, false);
|
||||
swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
|
||||
driverXbox.getLeftX() * -1), rot, false);
|
||||
}, swerveDrive);
|
||||
}
|
||||
|
||||
public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) {
|
||||
return poseEstimator;
|
||||
}
|
||||
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()));
|
||||
|
||||
/*
|
||||
* // static public NetworkTable table =
|
||||
* //
|
||||
* NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME
|
||||
* );
|
||||
* // static public NetworkTableEntry ty = table.getEntry("ty");
|
||||
* // static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
|
||||
*
|
||||
* // how many degrees back is your limelight rotated from perfectly vertical?
|
||||
* static double limelightMountAngleDegrees = 25.0;
|
||||
*
|
||||
* // distance from the center of the Limelight lens to the floor
|
||||
* static double limelightLensHeightInches = 27.5;
|
||||
*
|
||||
* // distance from the target to the floor
|
||||
* static double goalHeightInches = 44;
|
||||
*
|
||||
* static double angleToGoalDegrees = limelightMountAngleDegrees +
|
||||
* Constants.LimeLight.LIMELIGHT_TY;
|
||||
* static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
|
||||
*
|
||||
* // calculate distance
|
||||
* static double distanceFromLimelightToGoalInches = (goalHeightInches -
|
||||
* limelightLensHeightInches)
|
||||
* / Math.tan(angleToGoalRadians);
|
||||
*
|
||||
* public static double getDistanceFromAprilTag() {
|
||||
* angleToGoalDegrees = limelightMountAngleDegrees +
|
||||
* Constants.LimeLight.LIMELIGHT_TY;
|
||||
* angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
|
||||
* distanceFromLimelightToGoalInches = (goalHeightInches -
|
||||
* limelightLensHeightInches)
|
||||
* / Math.tan(angleToGoalRadians);
|
||||
* return distanceFromLimelightToGoalInches;
|
||||
* }
|
||||
*/
|
||||
Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE,
|
||||
Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose);
|
||||
}
|
||||
|
||||
public static Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
|
||||
public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) {
|
||||
/* for (var result : camera.getAllUnreadResults()) {
|
||||
visionEst = poseEstimator.estimateCoprocMultiTagPose(result);
|
||||
|
||||
if (visionEst.isPresent()) {
|
||||
EstimatedRobotPose estimatedPose = visionEst.get();
|
||||
swerveDrive.getSwerveDrive()
|
||||
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
|
||||
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() {
|
||||
/*
|
||||
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
|
||||
* photonVision.getLatestResult().getBestTarget().getYaw());
|
||||
* Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
|
||||
* photonVision.getLatestResult().getBestTarget().getPitch());
|
||||
* Shuffleboard.getTab("Vision").add("Limelight TX Value",
|
||||
* LimelightHelpers.getTX("limelight"));
|
||||
* Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
|
||||
* LimelightHelpers.getFiducialID("limelight"));
|
||||
* Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
|
||||
* Shuffleboard.getTab("Vision").addCamera("Photon",
|
||||
* "Arducam_OV9281_USB_Camera",
|
||||
* "http://photonvision.local:5800");
|
||||
*/
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -27,14 +27,19 @@ import edu.wpi.first.math.trajectory.Trajectory;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.subsystems.TargetingSubsystems;
|
||||
import frc.robot.subsystems.swervedrive.Vision.Cameras;
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.lang.reflect.Field;
|
||||
import java.util.Arrays;
|
||||
import java.util.Optional;
|
||||
import java.util.concurrent.atomic.AtomicReference;
|
||||
@@ -64,11 +69,16 @@ public class SwerveSubsystem extends SubsystemBase {
|
||||
*/
|
||||
private final boolean visionDriveTest = true;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* PhotonVision class to keep an accurate odometry.
|
||||
*/
|
||||
private Vision vision;
|
||||
|
||||
private final Field2d rebuiltField = new Field2d();
|
||||
/**
|
||||
* Initialize {@link SwerveDrive} with the directory provided.
|
||||
*
|
||||
@@ -117,6 +127,10 @@ public class SwerveSubsystem extends SubsystemBase {
|
||||
swerveDrive.stopOdometryThread();
|
||||
}
|
||||
setupPathPlanner();
|
||||
SmartDashboard.putData("Rebuilt Field", rebuiltField);
|
||||
Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS = new PathConstraints(1, 4.0,
|
||||
swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(360));
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -146,8 +160,13 @@ public class SwerveSubsystem extends SubsystemBase {
|
||||
// When vision is enabled we must manually update odometry in SwerveDrive
|
||||
if (visionDriveTest) {
|
||||
swerveDrive.updateOdometry();
|
||||
//vision.updatePoseEstimation(swerveDrive);
|
||||
vision.updatePoseEstimation(swerveDrive);
|
||||
}
|
||||
|
||||
SmartDashboard.putString("Robot Pose ", "x: " + swerveDrive.getPose().getX() + "\ny: "
|
||||
+ swerveDrive.getPose().getY() + "\nrot: " + swerveDrive.getPose().getRotation());
|
||||
|
||||
rebuiltField.setRobotPose(getPose());
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -188,9 +207,9 @@ public class SwerveSubsystem extends SubsystemBase {
|
||||
new PPHolonomicDriveController(
|
||||
// PPHolonomicController is the built in path following controller for holonomic
|
||||
// drive trains
|
||||
new PIDConstants(5.0, 0.0, 0.0),
|
||||
new PIDConstants(3.6, 0.0, 0.0),
|
||||
// Translation PID constants
|
||||
new PIDConstants(3.8, 0.0, 0.0)
|
||||
new PIDConstants(3.675, 0.0, 0.00)
|
||||
// Rotation PID constants
|
||||
),
|
||||
config,
|
||||
@@ -274,6 +293,48 @@ public class SwerveSubsystem extends SubsystemBase {
|
||||
);
|
||||
}
|
||||
|
||||
public Command driveToClimbPoseOffsetted(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
|
||||
|
||||
Pose2d goal;
|
||||
Optional<Alliance> alliance = DriverStation.getAlliance();
|
||||
if (alliance.get() == Alliance.Blue) {
|
||||
goal = blueAlliancePose;
|
||||
} else {
|
||||
goal = redAlliancePose;
|
||||
}
|
||||
// Create the constraints to use while pathfinding
|
||||
PathConstraints constraints = new PathConstraints(
|
||||
swerveDrive.getMaximumChassisVelocity(), 4.0,
|
||||
swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720));
|
||||
|
||||
// Since AutoBuilder is configured, we can use it to build pathfinding commands
|
||||
return AutoBuilder.pathfindToPose(
|
||||
goal,
|
||||
constraints,
|
||||
edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
|
||||
);
|
||||
}
|
||||
|
||||
public Command driveIntoClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
|
||||
|
||||
Pose2d goal;
|
||||
Optional<Alliance> alliance = DriverStation.getAlliance();
|
||||
if (alliance.get() == Alliance.Blue) {
|
||||
goal = blueAlliancePose;
|
||||
} else {
|
||||
goal = redAlliancePose;
|
||||
}
|
||||
// Create the constraints to use while pathfinding
|
||||
|
||||
|
||||
// Since AutoBuilder is configured, we can use it to build pathfinding commands
|
||||
return AutoBuilder.pathfindToPose(
|
||||
goal,
|
||||
Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS,
|
||||
edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive with {@link SwerveSetpointGenerator} from 254, implemented by
|
||||
* PathPlanner.
|
||||
|
||||
@@ -39,6 +39,7 @@ 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
|
||||
@@ -306,32 +307,35 @@ public class Vision {
|
||||
*/
|
||||
enum Cameras {
|
||||
/**
|
||||
* Left Camera
|
||||
* Back Left Camera
|
||||
*/
|
||||
LEFT_CAM("left",
|
||||
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
|
||||
new Translation3d(Units.inchesToMeters(12.056),
|
||||
Units.inchesToMeters(10.981),
|
||||
Units.inchesToMeters(8.44)),
|
||||
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
|
||||
*/
|
||||
RIGHT_CAM("right",
|
||||
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
|
||||
new Translation3d(Units.inchesToMeters(12.056),
|
||||
Units.inchesToMeters(-10.981),
|
||||
Units.inchesToMeters(8.44)),
|
||||
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
|
||||
*/
|
||||
CENTER_CAM("center",
|
||||
new Rotation3d(0, Units.degreesToRadians(18), 0),
|
||||
new Translation3d(Units.inchesToMeters(-4.628),
|
||||
Units.inchesToMeters(-10.687),
|
||||
Units.inchesToMeters(16.129)),
|
||||
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));
|
||||
|
||||
|
||||
/**
|
||||
* 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.
|
||||
|
||||
Reference in New Issue
Block a user