Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

turned off idle speed for testing; added sysid code

Browse Source
This commit is contained in:
Mehooliu
2026-03-12 19:21:00 -04:00
parent 0e5561dba0
commit 4bb240138d
35 changed files with 950 additions and 2409 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.DataLogTool/datalogtool.json Normal file
View File

@@ -0,0 +1 @@
{}

1
.SysId/sysid.json Normal file
View File

@@ -0,0 +1 @@
{}

19
src/main/deploy/pathplanner/autos/90 Degree Test Auto.auto
View File

@@ -1,19 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "HP to hub and shoot"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

50
src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto → src/main/deploy/pathplanner/autos/Center Auto.auto
View File

@@ -10,28 +10,28 @@
"name": "Deploy_Intake_Command"
}
},
{
"type": "path",
"data": {
"pathName": "Starting to HP"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.5
}
},
{
"type": "path",
"data": {
"pathName": "HP to Hub"
}
},
{
"type": "named",
"data": {
"name": "Auto_Aim_To_Hub"
"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"
}
},
{
@@ -43,25 +43,25 @@
{
"type": "wait",
"data": {
"waitTime": 3.0
"waitTime": 2.0
}
},
{
"type": "named",
"data": {
"name": "Kill_All"
"name": "Assist_Shooter"
}
},
{
"type": "named",
"type": "path",
"data": {
"name": "PathPlan_To_Climb_Right_Offsetted"
"pathName": "Left Shoot to Offsetted-Left Climb"
}
},
{
"type": "named",
"type": "path",
"data": {
"name": "PathPlan_Into_Climb_Right"
"pathName": "Offsetted-Left to Left Climb"
}
},
{

31
src/main/deploy/pathplanner/autos/HP to Hub and Shoot Auto.auto
View File

@@ -1,31 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Deploy_Intake_Command"
}
},
{
"type": "path",
"data": {
"pathName": "HP to hub and shoot"
}
},
{
"type": "named",
"data": {
"name": "Shoot_Fuel_Command"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

10
src/main/deploy/pathplanner/autos/HP Auto + Climb.auto → src/main/deploy/pathplanner/autos/Human Player Auto.auto
View File

@@ -19,7 +19,7 @@
{
"type": "wait",
"data": {
"waitTime": 1.5
"waitTime": 1.25
}
},
{
@@ -37,7 +37,7 @@
{
"type": "wait",
"data": {
"waitTime": 1.0
"waitTime": 2.0
}
},
{
@@ -46,12 +46,6 @@
"name": "Assist_Shooter"
}
},
{
"type": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "named",
"data": {

79
src/main/deploy/pathplanner/autos/Left Side + Center Fuel Auto.auto Normal file
View File

@@ -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
}

2
src/main/deploy/pathplanner/navgrid.json
View File

File diff suppressed because one or more lines are too long

54
src/main/deploy/pathplanner/paths/Center to Floor Intake.path Normal file
View File

@@ -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
}

59
src/main/deploy/pathplanner/paths/Collect Left-Center Fuel to Shoot.path Normal file
View File

@@ -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
}

54
src/main/deploy/pathplanner/paths/Collect Left-Center Fuel.path Normal file
View File

@@ -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
}

54
src/main/deploy/pathplanner/paths/Floor Intake to Left Shoot.path Normal file
View File

@@ -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
View 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
}

20
src/main/deploy/pathplanner/paths/HP to Hub.path
View File

@@ -3,16 +3,16 @@
"waypoints": [
{
"anchor": {
"x": 0.447,
"x": 0.46,
"y": 0.634
},
"prevControl": null,
"nextControl": {
"x": 0.8005533905932738,
"y": 0.9875533905932738
"x": 0.8135533905932739,
"y": 0.9875533905932734
},
"isLocked": false,
"linkedName": null
"linkedName": "HP Waypoint"
},
{
"anchor": {
@@ -25,7 +25,7 @@
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "Right Shoot Position Waypoint"
}
],
"rotationTargets": [],
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 4.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 27.207
"rotation": 36.0
},
"reversed": false,
"folder": "Start to HP to Hub to Climb",
"folder": "HP to Shoot to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

18
src/main/deploy/pathplanner/paths/Hub to side of Climb.path
View File

@@ -12,20 +12,20 @@
"y": 2.3289899283371653
},
"isLocked": false,
"linkedName": null
"linkedName": "Right Shoot Position Waypoint"
},
{
"anchor": {
"x": 1.25,
"y": 2.0457840909090907
"x": 1.14,
"y": 2.046
},
"prevControl": {
"x": 1.7198463103929542,
"y": 2.2167941625719254
"x": 1.6098463103929541,
"y": 2.2170100716628345
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "Offsetted-Climb Waypoint"
}
],
"rotationTargets": [],
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -99.66394495351781
"rotation": -90.0
},
"reversed": false,
"folder": "Start to HP to Hub to Climb",
"folder": "HP to Shoot to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 27.207
"rotation": 36.0
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/Left Shoot to Offsetted-Left Climb.path Normal file
View File

@@ -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
}

20
src/main/deploy/pathplanner/paths/Hub to Shoot.path → src/main/deploy/pathplanner/paths/Left to Floor Intake.path
View File

@@ -4,28 +4,28 @@
{
"anchor": {
"x": 3.5185568181818176,
"y": 4.024693181818182
"y": 7.219806818181819
},
"prevControl": null,
"nextControl": {
"x": 2.879534090909091,
"y": 4.024693181818182
"x": 1.5190340909090907,
"y": 6.12728409090909
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.281738636363636,
"y": 4.024693181818182
"x": 1.1789090909090907,
"y": 5.941761363636363
},
"prevControl": {
"x": 2.8279999999999994,
"y": 4.024693181818182
"x": 2.9929090909090905,
"y": 6.910602272727273
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "Left Intake Balls Waypoint"
}
],
"rotationTargets": [],
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": 180.0
},
"reversed": false,
"folder": "Middle Shoot to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
"rotation": 180.0
},
"useDefaultConstraints": true
}

26
src/main/deploy/pathplanner/paths/Side of Climb to Climb.path → src/main/deploy/pathplanner/paths/Offsetted-Left to Left Climb.path
View File

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 0.8,
"y": 2.0
"x": 1.14,
"y": 5.0
},
"prevControl": null,
"nextControl": {
"x": 0.8,
"y": 2.5
"x": 1.14,
"y": 4.292625343835472
},
"isLocked": false,
"linkedName": null
"linkedName": "Ofsetted-Left Climb Waypoint"
},
{
"anchor": {
"x": 0.8,
"y": 2.7
"x": 1.14,
"y": 4.25
},
"prevControl": {
"x": 0.8,
"y": 2.2
"x": 1.14,
"y": 4.70494908797556
},
"nextControl": null,
"isLocked": false,
@@ -34,7 +34,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.0,
"maxAcceleration": 1.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.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": -90.0
"rotation": 90.0
},
"useDefaultConstraints": false
}

26
src/main/deploy/pathplanner/paths/Side into Climb.path
View File

@@ -3,29 +3,29 @@
"waypoints": [
{
"anchor": {
"x": 1.25,
"x": 1.14,
"y": 2.046
},
"prevControl": null,
"nextControl": {
"x": 1.302590909090909,
"y": 2.6126590909090908
"x": 1.1561466905187832,
"y": 2.7126296958855094
},
"isLocked": false,
"linkedName": null
"linkedName": "Offsetted-Right Climb Waypoint"
},
{
"anchor": {
"x": 1.37,
"y": 3.5
"x": 1.14,
"y": 3.25
},
"prevControl": {
"x": 1.3761477272727272,
"y": 3.190795454545455
"x": 1.146147727272727,
"y": 2.940795454545455
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "Right Climb Waypoint"
}
],
"rotationTargets": [],
@@ -33,7 +33,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 0.5,
"maxVelocity": 1.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -99.664
"rotation": -90.0
},
"reversed": false,
"folder": "Start to HP to Hub to Climb",
"folder": "HP to Shoot to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": -99.664
"rotation": -90.0
},
"useDefaultConstraints": false
}

49
src/main/deploy/pathplanner/paths/HP to hub and shoot.path → src/main/deploy/pathplanner/paths/Start to Over Left Ramp.path
View File

@@ -3,62 +3,53 @@
"waypoints": [
{
"anchor": {
"x": 0.4655813953488368,
"y": 0.6682289803220032
"x": 3.5185568181818176,
"y": 7.2095
},
"prevControl": null,
"nextControl": {
"x": 3.580858676207512,
"y": -0.27284436493738906
"x": 1.1273750000027687,
"y": 5.385193181820003
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.591109123434704,
"y": 2.1771914132379244
"x": 4.672920454548223,
"y": 5.477954545456365
},
"prevControl": {
"x": 3.078571154198153,
"y": 2.162419836548123
"x": 3.3539345896947985,
"y": 5.471848129415376
},
"nextControl": {
"x": 2.055670840787119,
"y": 2.1934168157423963
"x": 6.899193181820951,
"y": 5.488261363638184
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.077871198568873,
"y": 3.86463327370304
"x": 7.734045454548223,
"y": 7.054897727274547
},
"prevControl": {
"x": 1.9420930232558136,
"y": 4.026887298747764
"x": 5.971579545454545,
"y": 7.271340909090909
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.5,
"rotationDegrees": 180.0
},
{
"waypointRelativePos": 1.567134268537075,
"rotationDegrees": -1.0
"linkedName": "Over Left Ramp Waypoint"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxVelocity": 4.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
@@ -67,13 +58,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": -90.0
},
"reversed": false,
"folder": null,
"folder": "Left Auto",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
"rotation": -90.0
},
"useDefaultConstraints": false
}

20
src/main/deploy/pathplanner/paths/Starting to HP.path
View File

@@ -4,28 +4,28 @@
{
"anchor": {
"x": 3.5159570661896247,
"y": 0.4572987477638639
"y": 0.8
},
"prevControl": null,
"nextControl": {
"x": 2.8344901610017894,
"y": 0.4248479427549193
"x": 2.6424772727272727,
"y": 0.7471249999999998
},
"isLocked": false,
"linkedName": null
"linkedName": "Start Right Trench Waypoint"
},
{
"anchor": {
"x": 0.457431818181818,
"y": 0.63375
"x": 0.46,
"y": 0.634
},
"prevControl": {
"x": 1.0093340606142802,
"y": 0.6337499999999999
"x": 1.0119022424324617,
"y": 0.6340000000000003
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "HP Waypoint"
}
],
"rotationTargets": [],
@@ -45,7 +45,7 @@
"rotation": 180.0
},
"reversed": false,
"folder": "Start to HP to Hub to Climb",
"folder": "HP to Shoot to Climb",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0

18
src/main/deploy/pathplanner/paths/test_1_Meter.path
View File

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 15.474,
"y": 3.437204545454546
"x": 1.0,
"y": 1.87
},
"prevControl": null,
"nextControl": {
"x": 16.473999999999986,
"y": 3.437204545454546
"x": 1.9999999999999858,
"y": 1.87
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.066,
"x": 3.0,
"y": 1.87
},
"prevControl": {
"x": -0.07677934128731523,
"y": 1.87
"x": 1.8572206587126847,
"y": 1.8700000000000003
},
"nextControl": null,
"isLocked": false,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 90.0
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 90.0
"rotation": 0.0
},
"useDefaultConstraints": false
}

3
src/main/deploy/pathplanner/settings.json
View File

@@ -3,8 +3,9 @@
"robotLength": 0.9,
"holonomicMode": true,
"pathFolders": [
"HP to Shoot to Climb",
"Middle Shoot to Climb",
"Start to HP to Hub to Climb"
"Left Auto"
],
"autoFolders": [],
"defaultMaxVel": 3.0,

2
src/main/deploy/swerve/neo/modules/backright.json
View File

@@ -3,7 +3,7 @@
"front": -10.9,
"left": -10.9
},
"absoluteEncoderOffset": 287.578125,
"absoluteEncoderOffset": 283.7109375,
"drive": {
"type": "sparkflex_neo",
"id": 11,

2
src/main/deploy/swerve/neo/modules/frontleft.json
View File

@@ -3,7 +3,7 @@
"front": 10.9,
"left": 10.9
},
"absoluteEncoderOffset": 36.03515625,
"absoluteEncoderOffset": 34.716796875,
"drive": {
"type": "sparkflex_neo",
"id": 13,

40
src/main/java/frc/robot/Constants.java
View File

@@ -74,6 +74,8 @@ public final class Constants {
// .withWidget(BuiltInWidgets.kNumberBar).getEntry();
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);
@@ -100,7 +102,7 @@ public final class Constants {
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
*/
public static double INDEXER_AND_RAMP_MOTOR_RPM = 30000;
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
@@ -118,7 +120,9 @@ public final class Constants {
* programmingTab.add("Desired Intake RPM", -1000)
* .withWidget(BuiltInWidgets.kNumberBar).getEntry();
*/
public static double INTAKE_WHEELS_MOTOR_RPM = -6250;
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() {
@@ -130,36 +134,36 @@ public final class Constants {
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
public static class IntakeRotatorPID {
public static final double INTAKE_ROTATOR_P = .08;
public static final double INTAKE_ROTATOR_P = .07;
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0.01;
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 = 1.4;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3;
public static final double INTAKE_RETRACT_ENCODER_VALUE = -5.5;
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 = .1;
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
public static class TargetingConstants {
public static final Pose2d BLUE_LEFT_CLIMB_POSE = new Pose2d(1.066, 4.633, Rotation2d.fromDegrees(90));
public static final Pose2d BLUE_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 5.133,
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 Pose2d BLUE_RIGHT_CLIMB_POSE = new Pose2d(1.066, 2.87, Rotation2d.fromDegrees(-90));
public static final Pose2d BLUE_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 2.37,
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));
@@ -198,12 +202,12 @@ public final class Constants {
new Rotation3d(0, 0, Math.toRadians(-200)));
public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d(
new Translation3d(Units.inchesToMeters(23.375), 0, Units.inchesToMeters(13)),
new Rotation3d(0, 74, 0));
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(0, 0, 0),
new Rotation3d(0, 0, 0));
new Translation3d(Units.inchesToMeters(1.5), Units.inchesToMeters(-11.25), Units.inchesToMeters(26)),
new Rotation3d(0, Math.toRadians(-10), 0));
}
public static class ClimberConstants {
@@ -215,7 +219,7 @@ public final class Constants {
public static final double CLIMBER_PID_I = 0;
public static final double CLIMBER_PID_D = 0;
public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 45;
public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 65;
public static final double CLIMBER_LOWERED_SETPOINT_VALUE = 0;
}

1947
src/main/java/frc/robot/LimelightHelpers.java
View File

File diff suppressed because it is too large Load Diff

9
src/main/java/frc/robot/Robot.java
View File

@@ -28,6 +28,7 @@ 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;
/**
@@ -44,10 +45,10 @@ public class Robot extends TimedRobot {
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();
@@ -72,6 +73,7 @@ public class Robot extends TimedRobot {
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
@@ -145,7 +147,7 @@ public class Robot extends TimedRobot {
// IntakeSubsystem.resetIntakeRotationEncoder();
m_robotContainer.setMotorBrake(true);
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_ClimberSubsystem.lowerRobotCommand();
//ShooterSubsystem.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM);
// Print the selected autonomous command upon autonomous init
System.out.println("Auto selected: " + m_autonomousCommand);
@@ -168,7 +170,8 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
m_ClimberSubsystem.lowerRobotCommand();
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

322
src/main/java/frc/robot/RobotContainer.java
View File

@@ -28,6 +28,7 @@ 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;
@@ -43,6 +44,7 @@ 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;
@@ -59,8 +61,8 @@ public class RobotContainer {
// 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);
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(),
@@ -71,8 +73,7 @@ public class RobotContainer {
private final SendableChooser<Command> autoChooser;
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
private final TargetingSubsystems m_TargetingSubsystems = new
TargetingSubsystems();
private final TargetingSubsystems m_TargetingSubsystems = new TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
/**
@@ -142,21 +143,37 @@ public class RobotContainer {
// 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()
.andThen(m_IntakeSubsystem.startIntakeMotorCommand()));
NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand());
NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand());
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());
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));
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));*/
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
@@ -173,132 +190,173 @@ public class RobotContainer {
}
/**
* 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);
/**
* 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());
driverXbox.rightBumper().whileTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
//driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly());
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.setShooterMotorsRPMCommand(-3400));
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.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.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(m_ShooterSubsystem.stopShooterCommand());
//.andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)));
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
// () -> -driverXbox.getLeftX()));
// 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));
// 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());
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());
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.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.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());
//topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))
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));
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
// topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))
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)));
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
// driverXbox.b().whileTrue(
// drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
// );
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());
}
}
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 to pass the autonomous command to the main {@link Robot} class.
@@ -323,8 +381,14 @@ public class RobotContainer {
CommandScheduler.getInstance().cancelAll();
}
public Command killAllCommand(){
return Commands.runOnce(()->killAll());
public Command killAllCommand() {
return Commands.runOnce(() -> killAll());
}
public Command setIdleShooterRPMCommand() {
return Commands.runOnce(() -> m_ShooterSubsystem
.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM))
.andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
}
}

10
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
View File

@@ -34,11 +34,18 @@ public class ClimberSubsystem extends SubsystemBase {
climberMotorPIDController = climberMotor.getClosedLoopController();
}
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 void lowerRobot() {
public static void lowerRobot() {
climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LOWERED_SETPOINT_VALUE, ControlType.kPosition);
}
@@ -62,5 +69,6 @@ public class ClimberSubsystem extends SubsystemBase {
@Override
public void periodic() {
SmartDashboard.putNumber("Climber Motor Encoder", climberMotor.getEncoder().getPosition());
SmartDashboard.putNumber("Climber motor power", climberMotor.get());
}
}

179
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -2,6 +2,8 @@ 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;
@@ -9,6 +11,7 @@ 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;
@@ -19,6 +22,13 @@ 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;
@@ -30,8 +40,17 @@ 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(5, 10);
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();
@@ -44,71 +63,71 @@ public class IntakeSubsystem extends SubsystemBase {
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);
intakeRotatorProfiledPIDController.setTolerance(0.05);
//intakeRotatorProfiledPIDController.setGoal(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT);
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
// 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
// Slot 1
.p(.05, ClosedLoopSlot.kSlot1)
.i(0, ClosedLoopSlot.kSlot1)
.d(.05, ClosedLoopSlot.kSlot1)
//Slot 2
.p(1,ClosedLoopSlot.kSlot2)
// Slot 2
.p(.8, ClosedLoopSlot.kSlot2)
.i(.001, ClosedLoopSlot.kSlot2)
.d(0.1, ClosedLoopSlot.kSlot2);
.d(0.3, ClosedLoopSlot.kSlot2);
intakeRotatorConfig.smartCurrentLimit(80);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorConfig.smartCurrentLimit(40);
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(-5);
intakeRotatorMotor.getEncoder().setPosition(-6);
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
}
/* public void goToPosition(double goalPosition) {
intakeRotatorProfiledPIDController.setGoal(goalPosition);
//double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition);
}
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)
{
public void rotateIntake(double speed) {
intakeRotatorMotor.set(speed);
}
public Command rotateIntakeCommand(double speed)
{
return runOnce(()->intakeRotatorMotor.set(speed));
public Command rotateIntakeCommand(double speed) {
return runOnce(() -> intakeRotatorMotor.set(speed));
}
public void startIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM,
public void startIntakeMotor(double speed) {
intakeWheelsMotorPIDController.setSetpoint(speed,
ControlType.kVelocity);
}
public void reverseIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1,
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST * -1,
ControlType.kVelocity);
}
@@ -116,8 +135,8 @@ public class IntakeSubsystem extends SubsystemBase {
intakeWheelsMotor.set(0);
}
public Command startIntakeMotorCommand() {
return runOnce(() -> startIntakeMotor());
public Command startIntakeMotorCommand(double speed) {
return runOnce(() -> startIntakeMotor(speed));
}
public Command reverseIntakeMotorCommand() {
@@ -128,54 +147,68 @@ public class IntakeSubsystem extends SubsystemBase {
return runOnce(() -> stopIntakeMotor());
}
/* 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 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 void deployIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.
INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0);
}
public Command deployIntakeCommand() {
return runOnce(() -> deployIntake());
}
public void retractIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.
INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1);
}
public Command retractIntakeCommand() {
return runOnce(() -> retractIntake());
}
public void assistFuelIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.
INTAKE_MIDDLE_ENCODER_VALUE,
ControlType.kPosition, ClosedLoopSlot.kSlot2);
}
public Command assistFuelIntakeCommand() {
return runOnce(() -> assistFuelIntake()).andThen(new
WaitCommand(1)).andThen(deployIntakeCommand())
.andThen(new WaitCommand(1));
}
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);
}
public Command retractIntakeCommand() {
return runOnce(() -> retractIntake());
}
public void assistFuelIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
ControlType.kPosition, ClosedLoopSlot.kSlot2);
}
public Command assistFuelIntakeCommand() {
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() {
//encoderValue = intakeRotatorEncoder.get();
//intakeRotatorMotor.setVoltage(intakeRotatorProfiledPIDController.calculate(encoderValue) * -12);
SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
SmartDashboard.putNumber("Intake Rotator Encoder Value", intakeRotatorEncoder.get());
SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
SmartDashboard.putNumber("Voltage Retract", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
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));
}
}

53
src/main/java/frc/robot/subsystems/LEDSubsystem.java
View File

@@ -31,13 +31,13 @@ public class LEDSubsystem extends SubsystemBase {
LEDPattern hubActiveBlinkPattern = hubActiveColor.blink(Second.of(0.5));
LEDPattern hubInactiveColor = LEDPattern.solid(Color.kRed);
LEDPattern hubInactiveBlinkPattern = hubInactiveColor.blink(Second.of(0.5));
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.5));
LEDPattern transitionBlinkPattern = transitionColor.blink(Second.of(0.2));
LEDPattern endGameColor = LEDPattern.solid(Color.kOrangeRed);
LEDPattern endGameBlinkPattern = endGameColor.blink(Second.of(0.2));
@@ -69,22 +69,47 @@ public class LEDSubsystem extends SubsystemBase {
allianceShiftPattern.applyTo(m_Buffer);
}
if (matchTime <= 140 && matchTime > 130 && !DriverStation.isAutonomous()) // transition
else
{
transitionBlinkPattern.applyTo(m_Left);
}
if (matchTime <= 140 && matchTime > 132) // transition
{
transitionBlinkPattern.applyTo(m_Left);
}
if (matchTime <= 130 && matchTime > 30 && !DriverStation.isAutonomous()) // shifts
{
allianceShiftPattern.applyTo(m_Left);
}
if(matchTime <= 132 && matchTime > 130)
{
hubInactiveBlinkPattern.applyTo(m_Buffer);
}
if (matchTime <= 30 && !DriverStation.isAutonomous()) // endgame
{
endGameBlinkPattern.applyTo(m_Left);
}
if(matchTime <= 107 && matchTime > 105 && isHubActive == false)
{
hubInactiveBlinkPattern.applyTo(m_Buffer);
}
allianceShiftPattern.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() {

46
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View File

@@ -26,7 +26,6 @@ import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkBaseConfig;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import frc.robot.LimelightHelpers;
public class ShooterSubsystem extends SubsystemBase {
private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
@@ -57,6 +56,7 @@ public class ShooterSubsystem extends SubsystemBase {
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)
@@ -64,7 +64,8 @@ public class ShooterSubsystem extends SubsystemBase {
.feedForward
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
centerShooterMotorConfig.smartCurrentLimit(80);
centerShooterMotorConfig.smartCurrentLimit(45);
centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
@@ -72,6 +73,7 @@ public class ShooterSubsystem extends SubsystemBase {
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)
@@ -79,7 +81,7 @@ public class ShooterSubsystem extends SubsystemBase {
.feedForward
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
leftShooterMotorConfig.smartCurrentLimit(80);
leftShooterMotorConfig.smartCurrentLimit(45);
leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController();
@@ -87,6 +89,7 @@ public class ShooterSubsystem extends SubsystemBase {
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)
@@ -94,7 +97,7 @@ public class ShooterSubsystem extends SubsystemBase {
.feedForward
.kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
.kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
rightShooterMotorConfig.smartCurrentLimit(80);
rightShooterMotorConfig.smartCurrentLimit(45);
rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
@@ -102,6 +105,7 @@ public class ShooterSubsystem extends SubsystemBase {
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();
@@ -121,21 +125,25 @@ public class ShooterSubsystem extends SubsystemBase {
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
}
public void setShooterMotorsRPMAuto(){
centerShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(-2700, 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(()-> setShooterMotorsRPMAuto()).andThen(new WaitCommand(1.5))
return runOnce(()-> setShooterMotorsRPM(Constants.ShooterConstants.AUTO_SHOOTER_RPM)).andThen(new WaitCommand(1))
.andThen(() -> setIndexerAndRampMotorRPM());
}
// test individual motor code
public void setLeftShooterMotorRPM() {
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
}
public Command testLeftShooterCommand() {
@@ -143,7 +151,7 @@ public class ShooterSubsystem extends SubsystemBase {
}
public void setRightShooterMotorRPM() {
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
}
public Command testRightShooterCommand() {
@@ -151,7 +159,7 @@ public class ShooterSubsystem extends SubsystemBase {
}
public void setCenterShooterMotorRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
centerShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
}
public Command testCenterShooterCommand() {
@@ -191,6 +199,7 @@ public class ShooterSubsystem extends SubsystemBase {
ControlType.kVelocity);
}
public void reverseIndexerAndRampMotorRPM() {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1,
ControlType.kVelocity);
@@ -218,7 +227,7 @@ public class ShooterSubsystem extends SubsystemBase {
public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1.5))
return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1))
.andThen(() -> setIndexerAndRampMotorRPM());
};
@@ -244,6 +253,17 @@ public class ShooterSubsystem extends SubsystemBase {
return runOnce(() -> stopShooters());
}
public void reverseShooter()
{
centerShooterMotor.set(.4);
leftShooterMotor.set(0.4);
rightShooterMotor.set(0.4);
}
public Command reverseShooterCommand()
{
return runOnce(()-> reverseShooter());
}
@Override
public void periodic() {

25
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
View File

@@ -35,7 +35,6 @@ 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;
@@ -53,7 +52,7 @@ import frc.robot.Constants;
public class TargetingSubsystems extends SubsystemBase {
PIDController photonAimPIDController = new PIDController(3, 0.01, 0);
PIDController photonAimPIDController = new PIDController(5, 0.01, 0);
public static Rotation2d hubThetaPose = new Rotation2d();
public static Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -138,8 +137,8 @@ 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);
}
@@ -168,12 +167,10 @@ public class TargetingSubsystems extends SubsystemBase {
public static void updateShooterRPM(Pose2d currentRobotPose) {
double distance = PhotonUtils.getDistanceToPose(currentRobotPose,
Constants.TargetingConstants.allianceHubPose);
Constants.ShooterConstants.SHOOTER_RPM =
(-293.84123 * Math.pow(distance, 3))
Constants.ShooterConstants.SHOOTER_RPM = Math.max((-293.84123 * Math.pow(distance, 3))
+ (1360.01497 * Math.pow(distance, 2))
- (2391.17127 * distance)
- 1249.22704;
- 1249.22704, -6000);
}
@@ -185,17 +182,5 @@ public class TargetingSubsystems extends SubsystemBase {
SmartDashboard.putString("Hub Pose", "x: " + Constants.TargetingConstants.allianceHubPose.getMeasureX() + " y: " + Constants.TargetingConstants.allianceHubPose.getY()
+ " angle: " + Constants.TargetingConstants.allianceHubPose.getRotation());
/*
* 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"));
*/
}
}

2
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View File

@@ -124,7 +124,7 @@ public class SwerveSubsystem extends SubsystemBase {
setupPhotonVision();
// Stop the odometry thread if we are using vision that way we can synchronize
// updates better.
//swerveDrive.stopOdometryThread();
swerveDrive.stopOdometryThread();
}
setupPathPlanner();
SmartDashboard.putData("Rebuilt Field", rebuiltField);