FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Compare commits

base: FairportRobotics:main
Branches Tags
FairportRobotics:main
FairportRobotics:ultrashooter
FairportRobotics:magicbot
FairportRobotics:turret-auto-tracking
FairportRobotics:Multicontroller
FairportRobotics:FRCCompIntakeMan
FairportRobotics:FRCComp
FairportRobotics:swerve-fix
FairportRobotics:CommandsSubsystems
FairportRobotics:Speed-tuning
FairportRobotics:python
FairportRobotics:turret
FairportRobotics:MIAMI-VALLEY-POST
FairportRobotics:MIAMI-VALLEY-PRE
FairportRobotics:FLR-FINAL
FairportRobotics:PRE-FLR
..
compare: FairportRobotics:MIAMI-VALLEY-PRE
Branches Tags
FairportRobotics:ultrashooter
FairportRobotics:main
FairportRobotics:magicbot
FairportRobotics:turret-auto-tracking
FairportRobotics:Multicontroller
FairportRobotics:FRCCompIntakeMan
FairportRobotics:FRCComp
FairportRobotics:swerve-fix
FairportRobotics:CommandsSubsystems
FairportRobotics:Speed-tuning
FairportRobotics:python
FairportRobotics:turret
FairportRobotics:MIAMI-VALLEY-POST
FairportRobotics:MIAMI-VALLEY-PRE
FairportRobotics:FLR-FINAL
FairportRobotics:PRE-FLR

No commits in common. "main" and "MIAMI-VALLEY-PRE" have entirely different histories.
main ... MIAMI-VALL

41 changed files with 518 additions and 1885 deletions
Show Stats Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit c0478b01a526712b8308ac329017857177a3c876 Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12

22
simgui-ds.json
View File

@ -1,9 +1,4 @@
{ {
"Keyboard 0 Settings": {
"window": {
"visible": true
}
},
"keyboardJoysticks": [ "keyboardJoysticks": [
{ {
"axisConfig": [ "axisConfig": [
@ -12,20 +7,17 @@
"incKey": 68 "incKey": 68
}, },
{ {
"decKey": 83, "decKey": 87,
"incKey": 87 "incKey": 83
}, },
{ {
"decayRate": 0.009999999776482582, "decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582 "keyRate": 0.009999999776482582
},
{},
{
"decKey": 81,
"incKey": 69
} }
], ],
"axisCount": 5, "axisCount": 3,
"buttonCount": 4, "buttonCount": 4,
"buttonKeys": [ "buttonKeys": [
90, 90,
@ -99,7 +91,7 @@
], ],
"robotJoysticks": [ "robotJoysticks": [
{ {
"guid": "Keyboard0" "guid": "030000004c050000cc09000011810000"
} }
] ]
} }

88
src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto
View File

@ -1,88 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "1.1 Hub to Shoot"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "ChassisAlignShoot"
}
},
{
"type": "wait",
"data": {
"waitTime": 10.0
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "1.2 Shoot to Depot Path"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "RunIntake"
}
},
{
"type": "wait",
"data": {
"waitTime": 10.0
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "1.3 Depot to Shoot Path"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "ChassisAlignShoot"
}
},
{
"type": "wait",
"data": {
"waitTime": 10.0
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

25
src/main/deploy/pathplanner/autos/9. Chaos Monkey.auto
View File

@ -1,25 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "9.1 Chaos Monkey Path"
}
},
{
"type": "named",
"data": {
"name": "Shoot preload"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

19
src/main/deploy/pathplanner/autos/HUB_START.auto
View File

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

19
src/main/deploy/pathplanner/autos/LEFT_TRENCH_START.auto
View File

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

19
src/main/deploy/pathplanner/autos/RIGHT_TRENCH_START.auto
View File

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

19
src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto
View File

@ -9,25 +9,6 @@
"data": { "data": {
"pathName": "S. Hub. shoot 8. E-Hub" "pathName": "S. Hub. shoot 8. E-Hub"
} }
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 10.0
}
},
{
"type": "named",
"data": {
"name": "TurretShoot"
}
}
]
}
} }
] ]
} }

58
src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
View File

@ -1,58 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 4.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.0,
"y": 4.0
},
"prevControl": {
"x": 2.5,
"y": 4.0
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -177.9047474356738
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
<<<<<<< HEAD:src/main/deploy/pathplanner/paths/Hub to Depot.path
"rotation": -179.99999999992184
=======
"rotation": -179.69626968995792
>>>>>>> c2fa27aa5cc22124b930645e802b8176658edb7a:src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
},
"useDefaultConstraints": false
}

54
src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 2.75,
"y": 4.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.0,
"y": 4.0
},
"prevControl": {
"x": 2.75,
"y": 4.0
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 177.4888349270938
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -179.0393266954795
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.0,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 2.2144854970378125,
"y": 3.8554998234204434
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.0,
"y": 6.0
},
"prevControl": {
"x": 1.2166471375173105,
"y": 5.875244167248359
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -178.09121133641756
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.4910281334858283
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/1.3 Depot to Shoot Path.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.0,
"y": 6.0
},
"prevControl": null,
"nextControl": {
"x": 1.453699105174549,
"y": 6.084627986890304
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.0,
"y": 4.0
},
"prevControl": {
"x": 2.266577414275921,
"y": 3.829411518660614
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 1.5,
"maxAngularVelocity": 100.0,
"maxAngularAcceleration": 100.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -179.02897806892076
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -179.69626968995792
},
"useDefaultConstraints": false
}

102
src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path
View File

@ -1,102 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5184925161825644,
"y": 7.5345006466771025
},
"prevControl": null,
"nextControl": {
"x": 4.489963675146704,
"y": 7.219438447522688
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.6,
"y": 6.812200117368956
},
"prevControl": {
"x": 7.532870181778978,
"y": 7.8099443669167
},
"nextControl": {
"x": 7.650893142287796,
"y": 6.055780073270545
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.6,
"y": 0.9617601315610045
},
"prevControl": {
"x": 7.654625343431328,
"y": 1.468826273060325
},
"nextControl": {
"x": 7.527819365080411,
"y": 0.2917349428743933
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.5,
"y": 0.6692650602433468
},
"prevControl": {
"x": 1.096874747061515,
"y": 0.6440356562159093
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 1.2332261810856255,
"rotationDegrees": -90.0
},
{
"waypointRelativePos": 1.7516108262639798,
"rotationDegrees": 0.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "",
"waypointRelativePos": 0,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -1.2282908533168928
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -0.1533900770684255
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/HUB_START.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.519556041182133,
"y": 4.059804750409042
},
"prevControl": null,
"nextControl": {
"x": 3.769556041182133,
"y": 4.059804750409042
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.519556041182133,
"y": 4.059804750409042
},
"prevControl": {
"x": 3.269556041182133,
"y": 4.059804750409042
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.0,
"y": 7.423545951843262
},
"prevControl": null,
"nextControl": {
"x": 4.25,
"y": 7.423545951843262
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 7.423545951843262
},
"prevControl": {
"x": 3.75,
"y": 7.423545951843262
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

4
src/main/deploy/pathplanner/paths/M. Depot. .path
View File

@ -36,7 +36,7 @@
"y": 7.138469879518072 "y": 7.138469879518072
}, },
"prevControl": { "prevControl": {
"x": 0.41241207330734936, "x": 0.4124120733073493,
"y": 6.876706525398199 "y": 6.876706525398199
}, },
"nextControl": { "nextControl": {
@ -88,7 +88,7 @@
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false

54
src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.0,
"y": 0.6236959156237152
},
"prevControl": null,
"nextControl": {
"x": 4.25,
"y": 0.6236959156237152
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 0.6236959156237152
},
"prevControl": {
"x": 3.75,
"y": 0.6236959156237152
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

10
src/main/deploy/pathplanner/paths/Relay. L-Tr..path
View File

@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.5383012048192777, "x": 3.5055180722891564,
"y": 7.4553734939759035 "y": 7.466301204819277
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 4.538301204819281, "x": 4.505518072289158,
"y": 7.4553734939759035 "y": 7.466301204819277
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@ -174,7 +174,7 @@
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false

10
src/main/deploy/pathplanner/paths/Relay. R-Tr..path
View File

@ -37,7 +37,7 @@
}, },
"prevControl": { "prevControl": {
"x": 7.691609840125085, "x": 7.691609840125085,
"y": 7.387443181134999 "y": 7.387443181134998
}, },
"nextControl": { "nextControl": {
"x": 7.64634196710383, "x": 7.64634196710383,
@ -56,7 +56,7 @@
"y": 7.138469879518072 "y": 7.138469879518072
}, },
"nextControl": { "nextControl": {
"x": 6.511975903614458, "x": 6.511975903614461,
"y": 7.138469879518072 "y": 7.138469879518072
}, },
"isLocked": false, "isLocked": false,
@ -85,11 +85,11 @@
}, },
"prevControl": { "prevControl": {
"x": 6.020824888067715, "x": 6.020824888067715,
"y": 0.8531028227756465 "y": 0.8531028227756468
}, },
"nextControl": { "nextControl": {
"x": 5.667271497474452, "x": 5.667271497474452,
"y": 1.2066562133689316 "y": 1.2066562133689314
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@ -178,7 +178,7 @@
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false

10
src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path
View File

@ -32,12 +32,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 2.677383349903843, "x": 3.473,
"y": 4.00221686746988 "y": 4.013
}, },
"prevControl": { "prevControl": {
"x": 2.4274139067638107, "x": 3.1233132530120478,
"y": 4.006125511257969 "y": 4.034855421686746
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -68,7 +68,7 @@
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false

23
src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path
View File

@ -16,27 +16,27 @@
}, },
{ {
"anchor": { "anchor": {
"x": 2.1723373493975906, "x": 2.3581084337349396,
"y": 4.00221686746988 "y": 4.00221686746988
}, },
"prevControl": { "prevControl": {
"x": 1.8772891566265066, "x": 1.3581084337349396,
"y": 4.024072289156626 "y": 4.00221686746988
}, },
"nextControl": { "nextControl": {
"x": 2.421654288011768, "x": 3.3581084337349396,
"y": 3.9837489460910525 "y": 4.00221686746988
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 2.1723373493975906, "x": 3.48366265060241,
"y": 4.00221686746988 "y": 4.00221686746988
}, },
"prevControl": { "prevControl": {
"x": 1.8226506024096385, "x": 3.133975903614458,
"y": 3.9912891566265065 "y": 3.9912891566265065
}, },
"nextControl": null, "nextControl": null,
@ -44,7 +44,12 @@
"linkedName": null "linkedName": null
} }
], ],
"rotationTargets": [], "rotationTargets": [
{
"waypointRelativePos": 0.936692913385826,
"rotationDegrees": 180.0
}
],
"constraintZones": [], "constraintZones": [],
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [ "eventMarkers": [
@ -63,7 +68,7 @@
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false

4
src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path
View File

@ -24,7 +24,7 @@
"y": 3.77441480327739 "y": 3.77441480327739
}, },
"nextControl": { "nextControl": {
"x": 2.3487442810046395, "x": 2.348744281004639,
"y": 4.0114647147949 "y": 4.0114647147949
}, },
"isLocked": false, "isLocked": false,
@ -68,7 +68,7 @@
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false

2
src/main/deploy/pathplanner/paths/Spin.path
View File

@ -72,7 +72,7 @@
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false

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

@ -6,7 +6,7 @@
"autoFolders": [], "autoFolders": [],
"defaultMaxVel": 3.0, "defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0, "defaultMaxAccel": 3.0,
"defaultMaxAngVel": 360.0, "defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0, "defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0, "defaultNominalVoltage": 12.0,
"robotMass": 44.4521, "robotMass": 44.4521,

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

@ -1,220 +1,124 @@
// Copyright (c) FIRST and other WPILib contributors. // Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of // 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. // the WPILib BSD license file in the root directory of this project.
package frc.robot; package frc.robot;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.InvertedValue;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.units.measure.Angle;
/** /**
* The Constants class provides a convenient place for teams to hold robot-wide * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* numerical or boolean constants. This class should not be used for any other * constants. This class should not be used for any other purpose. All constants should be declared
* purpose. All constants should be declared globally (i.e. public static). Do * globally (i.e. public static). Do not put anything functional in this class.
* not put anything functional in this class.
* *
* <p> * <p>It is advised to statically import this class (or one of its inner classes) wherever the
* It is advised to statically import this class (or one of its inner classes) * constants are needed, to reduce verbosity.
* wherever the constants are needed, to reduce verbosity.
*/ */
public final class Constants { public final class Constants {
public static class OperatorConstants {
public static final int kDriverControllerPort = 0;
}
public static class OperatorConstants { public static class CameraConstanst {
public static final int kDriverControllerPort = 0; public static class IDFilters {
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 };
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 };
} }
public static class CameraConstanst { public static final String BACK_CAMERA_NAME = "limelight-back";
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
public static class IDFilters { public static final String FRONT_CAMERA_NAME = "limelight-front";
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11}; }
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
}
public static final String BACK_CAMERA_NAME = "limelight-back";
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
public static final String FRONT_CAMERA_NAME = "limelight-front";
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
public static final double[] BLUE_ZONE_COOR = new double[]{0, 3.5};
public static final double[] RED_ZONE_COOR = new double[]{13, 16};
public static final double[] NEUTRAL_ZONE_COOR = new double[]{5.5, 11};
public static final double[] NEUTRAL1_ZONE_COOR = new double[]{5.5, 11, 0, 3};
public static final double[] NEUTRAL2_ZONE_COOR = new double[]{5.5, 11, 5, 8};
public static final double[] BLUE1_ZONE_COOR = new double[]{0, 3.5, 0, 3};
public static final double[] BLUE2_ZONE_COOR = new double[]{0, 3.5, 5, 8};
public static final double[] RED1_ZONE_COOR = new double[]{13, 16, 0, 3};
public static final double[] RED2_ZONE_COOR = new double[]{13, 16, 5, 8};
public static class AdvantageKitConstants{
public enum RobotType{
SIM,
REAL,
REPLAY
} }
public static final RobotType CURRENT_MODE = RobotType.REAL;
public static class AdvantageKitConstants { }
// Swerve offsets as of 3/2/26
public enum RobotType { public static class SwerveDriveOffsets {
SIM, public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
REAL, public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
REPLAY public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
} public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
public static final RobotType CURRENT_MODE = RobotType.REAL; }
} /*
// Swerve offsets as of 3/2/26
public static class SwerveDriveOffsets {
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
}
/*
* Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
* Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
* Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot * Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot
* Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot * Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot
* Laters numbers will be added for more subsystems * Laters numbers will be added for more subsystems
* Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
*/ */
public static class SwerveDriveIDs { public static class SwerveDriveIDs {
public static final int FRONT_LEFT_STEER_ID = 11;
public static final int FRONT_LEFT_DRIVE_ID = 12;
public static final int FRONT_LEFT_ENCODER_ID = 13;
public static final int FRONT_RIGHT_STEER_ID = 21;
public static final int FRONT_RIGHT_DRIVE_ID = 22;
public static final int FRONT_RIGHT_ENCODER_ID = 23;
public static final int BACK_LEFT_STEER_ID = 31;
public static final int BACK_LEFT_DRIVE_ID = 32;
public static final int BACK_LEFT_ENCODER_ID = 33;
public static final int BACK_RIGHT_STEER_ID = 41;
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
//Bird
public static class ExtraIDEntities {
public static final int PIGEON_ID = 1;
}
public static final int FRONT_LEFT_STEER_ID = 11; public static class IntakeConstants {
public static final int FRONT_LEFT_DRIVE_ID = 12; public static final int DEPLOY_MOTOR_ID = 17;
public static final int FRONT_LEFT_ENCODER_ID = 13; public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
public static final int FRONT_RIGHT_STEER_ID = 21;
public static final int FRONT_RIGHT_DRIVE_ID = 22;
public static final int FRONT_RIGHT_ENCODER_ID = 23;
public static final int BACK_LEFT_STEER_ID = 31;
public static final int BACK_LEFT_DRIVE_ID = 32;
public static final int BACK_LEFT_ENCODER_ID = 33;
public static final int BACK_RIGHT_STEER_ID = 41;
public static final int BACK_RIGHT_DRIVE_ID = 42;
public static final int BACK_RIGHT_ENCODER_ID = 43;
}
public static class RobotChassisLimits { public static final int INTAKE_MOTOR_ID = 2;
// Max velocity in meters per second public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
}
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; public static class ShooterConstants {
// In meters per second squared public static final int LAUNCHER_MOTOR_ID = 3;
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; public static final boolean LAUNCHER_MOTOR_INVERTED = false;
// In radians per second public static final int TURRET_MOTOR_ID = 4;
public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
// In radians per second squared public static final int TURRET_LIMIT_CHANNEL = 9;
public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI; public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
} public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
public static final double HOMING_SPEED = .35;
public static final double TURRET_GEAR_RATIO = 6;
//Bird public static final int HOOD_SERVO_CHANNEL = 0;
public static class ExtraIDEntities { public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
public static final int HOOD_ENCODER_ID = 4;
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 300;
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
}
public static final int PIGEON_ID = 1; public static class HopperConstants {
} public static final int KICKER_MOTOR_ID = 9;
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
public static class IntakeConstants { public static final int SPINDEXER_MOTOR_ID = 8;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final int DEPLOY_MOTOR_ID = 17; }
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
public static final int INTAKE_MOTOR_ID = 2;
public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
}
public static class ShooterConstants {
public static final int LAUNCHER_MOTOR_ID = 3;
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
public static final int TURRET_MOTOR_ID = 4;
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
public static final int TURRET_LIMIT_CHANNEL = 9;
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
public static final double HOMING_SPEED = .05;
public static final double TURRET_GEAR_RATIO = 6;
public static final int HOOD_SERVO_CHANNEL = 0;
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
public static final int HOOD_ENCODER_ID = 4;
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
public static final double DEFLECTOR_STORED_ANGLE = 0;
public static final double DEFLECTOR_SET_ANGLE1 = 100;
public static final double DEFLECTOR_SET_ANGLE2 = 200;
public static final double DEFLECTOR_SET_ANGLE3 = 300;
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
}
public static class HopperConstants {
public static final int KICKER_MOTOR_ID = 9;
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
public static final int SPINDEXER_MOTOR_ID = 8;
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
}
public static class FieldPoses {
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
// These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side.
// Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
// Rotation are zero because they don't matter.
public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 1.72, 0, new Rotation3d());
public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d());
public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d());
public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
}
// These represent the dividing lines for the regions in our shooting strategy.
public static class ShootingRegionDimensions {
public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0;
public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5;
public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0;
public static double BLUE_RIGHT_REGION_Y_MAX = 3.0;
public static double RED_LEFT_REGION_Y_MAX = 3.0; // same meaning as the previous one, but from red perspective
public static double BLUE_LEFT_REGION_Y_MIN = 5.0;
public static double RED_RIGHT_REGION_Y_MIN = 5.0; // same meaning as the previous one, but from red perspective
}
// Regions that define the AutoTurret shooting behaviors.
// Color ID in the name reflects robot's alliance since region definitions vary by alliance.
// Left/Right are from driver's perspective.
public enum ShootingRegion {
NON_SHOOTING_REGION, // Any part of the field that's not in one of the regions
BLUE_OWN_ALLIANCE_ZONE,
BLUE_LEFT_NEUTRAL_ZONE,
BLUE_RIGHT_NEUTRAL_ZONE,
BLUE_LEFT_OPPONENT_ALLIANCE_ZONE,
BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE,
RED_OWN_ALLIANCE_ZONE,
RED_LEFT_NEUTRAL_ZONE,
RED_RIGHT_NEUTRAL_ZONE,
RED_LEFT_OPPONENT_ALLIANCE_ZONE,
RED_RIGHT_OPPONENT_ALLIANCE_ZONE
}
} }

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

@ -14,8 +14,6 @@ import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.littletonrobotics.junction.wpilog.WPILOGWriter;
import com.ctre.phoenix6.swerve.SwerveModule;
import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
@ -92,9 +90,6 @@ public class Robot extends LoggedRobot {
// Run all POST tests after robot has been initialized // Run all POST tests after robot has been initialized
PostyManager.getInstance().runAllPOSTs(); PostyManager.getInstance().runAllPOSTs();
m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(false);
} }
/** /**
@ -122,7 +117,6 @@ public class Robot extends LoggedRobot {
/** This function is called once each time the robot enters Disabled mode. */ /** This function is called once each time the robot enters Disabled mode. */
@Override @Override
public void disabledInit() { public void disabledInit() {
m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(true);
} }
@Override @Override

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

@ -1,11 +1,10 @@
// Copyright (c) FIRST and other WPILib contributors. // Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of // 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. // the WPILib BSD license file in the root directory of this project.
package frc.robot; package frc.robot;
import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@ -13,8 +12,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.OperatorConstants;
import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.AutoShootCommandChassisTurret;
import frc.robot.commands.AutoTurretCommand; import frc.robot.commands.AutoTurretCommand;
import frc.robot.commands.AutoTurretShootCommand; import frc.robot.commands.ManualHopperCommand;
import frc.robot.commands.Reverse; import frc.robot.commands.ManualIntake;
import frc.robot.commands.ManualShootCommand;
import frc.robot.commands.SetDeflectorCommand; import frc.robot.commands.SetDeflectorCommand;
import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.HopperSubsystem;
@ -22,82 +22,64 @@ import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.TurretSubsystem;
/** /**
* This class is where the bulk of the robot should be declared. Since * This class is where the bulk of the robot should be declared. Since Command-based is a
* Command-based is a "declarative" paradigm, very little robot logic should * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
* actually be handled in the {@link Robot} periodic methods (other than the * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* scheduler calls). Instead, the structure of the robot (including subsystems, * subsystems, commands, and trigger mappings) should be declared here.
* commands, and trigger mappings) should be declared here.
*/ */
public class RobotContainer { public class RobotContainer {
// The robot's subsystems and commands are defined here... // The robot's subsystems and commands are defined here...
// Replace with CommandPS4Controller or CommandJoystick if needed // Replace with CommandPS4Controller or CommandJoystick if needed
private final CommandXboxController m_driverController =
new CommandXboxController(OperatorConstants.kDriverControllerPort);
private final CommandXboxController m_driverController // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
= new CommandXboxController(OperatorConstants.kDriverControllerPort); public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort); public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); public final TurretSubsystem turretSubsystem = new TurretSubsystem();
public final TurretSubsystem turretSubsystem = new TurretSubsystem(); public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
public final HopperSubsystem hopperSubsystem = new HopperSubsystem(); /** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
/** turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
* The container for the robot. Contains subsystems, OI devices, and // turretSubsystem.homeTurret();
* commands.
*/
public RobotContainer() {
// turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); // Configure the trigger bindings
// turretSubsystem.homeTurret(); configureBindings();
// Configure the trigger bindings }
configureBindings();
NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); /**
NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake(m_driverController.getHID())); * Use this method to define your trigger->command mappings. Triggers can be created via the
NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)); * {@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}.
* */
turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); private void configureBindings() {
} m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
/** m_driverController.rightBumper().onTrue(intakeSubsystem.intake());
* Use this method to define your trigger->command mappings. Triggers can be m_driverController.y().onTrue(intakeSubsystem.deploy());
* created via the m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
* {@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() {
// m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
// m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
// m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
// m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
//m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); /**
m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); * Use this to pass the autonomous command to the main {@link Robot} class.
m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID()))); *
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); * @return the command to run in autonomous
m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem))); */
m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem))); public Command getAutonomousCommand() {
m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID())); // An example command will be run in autonomous
m_driverController.y().onTrue(intakeSubsystem.deploy()); return Commands.parallel(Commands.run(()-> {/*turretSubsystem.homeTurret();*/}, turretSubsystem), driveSubsystem.getAutoCommand());
} }
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An example command will be run in autonomous
return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> {
turretSubsystem.homeTurret();
}, turretSubsystem), driveSubsystem.getAutoCommand());
}
} }

173
src/main/java/frc/robot/Utils.java
View File

@ -1,173 +0,0 @@
package frc.robot;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.Constants;
import frc.robot.Constants.ShootingRegion;
import frc.robot.Constants.ShootingRegionDimensions;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.units.measure.Angle;
public class Utils {
/**
* Calculates the needed angle of the turret for a distance from the HUB.
* @param distanceToTag
* @return number of rotations of the turret, robot relative
*/
public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
// Calculate turret Angle
double cameraDistanceToTarget = distanceToTag + .6;
double cameraToShootDistance = 0.26035;
// a^2 + b^2 = c^2
double shooterDistanceToTarget = Math
.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
double robotTurretRotations = (robotTurretAngle / Math.PI);
return robotTurretRotations;
}
/**
* Determines the region for the robot based on its location and alliance.
* Assumes the robot is a 27" square. Thresholds based on observations from simulator.
* @param robotPose: the robot's pose
* @param alliance: the red/blue alliance of the robot, as a sanity check
* @return the robot's shooting region
*/
public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION; // the default return value if robot isn't in a valid region
double robotX = robotPose.getX();
double robotY = robotPose.getY();
// Receommended: Add a sanity check for invalid robot poses.
if (alliance == Alliance.Blue) {
if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
}
}
else if (alliance == Alliance.Red) {
if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
}
else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
}
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
}
}
Logger.recordOutput("Utils-ShootingRegion", region);
return region;
}
public static double getHoodAngleForDistance(double distanceInMeters){
if(distanceInMeters >= 4){
return 200;
}
return 0;
}
public static double getLauncherRPMForDistance(double distanceInMeters){
return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
}
/**
* Determines the appropriate shooting target for the robot based on its alliance
* and its current region on the field. There are 10 possible targets:
* Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
* and these exist for both red and blue alliances.
* Right/left are from driver's perspective.
* @param region: the identifier of the robot's current field region
* @param alliance: the red/blue alliance of the robot, as a sanity check
* @return the shooting target for a robot with the provided properties. Returns all-zero
* Pose3d object if the region/alliance combination is invalid.
*/
public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
Pose3d shootingTarget = new Pose3d();
if (alliance == Alliance.Blue) {
switch (region) {
case BLUE_OWN_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
break;
case BLUE_LEFT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case BLUE_RIGHT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
default:
break;
}
}
else if (alliance == Alliance.Red) {
switch (region) {
case RED_OWN_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
break;
case RED_LEFT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case RED_RIGHT_NEUTRAL_ZONE:
shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
break;
case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
break;
default:
break;
}
}
return shootingTarget;
}
}

21
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants; import frc.robot.Constants;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.TurretSubsystem;
@ -55,8 +54,8 @@ public class AutoShootCommandChassisTurret extends Command{
this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0); cameraAutoCenterController = new PIDController(0.1, 0, 0.0);
cameraAutoCenterController.setTolerance(0.2); // Units degrees cameraAutoCenterController.setTolerance(0.2); // Units is meters
cameraAutoCenterController.setSetpoint(0); cameraAutoCenterController.setSetpoint(0);
@ -75,13 +74,12 @@ public class AutoShootCommandChassisTurret extends Command{
} }
}); });
turretSubsystem.setLauncher(3000); turretSubsystem.setLauncher(5500);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
hopperSubsystem.spindexerOn(); hopperSubsystem.spindexerOn();
cameraAutoCenterController.calculate(-100); cameraAutoCenterController.calculate(-100);
// Change to HUB tracking pipeline // Change to HUB tracking pipeline
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER); LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER);
LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters); LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
Pose3d botPose = driveSubsystem.getBotPose(); Pose3d botPose = driveSubsystem.getBotPose();
@ -93,7 +91,8 @@ public class AutoShootCommandChassisTurret extends Command{
Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians()); // Add 180 because we want the back of the robot to face the HUB
deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
} }
@Override @Override
@ -107,7 +106,7 @@ public class AutoShootCommandChassisTurret extends Command{
Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
Pose3d botPose = driveSubsystem.getBotPose(); Pose3d botPose = driveSubsystem.getBotPose();
//driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
} }
else // We have an april tag, center it to the camera frame else // We have an april tag, center it to the camera frame
{ {
@ -115,12 +114,6 @@ public class AutoShootCommandChassisTurret extends Command{
// [tx, ty, tz, pitch, yaw, roll] // [tx, ty, tz, pitch, yaw, roll]
double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(targetsPose.length >= 1){ if(targetsPose.length >= 1){
Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
// Calculate turret Angle
double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
turretSubsystem.setTurretMotorRotation(-turretAngle);
driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
} }
} }

130
src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java
View File

@ -1,130 +0,0 @@
package frc.robot.commands;
import java.util.Arrays;
import java.util.List;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathConstraints;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.Constants;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class AutoShootCommandChassisTurretPathPlanner extends Command {
// For Chassis Control
private DriveSubsystem driveSubsystem;
private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem;
private int[] tagFilters;
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Pose3d closestTag;
private Pose2d targetPPPose;
private Command pathFollowCommand;
public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem,
TurretSubsystem turretSubsystem) {
this.driveSubsystem = driveSubsystem;
this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem;
this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
}
@Override
public void initialize() {
DriverStation.getAlliance().ifPresent((aliance) -> {
Pose3d targetHub;
if (aliance == Alliance.Blue) {
tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
targetHub = Constants.FieldPoses.BLUE_HUB_POSE;
} else {
tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
targetHub = Constants.FieldPoses.RED_HUB_POSE;
}
Pose3d botPose = driveSubsystem.getBotPose();
// Find closest target to us
List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get())
.toList();
closestTag = botPose.nearest(tagPoses);
Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag);
Translation2d delta = botPose.getTranslation().toTranslation2d()
.minus(closestTag.getTranslation().toTranslation2d());
Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta);
// Add 180 because we want the back of the robot to face the HUB
// deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
// Translate back 1 meter from HUB
Translation2d newTrans = targetHub.getTranslation().toTranslation2d()
.plus(new Translation2d(1.5, delta.getAngle()));
targetPPPose = new Pose2d(newTrans, delta.getAngle());
Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose);
pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose,
new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY,
Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL,
Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY,
Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL));
CommandScheduler.getInstance().schedule(pathFollowCommand);
});
turretSubsystem.setLauncher(5500);
hopperSubsystem.spindexerOn();
}
@Override
public void execute() {
// If robot is centered on HUB and shooter is up to speed. Run the kicker
if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) {
Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
hopperSubsystem.feedKicker();
} else {
Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
CommandScheduler.getInstance().cancel(pathFollowCommand);
turretSubsystem.setLauncher(0);
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
driveSubsystem.stopDrive();
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME,
Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
}
}

42
src/main/java/frc/robot/commands/AutoTurretCommand.java
View File

@ -17,8 +17,6 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants; import frc.robot.Constants;
import frc.robot.Constants.ShootingRegion;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.TurretSubsystem;
@ -29,7 +27,12 @@ public class AutoTurretCommand extends Command {
private TurretSubsystem turretSubsystem; private TurretSubsystem turretSubsystem;
private DriveSubsystem driveSubsystem; private DriveSubsystem driveSubsystem;
private Alliance alliance; private int[] shootingTagFilters;
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
private Pose3d closestTag;
private Pose3d scoringHubPose;
public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) { public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
this.turretSubsystem = turretSubsystem; this.turretSubsystem = turretSubsystem;
@ -45,12 +48,17 @@ public class AutoTurretCommand extends Command {
DriverStation.getAlliance().ifPresent((aliance) -> { DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) { if (aliance == Alliance.Blue) {
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE); shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
aliance = Alliance.Blue;
scoringHubPose = fieldTags.getTagPose(26).get();
scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
} else { } else {
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE); shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
aliance = Alliance.Red;
scoringHubPose = fieldTags.getTagPose(10).get();
scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
} }
Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
}); });
} }
@ -58,28 +66,18 @@ public class AutoTurretCommand extends Command {
@Override @Override
public void execute() { public void execute() {
ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), DriverStation.getAlliance().get());
if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){
Pose3d targetPose = Utils.findShootingTarget(shootingRegion, DriverStation.getAlliance().get());
turretSubsystem.setTurretTargetPose(targetPose);
}
Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
Pose3d botPose = driveSubsystem.getBotPose(); Pose3d botPose = driveSubsystem.getBotPose();
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5)))); new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngle().in(Units.Rotations)))));
Logger.recordOutput("AutoTurret-TurretPose", turretPose); Logger.recordOutput("AutoTurret-TurretPose", turretPose);
Translation2d targetTranslation = turretSubsystem.getTurretTargetPose().getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurret-Translation", targetTranslation); Logger.recordOutput("AutoTurret-Translation", targetTranslation);
if(turretSubsystem.isTurretReady()){ if(turretSubsystem.isTurretReady()){
turretSubsystem.setTurretFieldRelative(botPose, targetTranslation.getAngle().getRotations()); turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretRobotRelative(0.25); // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
// turretSubsystem.setTurretMotorRotation(0);
} }
} }

90
src/main/java/frc/robot/commands/AutoTurretShootCommand.java
View File

@ -1,90 +0,0 @@
package frc.robot.commands;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
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 frc.robot.Constants;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.TurretSubsystem;
public class AutoTurretShootCommand extends Command{
private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem;
private IntakeSubsystem intakeSubsystem;
private DriveSubsystem driveSubsystem;
private Command waitCommand = Commands.waitSeconds(1.5);
private Alliance alliance;
public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
this.hopperSubsystem = hopperSubsystem;
this.turretSubsystem = turretSubsystem;
this.intakeSubsystem = intakeSubsystem;
this.driveSubsystem = driveSubsystem;
addRequirements(hopperSubsystem, intakeSubsystem);
}
@Override
public void initialize() {
hopperSubsystem.spindexerOn();
CommandScheduler.getInstance().schedule(waitCommand);
alliance = DriverStation.getAlliance().get();
Logger.recordOutput("AutoTurretShoot-State", "SHOOTING");
}
@Override
public void execute() {
double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance);
turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(distance));
turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(distance));
if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
hopperSubsystem.feedKicker();
}else{
hopperSubsystem.stopKicker();
}
}
@Override
public boolean isFinished() {
return false;
}
@Override
public void end(boolean interrupted) {
waitCommand.cancel();
hopperSubsystem.spindexerOff();
hopperSubsystem.stopKicker();
turretSubsystem.setLauncher(0);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
Logger.recordOutput("AutoTurretShoot-State", "NOT SHOOTING");
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
} else {
turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
}
});
// turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}

58
src/main/java/frc/robot/commands/ManualHopperCommand.java Normal file
View File

@ -0,0 +1,58 @@
// 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.commands;
import frc.robot.subsystems.HopperSubsystem;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. */
public class ManualHopperCommand extends Command {
private final HopperSubsystem m_subsystem;
private final XboxController m_xboxController;
public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) {
m_subsystem = subsystem;
m_xboxController = xboxController;
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_subsystem.spindexerOn();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
{
m_subsystem.feedKicker();
}
else
{
m_subsystem.stopKicker();
}
if(m_xboxController.getRightBumper())
{
m_subsystem.spindexerOn();
}
if(m_xboxController.getLeftBumper())
{
m_subsystem.spindexerOff();
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

52
src/main/java/frc/robot/commands/ManualIntake.java Normal file
View File

@ -0,0 +1,52 @@
// 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.commands;
import frc.robot.subsystems.IntakeSubsystem;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. */
public class ManualIntake extends Command {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final IntakeSubsystem m_subsystem;
private final XboxController m_XboxController;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) {
m_subsystem = subsystem;
m_XboxController = xboxController;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_XboxController.getYButton()==true)
{
m_subsystem.deploy();
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

48
src/main/java/frc/robot/commands/ManualShootCommand.java Normal file
View File

@ -0,0 +1,48 @@
// 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.commands;
import frc.robot.subsystems.TurretSubsystem;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj2.command.Command;
/** An example command that uses an example subsystem. */
public class ManualShootCommand extends Command {
private final TurretSubsystem m_subsystem;
private final XboxController m_controller;
public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) {
m_subsystem = subsystem;
m_controller = controller;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0)
{
m_subsystem.turretRotateDir(-.5);
m_controller.setRumble(RumbleType.kRightRumble, 1);
}
else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0)
{
m_subsystem.turretRotateDir(.5);
m_controller.setRumble(RumbleType.kLeftRumble, 1);
}
else
{
m_subsystem.turretRotateDir(0);
m_controller.setRumble(RumbleType.kBothRumble, 0);
}
}
@Override
public boolean isFinished() {
return false;
}
}

57
src/main/java/frc/robot/commands/Reverse.java
View File

@ -1,57 +0,0 @@
// 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.commands;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
/**
* An example command that uses an example subsystem.
*/
public class Reverse extends Command {
private IntakeSubsystem intakeSubsystem;
private HopperSubsystem hopperSubsystem;
private XboxController m_XboxController;
/**
* Creates a new ExampleCommand.
*
* @param subsystem The subsystem used by this command.
*/
public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem, XboxController m_XboxController) {
this.intakeSubsystem = intakeSubsystem;
this.hopperSubsystem = hopperSubsystem;
this.m_XboxController = m_XboxController;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
intakeSubsystem.reverseIntake();
hopperSubsystem.spindexerReverse();
hopperSubsystem.reverseKicker();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
hopperSubsystem.stopKicker();
hopperSubsystem.spindexerOff();
intakeSubsystem.stopIntake(m_XboxController);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

86
src/main/java/frc/robot/commands/ZoneCheck.java
View File

@ -1,86 +0,0 @@
package frc.robot.commands;
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.DriveSubsystem;
public class ZoneCheck extends Command {
private Zone zone;
private DriveSubsystem driveSubsystem;
public ZoneCheck(DriveSubsystem driveSubsystem)
{
this.driveSubsystem = driveSubsystem;
}
public enum Zone {
Blue,
Red,
Neutral,
INIT;
}
@Override
public void initialize() {
zone = Zone.INIT;
}
@Override
public void execute() {
getZone();
Logger.recordOutput("FieldZone", zone);
DriverStation.getAlliance().ifPresent((aliance) -> {
if (aliance == Alliance.Blue) {
switch (zone) {
case Blue:
System.out.println("BLUE");
break;
case Neutral:
System.out.println("NEUTRAL");
break;
case Red:
System.out.println("RED");
break;
default:
System.out.println("****");
}
} else {
switch (zone) {
case Blue:
System.out.println("BLUE");
break;
case Neutral:
System.out.println("NEUTRAL");
break;
case Red:
System.out.println("RED");
break;
default:
System.out.println("****");
}
}
});
}
@Override
public boolean isFinished() {
return false;
}
private void getZone() {
if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
zone = Zone.Red;
} else if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
zone = Zone.Neutral;
} else {
zone = Zone.Blue;
}
}
}

80
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -1,9 +1,8 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.TestableSubsystem;
import org.fairportrobotics.frc.posty.test.PostTest; import org.fairportrobotics.frc.posty.test.PostTest;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
@ -14,7 +13,6 @@ import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.util.PathPlannerLogging;
import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
@ -22,12 +20,12 @@ import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants; import frc.robot.Constants;
public class DriveSubsystem extends TestableSubsystem { public class DriveSubsystem extends TestableSubsystem {
@ -35,7 +33,7 @@ public class DriveSubsystem extends TestableSubsystem {
private SendableChooser<Command> autoChooser; private SendableChooser<Command> autoChooser;
private SwerveBuilder swerveBuilder = new SwerveBuilder(); private SwerveBuilder swerveBuilder = new SwerveBuilder();
public SwerveDriveSystem driveSystem; private SwerveDriveSystem driveSystem;
private double driveP = 0.1; private double driveP = 0.1;
private double driveI = 0.0; private double driveI = 0.0;
@ -52,8 +50,8 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem = swerveBuilder driveSystem = swerveBuilder
.withCanbusName("Drive") .withCanbusName("Drive")
.withPigeonId(Constants.ExtraIDEntities.PIGEON_ID) .withPigeonId(Constants.ExtraIDEntities.PIGEON_ID)
.withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY) .withMaxLinearVelocity(3.0)
.withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY) .withMaxAngularVelocity(Math.PI)
.withSwerveModule( .withSwerveModule(
swerveBuilder.new SwerveModuleBuilder() swerveBuilder.new SwerveModuleBuilder()
.withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID) .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID)
@ -135,8 +133,9 @@ public class DriveSubsystem extends TestableSubsystem {
.build()) .build())
.build(); .build();
// Set vision measurement confidence values // Set vision measurement confidence values
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.1)); driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7));
this.setDefaultCommand(Commands.run(new Runnable() { this.setDefaultCommand(Commands.run(new Runnable() {
@ -149,6 +148,7 @@ public class DriveSubsystem extends TestableSubsystem {
Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates()); Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates()); Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());
} }
}, this)); }, this));
@ -161,10 +161,10 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem::getRobotPose2d, driveSystem::getRobotPose2d,
driveSystem::setPose2d, driveSystem::setPose2d,
driveSystem::getRobotRelativeSpeeds, driveSystem::getRobotRelativeSpeeds,
(speeds, feedforwards) -> {driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(-speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond, driveSystem.getRobotHeading()), new Translation2d());}, (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
new PPHolonomicDriveController( new PPHolonomicDriveController(
new PIDConstants(5, 0, 0), new PIDConstants(5.0, 0, 0),
new PIDConstants(Math.PI, 0, 0)), new PIDConstants(5.0, 0, 0)),
config, config,
() -> { () -> {
var alliance = DriverStation.getAlliance(); var alliance = DriverStation.getAlliance();
@ -179,15 +179,10 @@ public class DriveSubsystem extends TestableSubsystem {
autoChooser.setDefaultOption("Do Nothing", Commands.none()); autoChooser.setDefaultOption("Do Nothing", Commands.none());
SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putData("Auto Chooser", autoChooser);
// PathPlannerLogging.setLogActivePathCallback((List<Pose2d> pose) -> {});
PathPlannerLogging.setLogTargetPoseCallback((Pose2d targetPose) -> { Logger.recordOutput("PathPlanner-TargetPose", targetPose);});
} catch (Exception e) { } catch (Exception e) {
e.printStackTrace(); e.printStackTrace();
} }
} }
@Override @Override
@ -197,45 +192,16 @@ public class DriveSubsystem extends TestableSubsystem {
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d()); Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.FRONT_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0); LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.BACK_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0); if(frontCameraPose.tagCount >= 1){
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
// MegaTag 2 impl Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
if(RobotState.isDisabled()){
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1);
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1);
}else{
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4);
LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4);
} }
LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME); if(backCameraPose.tagCount >= 1){
LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME); driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
if(frontTag.tagCount >= 2){
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds);
Logger.recordOutput("FrontPose-Estimate", frontTag.pose);
Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2);
} }
if(backTag.tagCount >= 2){
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds);
Logger.recordOutput("BackPose-Estimate", backTag.pose);
Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2);
}
// MegaTag 1 Impl
// LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
// if (frontCameraPose.tagCount >= 2) {
// driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
// Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
// }
// LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
// if (backCameraPose.tagCount >= 2) {
// driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
// Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
// }
} }
@Override @Override
@ -244,15 +210,15 @@ public class DriveSubsystem extends TestableSubsystem {
driveSystem.simulationPeriodic(); driveSystem.simulationPeriodic();
} }
public Command getAutoCommand() { public Command getAutoCommand(){
return autoChooser.getSelected(); return autoChooser.getSelected();
} }
public void stopDrive() { public void stopDrive(){
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
} }
public void rotateChassis(double rotSpeed) { public void rotateChassis(double rotSpeed){
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
} }

107
src/main/java/frc/robot/subsystems/HopperSubsystem.java
View File

@ -1,114 +1,45 @@
// Copyright (c) FIRST and other WPILib contributors. // Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of // 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. // the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems; package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.TestableSubsystem;
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
import org.fairportrobotics.frc.posty.test.PostTest; import org.fairportrobotics.frc.posty.test.PostTest;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.Units; import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants; import frc.robot.Constants;
public class HopperSubsystem extends TestableSubsystem { public class HopperSubsystem extends TestableSubsystem {
private TalonFX spindexerMotor; private TalonFX spindexerMotor;
private TalonFX kickerMotor; private TalonFX kickerMotor;
private VelocityVoltage kickerControl = new VelocityVoltage(0); /** Creates a new HopperSubsystem. */
private StatusSignal<AngularVelocity> kickerVelocity; public HopperSubsystem() {
spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION));
/** kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
* Creates a new HopperSubsystem. kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION));
*/ }
public HopperSubsystem() {
spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
spindexerMotor.getConfigurator().apply(
new TalonFXConfiguration()
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
.withSlot0(
new Slot0Configs()
.withKP(0)
.withKI(0)
.withKD(0)
.withKS(0)));
kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); public void feedKicker() {kickerMotor.set(0.75);}
kickerMotor.getConfigurator().apply(
new TalonFXConfiguration()
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
)
.withSlot0(new Slot0Configs()
.withKP(0)
.withKI(0)
.withKD(0)
.withKS(0)
));
kickerVelocity = kickerMotor.getVelocity();
}
public void feedKicker() { public void reverseKicker() {kickerMotor.set(-0.5);}
//kickerMotor.setControl(kickerControl.withVelocity(1000));
kickerMotor.set(0.75);
}
public void reverseKicker() { public void stopKicker() {kickerMotor.stopMotor();}
//kickerMotor.setControl(kickerControl.withVelocity(-500));
kickerMotor.set(-0.5);
}
public void stopKicker() { public void spindexerOn() { spindexerMotor.set(0.75);}
kickerMotor.stopMotor();
}
public void spindexerOn() { public void spindexerOff() { spindexerMotor.stopMotor();}
spindexerMotor.set(0.75);
}
public void spindexerReverse() { @PostTest(enabled = true)
spindexerMotor.set(-0.75); public void hopperSystem_CANDevicesConnected(){
} assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
public void spindexerOff() { }
spindexerMotor.stopMotor();
}
public Command spinny() {
return this.runOnce(() -> {
if (spindexerMotor.get() == 0) {
spindexerOn();
} else {
spindexerOff();
}
});
}
@Override
public void periodic() {
Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
}
@PostTest(enabled = true)
public void hopperSystem_CANDevicesConnected() {
assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
}
} }

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

@ -1,93 +1,79 @@
// Copyright (c) FIRST and other WPILib contributors. // Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of // 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. // the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems; package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.TestableSubsystem;
import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
import org.fairportrobotics.frc.posty.test.PostTest; import org.fairportrobotics.frc.posty.test.PostTest;
import static org.fairportrobotics.frc.robolib.motors.Utils.SetMotorDirection;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants; import frc.robot.Constants;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import static org.fairportrobotics.frc.robolib.motors.Utils.*;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
public class IntakeSubsystem extends TestableSubsystem { public class IntakeSubsystem extends TestableSubsystem {
/** Creates a new ExampleSubsystem. */
/** private TalonFX intakeMotor;
* Creates a new ExampleSubsystem. private WPI_TalonSRX deployMotor;
*/
private TalonFX intakeMotor; public IntakeSubsystem()
private WPI_TalonSRX deployMotor; {
intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
public IntakeSubsystem() { deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID); deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION); deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
}
deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID); public Command deploy()
deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); {
deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); return Commands.sequence(
deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); Commands.deadline(
} Commands.waitSeconds(2),
this.runEnd(
() -> {deployMotor.set(0.5);}, // Bump this speed up if intake doesn't deploy
() -> {deployMotor.stopMotor();}
)
),
Commands.deadline(
Commands.waitSeconds(2),
this.runEnd(
() -> { deployMotor.set(-0.5); }, // Same here
() -> { deployMotor.stopMotor(); }
)
)
);
}
public Command deploy() { public void setSpeed(double speed)
return Commands.sequence( {
Commands.deadline( intakeMotor.set(speed);
Commands.waitSeconds(2), }
this.runOnce(
() -> {
deployMotor.set(1);
} // Bump this speed up if intake doesn't deploy
)
),
this.runOnce(() -> deployMotor.stopMotor())
);
}
public Command resetDeploy() { public Command intake()
return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor()); {
} return this.runOnce(() -> {if(intakeMotor.get()!=0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
}
public void setSpeed(double speed) { public Command reverseIntake()
intakeMotor.set(speed); {
} return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
}
public Command stopIntake(XboxController m_XboxController) { @PostTest(enabled = true)
return this.runOnce(() -> { public void IntakeSubsystem_CANDevicesConnected(){
intakeMotor.stopMotor(); assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
m_XboxController.setRumble(RumbleType.kBothRumble, .0); assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
}); }
}
public Command intake(XboxController m_XboxController) {
return this.runOnce(() -> {
if (intakeMotor.get() == 0) {
setSpeed(-.5);
m_XboxController.setRumble(RumbleType.kBothRumble, .5);
} else {
intakeMotor.stopMotor();
m_XboxController.setRumble(RumbleType.kBothRumble, .0);
}
});
}
public Command reverseIntake() {
return this.runEnd(() -> setSpeed(.5), () -> {
intakeMotor.stopMotor();
});
}
@PostTest(enabled = true)
public void IntakeSubsystem_CANDevicesConnected() {
assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
}
} }

157
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -7,7 +7,6 @@ package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.TestableSubsystem;
import org.fairportrobotics.frc.posty.test.PostTest; import org.fairportrobotics.frc.posty.test.PostTest;
import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
@ -35,9 +34,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkMaxConfig;
import frc.robot.Constants; import frc.robot.Constants;
import frc.robot.Robot;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.AngleUnit;
import edu.wpi.first.units.Units; import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
@ -62,19 +58,13 @@ public class TurretSubsystem extends TestableSubsystem {
private StatusSignal<Angle> turretMotorPosition; private StatusSignal<Angle> turretMotorPosition;
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit; private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit; private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
private StatusSignal<Double> turretPositionError;
private double forwardLimit = 0;
private double reverseLimit = 0;
private SparkMax launcherMotor; private SparkMax launcherMotor;
private SparkClosedLoopController launcherControler; private SparkClosedLoopController launcherControler;
private double turretOffsetInRotations = 0; private Angle turretOffset = Angle.ofBaseUnits(0, Units.Degrees);
private Servo hood; private Servo hood;
private Pose3d turretTarget;
public TurretSubsystem() { public TurretSubsystem() {
// Turret configuration // Turret configuration
@ -84,9 +74,9 @@ public class TurretSubsystem extends TestableSubsystem {
new TalonFXConfiguration() new TalonFXConfiguration()
.withSlot0( .withSlot0(
new Slot0Configs() new Slot0Configs()
.withKP(6) .withKP(2)
.withKI(0) .withKI(1)
.withKD(0.3) .withKD(0)
.withKS(0) .withKS(0)
) )
.withMotorOutput( .withMotorOutput(
@ -106,7 +96,6 @@ public class TurretSubsystem extends TestableSubsystem {
turretMotorPosition = turretMotor.getPosition(); turretMotorPosition = turretMotor.getPosition();
turretMotorForwardLimit = turretMotor.getForwardLimit(); turretMotorForwardLimit = turretMotor.getForwardLimit();
turretMotorReverseLimit = turretMotor.getReverseLimit(); turretMotorReverseLimit = turretMotor.getReverseLimit();
turretPositionError = turretMotor.getClosedLoopError();
turretSim = turretMotor.getSimState(); turretSim = turretMotor.getSimState();
@ -115,8 +104,8 @@ public class TurretSubsystem extends TestableSubsystem {
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
config.voltageCompensation(10); config.voltageCompensation(10);
config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001 config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001
config.closedLoop.feedForward.kS(0.2).kV(0.00005).kA(0.001); config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00);
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
config.closedLoop.positionWrappingEnabled(false); config.closedLoop.positionWrappingEnabled(false);
@ -142,8 +131,12 @@ public class TurretSubsystem extends TestableSubsystem {
public boolean isLauncherUpToSpeed() { public boolean isLauncherUpToSpeed() {
// Valid if velcity is within 10% of setpoint. // Valid if velcity is within 10% of setpoint.
// May need to lower this // May need to lower this
return true; return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1;
// return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03; }
public void turretControl(double position)
{
turretMotor.setControl(new PositionVoltage(position));
} }
public void setTargetElevation(double elev) public void setTargetElevation(double elev)
@ -168,9 +161,6 @@ public class TurretSubsystem extends TestableSubsystem {
public void homeTurret() public void homeTurret()
{ {
if(turretState != TurretState.INIT){
return;
}
turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
turretState = TurretState.HOMING; turretState = TurretState.HOMING;
} }
@ -184,69 +174,59 @@ public class TurretSubsystem extends TestableSubsystem {
turretMotor.set(speed); turretMotor.set(speed);
} }
public double getTurretAngleRobotRelative() public Angle getTurretAngle()
{ {
return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO; return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
} }
public void setTurretTargetPose(Pose3d targetPose){ private Angle getTurretMotorPosition(){
turretTarget = targetPose; return turretMotorPosition.refresh().getValue().minus(turretOffset);
}
public Pose3d getTurretTargetPose(){
return turretTarget;
}
private double getTurretMotorPosition(){
return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
} }
@Override @Override
public void periodic() { public void periodic() {
if (turretState == TurretState.HOMING) {
if (turretLimitSwitch.get()) { // Turret has tripped the switch if(turretState == TurretState.HOMING)
{
turretMotor.set(0.05);
if(turretLimitSwitch.get())
{ // Turret has tripped the switch
turretMotor.stopMotor(); turretMotor.stopMotor();
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323); turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
if (!Robot.isReal()) {
turretOffsetInRotations = 0; // Flip turret around
}
turretState = TurretState.READY; turretState = TurretState.READY;
forwardLimit = (1.5 + turretOffsetInRotations); Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
reverseLimit = (-1.5 + turretOffsetInRotations); Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit); Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply( turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs() new SoftwareLimitSwitchConfigs()
.withForwardSoftLimitEnable(true) .withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(forwardLimit) .withForwardSoftLimitThreshold(forwardLimit)
.withReverseSoftLimitEnable(true) .withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit)); .withReverseSoftLimitThreshold(reverseLimit)
// setTurretRobotRelative(0); );
// setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY //setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees));
} }
} }
if (LOGGING) { if(LOGGING)
{
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get()); Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState); Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative()); Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition()); Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
turretMotorPosition.refresh().getValue().in(Units.Rotations)); Logger.recordOutput("TurretSubsystem-TurretRequestedPos", turretControlMode.getPositionMeasure().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees));
turretControlMode.getPositionMeasure().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble());
Logger.recordOutput("TurretSubsystem-LauncherSetpoint(RPM)", launcherControler.getSetpoint());
} }
} }
public void setTurretFieldRelative(Pose3d robotPose, double pos) public void setTurretRotation(Angle pos)
{ {
if(turretState != TurretState.READY) if(turretState != TurretState.READY)
{ {
@ -254,59 +234,12 @@ public class TurretSubsystem extends TestableSubsystem {
return; return;
} }
double correctedPos = -pos; // Flip angle to match CCW positive. Add 0.5 to flip around setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
double robotRotations = robotPose.getRotation().toRotation2d().getRotations();
Rotation2d turretAngle = Rotation2d.fromRotations(correctedPos).plus(Rotation2d.fromRotations(robotRotations)).minus(Rotation2d.fromRotations(0.5));
Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations());
setTurretRobotRelative(((turretAngle.getRotations())));
} }
public void setTurretRobotRelative(double pos) public void setTurretMotorRotation(Angle pos){
{ turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
if(turretState != TurretState.READY) turretSim.setRawRotorPosition(pos.plus(turretOffset));
{
// Not ready to set angle
return;
}
double correctedPos = -pos; // Flip angle to match CCW positive
setTurretMotorRotation(correctedPos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
}
public void setTurretMotorRotation(double pos){
double posWithOffset = pos + turretOffsetInRotations;
Logger.recordOutput("Blah", posWithOffset);
if(Robot.isSimulation()){
if(posWithOffset >= forwardLimit){
turretSim.setForwardLimit(true);
} else {
turretSim.setForwardLimit(false);
}
if(posWithOffset <= reverseLimit){
turretSim.setReverseLimit(true);
} else {
turretSim.setReverseLimit(false);
}
}
turretMotor.setControl(turretControlMode.withPosition(posWithOffset));
turretSim.setRawRotorPosition(-posWithOffset);
}
public boolean isTurretAtTarget(){
return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
}
private double getRequestedPosTurretRelative() {
return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
} }
@PostTest() @PostTest()