+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "AP1"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station R P2"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station R P3"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "P 1 - Align After"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 11/20 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Official Lollipop 1"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Lollipop 2"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Lollipop 3"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Lollipop 4"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "P 1"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 11/20 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Gronud P2"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "parallel",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Left"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "wait",
- "data": {
- "waitTime": 1.0
- }
- },
- {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- ]
- }
- }
- ]
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "GroundB #6"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "parallel",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Right"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "wait",
- "data": {
- "waitTime": 1.0
- }
- },
- {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- ]
- }
- }
- ]
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "P 1 - Align After"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 11/20 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Sweep P2"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Drive To Left Station"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "LSweep P3"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "GrondP4"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Drive To Left Station"
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "P 1"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 11/20 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Gronud P2"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "P 5"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Left"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "GroundB #6"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "P 3"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "3d intake peice"
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "P 1 Mirrored"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 9/22 Left"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Gronud P2 Mirrored"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "P 5 Mirrored"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 8/17 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "GroundB #6 Mirrored"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "P 3 Mirrored"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 8/17 Left"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "P 1 - Align After"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 11/20 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station P2"
- }
- },
- {
- "type": "parallel",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Station Intake"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "StationB Alt Opt Left - Align After"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Left"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": " Station P4"
- }
- },
- {
- "type": "parallel",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Station Intake"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": " StationB Alt Opt - Align After"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "P 1"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 11/20 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station P2"
- }
- },
- {
- "type": "parallel",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Station Intake"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "StationP 3"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Left"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": " Station P4"
- }
- },
- {
- "type": "parallel",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Station Intake"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "StationB Alt Opt"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive To 6/19 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "path",
- "data": {
- "pathName": "AP1"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive to 9/22 Left"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station R P2"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Drive to Right Station Intake"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Station Intake"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station R P3"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive to 8/17 Right"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station R P4"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Drive to Right Station Intake"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Station Intake"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Station R P5"
- }
- },
- {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Drive to 8/17 Left"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "OuttakeCoral"
- }
- },
- {
- "type": "named",
- "data": {
- "name": "Lower Elevator"
- }
- }
- ]
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.66,
- "y": 5.12
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.909326741052695,
- "y": 5.101664891709005
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.722,
- "y": 7.129
- },
- "prevControl": {
- "x": 1.5210513367419152,
- "y": 7.294369927869808
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 0.6049966239027695,
- "maxWaypointRelativePos": 1.0,
- "constraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "IntakeCoral",
- "waypointRelativePos": 0.0,
- "endWaypointRelativePos": 1.0,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -145.0
- },
- "reversed": false,
- "folder": null,
- "idealStartingState": {
- "velocity": 0.0,
- "rotation": -150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.9300204918032788,
- "y": 7.243698770491803
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.930020491803279,
- "y": 7.243698770491803
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.6202868852459016,
- "y": 5.589395491803279
- },
- "prevControl": {
- "x": 2.6202868852459016,
- "y": 5.589395491803279
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -145.66978280449666
- },
- "reversed": false,
- "folder": null,
- "idealStartingState": {
- "velocity": 0,
- "rotation": -141.34019174590986
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 7.18063524590164,
- "y": 0.48
- },
- "prevControl": null,
- "nextControl": {
- "x": 6.483621180258631,
- "y": 1.147118232765553
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 4.81183,
- "y": 2.768
- },
- "prevControl": {
- "x": 5.174076079601306,
- "y": 2.4481272403373495
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.4514601420678732,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": 29.999999999999996
- },
- "reversed": false,
- "folder": null,
- "idealStartingState": {
- "velocity": 0,
- "rotation": 0.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 7.229,
- "y": 3.8608
- },
- "prevControl": null,
- "nextControl": {
- "x": 6.903565973322253,
- "y": 3.8608
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 5.74,
- "y": 3.8608
- },
- "prevControl": {
- "x": 6.308787430236012,
- "y": 3.8608
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.7829518547750566,
- "endWaypointRelativePos": null,
- "command": null
- }
- ],
- "globalConstraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 0.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": 90.0
- },
- "reversed": false,
- "folder": null,
- "idealStartingState": {
- "velocity": 0,
- "rotation": 90.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 7.229,
- "y": 4.191
- },
- "prevControl": null,
- "nextControl": {
- "x": 6.903565973322253,
- "y": 4.191
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 5.74,
- "y": 4.191
- },
- "prevControl": {
- "x": 6.308787430236012,
- "y": 4.191
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [],
- "globalConstraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 0.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": 90.0
- },
- "reversed": false,
- "folder": null,
- "idealStartingState": {
- "velocity": 0,
- "rotation": 90.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.721,
- "y": 5.077
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.7080822290916022,
- "y": 4.827333960669942
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.260710227277713,
- "y": 7.240838068181818
- },
- "prevControl": {
- "x": 3.769261363646335,
- "y": 7.031434659090908
- },
- "nextControl": {
- "x": 2.5266509333291625,
- "y": 7.543097777451788
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 0.8076988636413492,
- "y": 6.572741477272728
- },
- "prevControl": {
- "x": 2.1270340909140755,
- "y": 7.342904545454545
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.4,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -53.616
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -145.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 5.0,
- "y": 2.81
- },
- "prevControl": null,
- "nextControl": {
- "x": 4.967147352430555,
- "y": 2.1109704137731478
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.6396306818181814,
- "y": 1.826
- },
- "prevControl": {
- "x": 3.976501550755575,
- "y": 1.7392387033253078
- },
- "nextControl": {
- "x": 2.8861082175925916,
- "y": 2.020070167824074
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.722,
- "y": 0.921
- },
- "prevControl": {
- "x": 2.7759913917824064,
- "y": 2.1777043547453694
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 1.25,
- "rotationDegrees": -39.29328640235812
- }
- ],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 1.6961512491559774,
- "maxWaypointRelativePos": 2.0,
- "constraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.55,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -36.0
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0.0,
- "rotation": 29.999999999999996
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 5.0,
- "y": 5.24
- },
- "prevControl": null,
- "nextControl": {
- "x": 4.856164772727273,
- "y": 5.775014204545455
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.6396306818181814,
- "y": 6.223735795454545
- },
- "prevControl": {
- "x": 3.9487500000000004,
- "y": 6.383281249999999
- },
- "nextControl": {
- "x": 3.061864563544763,
- "y": 5.925533927958591
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.722,
- "y": 7.129
- },
- "prevControl": {
- "x": 2.562698863636364,
- "y": 6.044247159090909
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 1.25,
- "rotationDegrees": -145.0
- }
- ],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 1.6961512491559774,
- "maxWaypointRelativePos": 2.0,
- "constraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.55,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 4.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -144.0
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0.0,
- "rotation": 150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 4.0073368,
- "y": 5.1909832
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.6414852850066137,
- "y": 5.481452904381226
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.885,
- "y": 7.011
- },
- "prevControl": {
- "x": 2.279280354370602,
- "y": 6.697959071416107
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.6463314097279476,
- "endWaypointRelativePos": 1.0,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 0.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -144.0
- },
- "reversed": false,
- "folder": "Ground BSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.885,
- "y": 7.011
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.1167904466171583,
- "y": 6.862521975540034
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.7213752,
- "y": 5.0258832
- },
- "prevControl": {
- "x": 3.3903399058878056,
- "y": 5.426007692748621
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -150.0
- },
- "reversed": false,
- "folder": "Ground BSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -144.0
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.722,
- "y": 7.129
- },
- "prevControl": null,
- "nextControl": {
- "x": 1.8649319141897391,
- "y": 6.267760789902279
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.1273,
- "y": 4.191
- },
- "prevControl": {
- "x": 2.1273,
- "y": 4.191
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.6806256151574805,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.5,
- "rotation": -90.89200324814249
- },
- "reversed": false,
- "folder": "Ground BSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -142.86256094570606
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.98,
- "y": 2.81
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.661208032083275,
- "y": 2.7146988994001573
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.722,
- "y": 0.921
- },
- "prevControl": {
- "x": 1.2995983363740153,
- "y": 0.5281644353113872
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.3291032261499149,
- "rotationDegrees": -32.65592295916657
- }
- ],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 0.5509790681971605,
- "maxWaypointRelativePos": 1.0,
- "constraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.11073598919649108,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -35.0
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -29.999999999999996
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.98,
- "y": 5.24
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.316855850468241,
- "y": 5.760793783082248
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.722,
- "y": 7.129
- },
- "prevControl": {
- "x": 2.338563708774429,
- "y": 6.649638232899023
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.3291032261499149,
- "rotationDegrees": -136.13455862983048
- }
- ],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 0.5509790681971605,
- "maxWaypointRelativePos": 1.0,
- "constraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.11073598919649108,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 4.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -142.62124693823438
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -150.82019924107797
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.7213752,
- "y": 5.0258832
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.2108522727272732,
- "y": 5.555639204545454
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.722,
- "y": 7.129
- },
- "prevControl": {
- "x": 2.061030502628888,
- "y": 6.716198757492258
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.3779527559055112,
- "endWaypointRelativePos": 1.0,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -144.0
- },
- "reversed": false,
- "folder": "Ground BSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 0.788,
- "y": 6.603
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.9316477272727273,
- "y": 6.26396875
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.721,
- "y": 5.025
- },
- "prevControl": {
- "x": 3.5675633667260422,
- "y": 5.222375782631895
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Elevator",
- "waypointRelativePos": 0.16647919010123857,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -150.0
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -56.976000000000006
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.239,
- "y": 5.843
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.189204371946254,
- "y": 5.904804878359121
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.6657752058351885,
- "y": 5.122
- },
- "prevControl": {
- "x": 3.1011126071083357,
- "y": 5.555954762962963
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Elevator",
- "waypointRelativePos": 0.4830139640748033,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.5,
- "rotation": -150.68614751980144
- },
- "reversed": false,
- "folder": "Lollipop Path",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -89.56435434461116
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.6657752058351885,
- "y": 5.122185271626888
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.232096205428022,
- "y": 5.337027431134217
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.224,
- "y": 4.023
- },
- "prevControl": {
- "x": 2.2404897088951454,
- "y": 4.642129432338781
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 0.6121951664719626,
- "maxWaypointRelativePos": 1.0,
- "constraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.6682840182086613,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -48.652092538649285
- },
- "reversed": false,
- "folder": "Lollipop Path",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -149.20719589802195
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.224,
- "y": 4.023
- },
- "prevControl": null,
- "nextControl": {
- "x": 1.9751816342630288,
- "y": 4.332334397903094
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 2.923609664351851,
- "y": 4.208198061342593
- },
- "prevControl": {
- "x": 2.412264463822535,
- "y": 4.184931100328749
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Elevator",
- "waypointRelativePos": 0.402936988901869,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.5,
- "rotation": -89.04406269082419
- },
- "reversed": false,
- "folder": "Lollipop Path",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -43.54279799705628
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.239,
- "y": 4.191
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.0626564776043614,
- "y": 3.9678578908798716
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.197960792824073,
- "y": 2.2002983940972216
- },
- "prevControl": {
- "x": 0.9319482350521202,
- "y": 1.9434278342202775
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 0.6121951664719626,
- "maxWaypointRelativePos": 1.0,
- "constraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.6682840182086613,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -48.652
- },
- "reversed": false,
- "folder": "Lollipop Path",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -90.0
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.198,
- "y": 2.2
- },
- "prevControl": null,
- "nextControl": {
- "x": 1.0216564776043615,
- "y": 1.976857890879872
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.23,
- "y": 4.191
- },
- "prevControl": {
- "x": 2.9639874422280474,
- "y": 3.9341294401230558
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 0.6121951664719626,
- "maxWaypointRelativePos": 1.0,
- "constraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.6682840182086613,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -90.0
- },
- "reversed": false,
- "folder": "Lollipop Path",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -36.0
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 5.026909205835189,
- "y": 5.287285271626888
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.9877799051430065,
- "y": 5.857346791883403
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.239,
- "y": 5.843
- },
- "prevControl": {
- "x": 1.9968631781709456,
- "y": 5.892619841156129
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 0.5537930782710281,
- "maxWaypointRelativePos": 1.0,
- "constraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- }
- ],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.40029835137795283,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -89.24717948106847
- },
- "reversed": false,
- "folder": "Lollipop Path",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 150.0
- },
- "useDefaultConstraints": true
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 7.065,
- "y": 7.315
- },
- "prevControl": null,
- "nextControl": {
- "x": 6.311723981584821,
- "y": 6.540661185128348
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 4.9713092,
- "y": 5.1909832
- },
- "prevControl": {
- "x": 5.454483001977311,
- "y": 5.695001290538291
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "backdrive",
- "waypointRelativePos": 0,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "backdrive"
- }
- }
- },
- {
- "name": "lower intake",
- "waypointRelativePos": 0.14206787687450353,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "lower intake"
- }
- }
- },
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.4625098658247798,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.5,
- "rotation": 150.0
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 7.065,
- "y": 0.735
- },
- "prevControl": null,
- "nextControl": {
- "x": 6.163789350689934,
- "y": 1.5557182436227601
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 5.094774590163934,
- "y": 2.712
- },
- "prevControl": {
- "x": 5.687902753152638,
- "y": 2.1760455531838283
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "backdrive",
- "waypointRelativePos": 0,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "backdrive"
- }
- }
- },
- {
- "name": "lower intake",
- "waypointRelativePos": 0.14206787687450353,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "lower intake"
- }
- }
- },
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.4625098658247798,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": 29.999999999999996
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 29.999999999999996
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 7.065,
- "y": 7.315
- },
- "prevControl": null,
- "nextControl": {
- "x": 6.407825499545448,
- "y": 6.60471106063858
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 5.094774590163934,
- "y": 5.33765368852459
- },
- "prevControl": {
- "x": 5.760558198478665,
- "y": 6.031726466482178
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "backdrive",
- "waypointRelativePos": 0,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "backdrive"
- }
- }
- },
- {
- "name": "lower intake",
- "waypointRelativePos": 0.14206787687450353,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "lower intake"
- }
- }
- },
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.4625098658247798,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": 150.0
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.722,
- "y": 0.921
- },
- "prevControl": null,
- "nextControl": {
- "x": 1.9852053519873245,
- "y": 0.9368665670159784
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.5843237704918027,
- "y": 2.796
- },
- "prevControl": {
- "x": 3.3462031912644794,
- "y": 2.7198515282593014
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Elevator",
- "waypointRelativePos": 0.16647919010123857,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -29.999999999999996
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -35.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.722,
- "y": 7.1288
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.513128546999364,
- "y": 6.385951337740788
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.5843237704918027,
- "y": 5.25373975409836
- },
- "prevControl": {
- "x": 3.1610940855407414,
- "y": 5.679961652038816
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Elevator",
- "waypointRelativePos": 0.16647919010123857,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 4.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -150.0
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0.0,
- "rotation": -145.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.722,
- "y": 0.921
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.0053161494766774,
- "y": 1.1670302077595403
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.8720286885245896,
- "y": 2.7
- },
- "prevControl": {
- "x": 3.5346593444490164,
- "y": 2.4590517448283564
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.39370078740157555,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -29.999999999999996
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -35.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.722,
- "y": 7.129
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.58836769386574,
- "y": 6.410185185185185
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.8720286885245896,
- "y": 5.3496413934426235
- },
- "prevControl": {
- "x": 3.2181991167653305,
- "y": 5.822776882447252
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.39370078740157555,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -151.83
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -145.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 4.9713092,
- "y": 5.1909832
- },
- "prevControl": null,
- "nextControl": {
- "x": 4.461440347544805,
- "y": 7.189854101642059
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.65,
- "y": 7.4
- },
- "prevControl": {
- "x": 3.1784221311513603,
- "y": 5.374715163937142
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.4991119005328578,
- "rotationDegrees": 61.586
- }
- ],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Station",
- "waypointRelativePos": 0.6590141796083744,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Station Setpoint"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": 36.0
- },
- "reversed": false,
- "folder": "Station Left",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.9517367941648103,
- "y": 5.287285271626888
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.4418679417096154,
- "y": 7.286156173268947
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.356,
- "y": 7.331
- },
- "prevControl": {
- "x": 2.8844221311513616,
- "y": 5.305715163937142
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.8063943161634158,
- "rotationDegrees": 61.586
- }
- ],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.6861642294713134,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Station Setpoint"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.5,
- "maxAcceleration": 3.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": 36.0
- },
- "reversed": false,
- "folder": "Station Left",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 4.812,
- "y": 2.768
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.6667827868852463,
- "y": 1.1836495901639348
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.525653409090909,
- "y": 0.7293892045454539
- },
- "prevControl": {
- "x": 3.111195051053366,
- "y": 2.9091389109644723
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.6287744227353493,
- "rotationDegrees": 131.817128535173
- }
- ],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Station",
- "waypointRelativePos": 0.6634844868735067,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Station Setpoint"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": 145.0
- },
- "reversed": false,
- "folder": "Station RSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 29.999999999999996
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.526,
- "y": 0.729
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.0456919758517573,
- "y": 2.2265382848833575
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 4.007,
- "y": 2.861
- },
- "prevControl": {
- "x": 3.745218130977533,
- "y": 2.6169235964968895
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Elevator",
- "waypointRelativePos": 0.16647919010123857,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -29.999999999999996
- },
- "reversed": false,
- "folder": "Station RSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 144.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 4.007,
- "y": 2.861
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.966570514239714,
- "y": 1.77435496905575
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.526,
- "y": 0.729
- },
- "prevControl": {
- "x": 2.4772300707441195,
- "y": 1.765652915018721
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Station",
- "waypointRelativePos": 0.6587112171837709,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Station Setpoint"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": 144.0
- },
- "reversed": false,
- "folder": "Station RSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -29.999999999999996
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.526,
- "y": 0.729
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.9585445192827198,
- "y": 1.9643408880242017
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.721,
- "y": 3.026
- },
- "prevControl": {
- "x": 3.1963255727771056,
- "y": 2.590703696539521
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.39370078740157555,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -29.999999999999996
- },
- "reversed": false,
- "folder": "Station RSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 144.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.722,
- "y": 7.403
- },
- "prevControl": null,
- "nextControl": {
- "x": 1.8649319141897391,
- "y": 6.541760789902279
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.2385,
- "y": 4.191
- },
- "prevControl": {
- "x": 2.2385,
- "y": 4.191
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.6806256151574805,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 0.5,
- "maxAcceleration": 0.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -90.89200324814249
- },
- "reversed": false,
- "folder": "Station Left",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 36.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.356,
- "y": 7.331
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.334068287037036,
- "y": 6.521408420138889
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.666,
- "y": 5.122185271626888
- },
- "prevControl": {
- "x": 2.836426070601852,
- "y": 5.99030882486763
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.6806256151574805,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -149.28470582855732
- },
- "reversed": false,
- "folder": "Station Left",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 36.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.356,
- "y": 7.331
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.320208548409597,
- "y": 6.4855863560267855
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.9517367941648103,
- "y": 5.287285271626888
- },
- "prevControl": {
- "x": 3.2517059326171873,
- "y": 5.978196498325892
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.6806256151574805,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -149.28470582855732
- },
- "reversed": false,
- "folder": "Station Left",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 36.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.65,
- "y": 7.4
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.334068287037036,
- "y": 6.521408420138889
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.5380135672433033,
- "y": 5.395889718191963
- },
- "prevControl": {
- "x": 2.708439637845155,
- "y": 6.264013271432705
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.6806256151574805,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -149.28470582855732
- },
- "reversed": false,
- "folder": "Station Left",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 36.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.65,
- "y": 7.4
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.9294370334836404,
- "y": 6.130971138637706
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.8229949951171873,
- "y": 5.580079432896205
- },
- "prevControl": {
- "x": 3.6027199899253537,
- "y": 5.6983119245714535
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.3836589698046169,
- "rotationDegrees": 150.75582343932
- }
- ],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.45374746792707615,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 3.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -151.83
- },
- "reversed": false,
- "folder": "Station Left",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 36.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 4.9713092,
- "y": 5.191
- },
- "prevControl": null,
- "nextControl": {
- "x": 4.267840909084701,
- "y": 7.908934659084283
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 0.7777840909028838,
- "y": 6.602656249993373
- },
- "prevControl": {
- "x": 2.183778409084702,
- "y": 7.44026988635701
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.5275310834813515,
- "rotationDegrees": -77.19714447023723
- }
- ],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.3,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0.0,
- "rotation": -56.976000000000006
- },
- "reversed": false,
- "folder": "Ground LSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 150.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 5.157,
- "y": 2.996
- },
- "prevControl": null,
- "nextControl": {
- "x": 4.011782786885246,
- "y": 1.411649590163935
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 2.831931818176833,
- "y": 0.5299573863636362
- },
- "prevControl": {
- "x": 3.285943759901909,
- "y": 0.6018426104701067
- },
- "nextControl": {
- "x": 1.6353409090859239,
- "y": 0.34049715909090883
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 0.688,
- "y": 1.368
- },
- "prevControl": {
- "x": 1.3261818181768328,
- "y": 0.709875
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [
- {
- "waypointRelativePos": 0.2984014209591429,
- "rotationDegrees": -36.0
- }
- ],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.55,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -127.476
- },
- "reversed": false,
- "folder": "Sweep Right Side",
- "idealStartingState": {
- "velocity": 0,
- "rotation": 29.999999999999996
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 0.688,
- "y": 1.368
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.2076919758517572,
- "y": 2.8655382848833577
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.0114204545454544,
- "y": 2.1852414772727276
- },
- "prevControl": {
- "x": 2.749638585522988,
- "y": 1.9411650737696169
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Elevator",
- "waypointRelativePos": 0.16647919010123857,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -29.999999999999996
- },
- "reversed": false,
- "folder": "Sweep Right Side",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -127.476
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 3.721,
- "y": 3.0259168
- },
- "prevControl": null,
- "nextControl": {
- "x": 2.6805705142397143,
- "y": 1.93927176905575
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 1.722,
- "y": 0.923
- },
- "prevControl": {
- "x": 2.6732300707441192,
- "y": 1.959652915018721
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Intake",
- "waypointRelativePos": 0.19856459330143417,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "IntakeCoral"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -36.0
- },
- "reversed": false,
- "folder": "Sweep Right Side",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -29.999999999999996
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 1.722,
- "y": 0.923
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.1545445192827195,
- "y": 2.1583408880242017
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 4.00733,
- "y": 2.86082
- },
- "prevControl": {
- "x": 3.482655572777105,
- "y": 2.4255236965395213
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.39370078740157555,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 1.5,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -29.999999999999996
- },
- "reversed": false,
- "folder": "Sweep Right Side",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -36.0
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "waypoints": [
- {
- "anchor": {
- "x": 0.808,
- "y": 6.57
- },
- "prevControl": null,
- "nextControl": {
- "x": 3.599744318181818,
- "y": 6.393252840909091
- },
- "isLocked": false,
- "linkedName": null
- },
- {
- "anchor": {
- "x": 3.2385,
- "y": 4.191
- },
- "prevControl": {
- "x": 2.2385,
- "y": 4.191
- },
- "nextControl": null,
- "isLocked": false,
- "linkedName": null
- }
- ],
- "rotationTargets": [],
- "constraintZones": [],
- "pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "MoveElevator",
- "waypointRelativePos": 0.6806256151574805,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "L4"
- }
- }
- }
- ],
- "globalConstraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 2.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- },
- "goalEndState": {
- "velocity": 0,
- "rotation": -90.89200324814249
- },
- "reversed": false,
- "folder": "Ground BSide",
- "idealStartingState": {
- "velocity": 0,
- "rotation": -53.616
- },
- "useDefaultConstraints": false
-}
\ No newline at end of file
import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
-import edu.wpi.first.math.geometry.Pose2d;
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.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.commands.DoNothing;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
-import frc.robot.commands.drive_comm.DriveToPose;
-import frc.robot.commands.gpm.IntakeCoral;
-import frc.robot.commands.gpm.MoveArm;
-import frc.robot.commands.gpm.MoveElevator;
-import frc.robot.commands.gpm.OuttakeCoral;
-import frc.robot.commands.gpm.StationIntake;
-import frc.robot.constants.ArmConstants;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.Constants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.constants.IntakeConstants;
import frc.robot.constants.VisionConstants;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.climb.Climb;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
-import frc.robot.subsystems.outtake.OuttakeAlpha;
-import frc.robot.subsystems.outtake.OuttakeComp;
import frc.robot.util.PathGroupLoader;
import frc.robot.util.Vision.DetectedObject;
import frc.robot.util.Vision.Vision;
// The robot's subsystems are defined here...
private Drivetrain drive = null;
private Vision vision = null;
- private Intake intake = null;
- private Indexer indexer = null;
- private Outtake outtake = null;
- private Elevator elevator = null;
- private Climb climb = null;
- private Arm arm = null;
private Command auto = new DoNothing();
- // Dashboard inputs
- // private final LoggedDashboardChooser<Command> autoChooser;
-
// Controllers are defined here
private BaseDriverConfig driver = null;
- @SuppressWarnings("unused")
private Operator operator = null;
/**
default:
case SwerveCompetition:
- outtake = new OuttakeComp();
- elevator = new Elevator();
- climb = new Climb();
- arm = new Arm();
- // Arm can move if the elevator is within tolerance of its safe setpoint or higher
- arm.setElevatorStowed(() -> elevator.getPosition() < ElevatorConstants.SAFE_SETPOINT - 0.025);
- // Elevator can only move down if the arm is in the intake setpoint
- elevator.setArmStowed(() -> arm.canMoveElevator());
case BetaBot:
- indexer = new Indexer();
- intake = new Intake();
- //SmartDashboard.putData("commadn schedule", CommandScheduler.getInstance());
vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
// fall-through
- // SmartDashboard.putData("RunIntakeAndIndexer", new RunIntakeAndIndexer(intake, indexer));
case Vivace:
case Phil:
- if (robotId == RobotId.Phil) {
- outtake = new OuttakeAlpha();
- }
- if (outtake != null) {
- //SmartDashboard.putData("OuttakeCoralBasic", new OutakeMotors(intake, outtake));
- // SmartDashboard.putData("OuttakeCoralBasic", new OutakeMotors(intake, outtake));
- // SmartDashboard.putData("l4 outake", new ScoreL4(elevator, outtake));
- }
case Vertigo:
drive = new Drivetrain(vision, new GyroIOPigeon2());
- driver = new PS5ControllerDriverConfig(drive, elevator, intake, indexer, outtake, climb, arm);
- //operator = new Operator(drive, elevator, intake, indexer, outtake, climb);
+ driver = new PS5ControllerDriverConfig(drive);
+ operator = new Operator(drive);
// Detected objects need access to the drivetrain
DetectedObject.setDrive(drive);
- //SignalLogger.start();
+ // SignalLogger.start();
driver.configureControls();
- //operator.configureControls();
+ operator.configureControls();
initializeAutoBuilder();
registerCommands();
PathGroupLoader.loadPathGroups();
// Load the auto command
try {
- PathPlannerAuto.getPathGroupFromAutoFile("Left Side");
- auto = new PathPlannerAuto("Left Side");
+ PathPlannerAuto.getPathGroupFromAutoFile("Command Name");
+ auto = new PathPlannerAuto("Path Name");
} catch (IOException | ParseException e) {
- e.printStackTrace();
+ e.printStackTrace();
}
drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
break;
// This is really annoying so it's disabled
DriverStation.silenceJoystickConnectionWarning(true);
- //addPaths();
// TODO: verify this claim.
// LiveWindow is causing periodic loop overruns
LiveWindow.disableAllTelemetry();
}
public void registerCommands() {
- if(intake != null && indexer != null && elevator != null && arm != null){
- NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(intake, indexer, elevator, outtake, arm));
- NamedCommands.registerCommand("lower intake", new InstantCommand(() -> intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT)));
- }
- if(elevator != null && outtake != null && arm != null){
- NamedCommands.registerCommand("OuttakeCoral", new OuttakeCoral(outtake, elevator, arm).withTimeout(1.5));
- NamedCommands.registerCommand("L4",
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
- new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)
- )
- );
- NamedCommands.registerCommand("backdrive", new InstantCommand(() -> outtake.setMotor(0.02)));
-
- NamedCommands.registerCommand("Lower Elevator", new SequentialCommandGroup(
- new InstantCommand(()->arm.setSetpoint(ArmConstants.INTAKE_SETPOINT)),
- new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))
- ));
-
- NamedCommands.registerCommand("Score L4", new SequentialCommandGroup(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
- new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)
- ),
- new OuttakeCoral(outtake, elevator, arm)
- ));
-
- NamedCommands.registerCommand("Score L3", new SequentialCommandGroup(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
- ),
- new OuttakeCoral(outtake, elevator, arm)
- ));
- NamedCommands.registerCommand("Score L2", new SequentialCommandGroup(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
- ),
- new OuttakeCoral(outtake, elevator, arm)
- ));
-
- NamedCommands.registerCommand("L3",
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
- )
- );
- NamedCommands.registerCommand("L2",
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)
- )
- );
- NamedCommands.registerCommand("Station Setpoint",
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.STATION_INTAKE_SETPOINT),
- new MoveArm(arm, ArmConstants.STATION_INTAKE_SETPOINT),
- new StationIntake(outtake)
- )
- );
-
- //NamedCommands.registerCommand("L1", new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT));
-
- NamedCommands.registerCommand("Station Intake", new StationIntake(outtake));
-
- Pose2d blueStationRight = new Pose2d(1.722, 0.923, Rotation2d.fromDegrees(-36));
- Pose2d blueStationLeft = new Pose2d(blueStationRight.getX(), FieldConstants.FIELD_WIDTH-blueStationRight.getY(), Rotation2d.fromDegrees(-144));
-
- Pose2d blueStationIntakeLeft = new Pose2d(1.65, 7.4, Rotation2d.fromDegrees(-144-180));
- Pose2d blueStationIntakeRight = new Pose2d(1.526, 0.729, Rotation2d.fromDegrees(-144-180));
-
- Pose2d redStationRight = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationRight.getX(), blueStationLeft.getY(), blueStationRight.getRotation().plus(new Rotation2d(Math.PI)));
- Pose2d redStationLeft = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationLeft.getX(), blueStationRight.getY(), blueStationLeft.getRotation().plus(new Rotation2d(Math.PI)));
- NamedCommands.registerCommand("Drive To Left Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationLeft));
- NamedCommands.registerCommand("Drive To Right Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationIntakeRight));
- NamedCommands.registerCommand("Drive To Left Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationIntakeLeft));
-
- NamedCommands.registerCommand("Drive To Right Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationRight));
- NamedCommands.registerCommand("Drive To 6/19 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_LEFT.l4Pose).withTimeout(1));
- NamedCommands.registerCommand("Drive To 6/19 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_RIGHT.l4Pose).withTimeout(1));
- NamedCommands.registerCommand("Drive To 7/18 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_LEFT.l4Pose));
- NamedCommands.registerCommand("Drive To 7/18 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_RIGHT.l4Pose));
- NamedCommands.registerCommand("Drive To 10/21 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_RIGHT.l4Pose));
- NamedCommands.registerCommand("Drive To 11/20 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_11_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_20_LEFT.l4Pose));
- NamedCommands.registerCommand("Drive To 11/20 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_11_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_20_RIGHT.l4Pose));
- NamedCommands.registerCommand("Drive To 9/22 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get()
- == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_LEFT.l4Pose));
-
-
- NamedCommands.registerCommand("Drive To 8/17 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_LEFT.l4Pose).withTimeout(1));
- NamedCommands.registerCommand("Drive To 8/17 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_RIGHT.l4Pose).withTimeout(1));
- }
}
- // public void addPaths(){
- // try {
- // PathPlannerAuto.getPathGroupFromAutoFile("Left Side");
- // }
- // catch (IOException | ParseException e) {
- // e.printStackTrace();
- // }
- // autoChooser.addDefaultOption("Left Side", new PathPlannerAuto("Left Side"));
-
- // autoChooser.addOption("Station Right Side", new PathPlannerAuto("Station Right Side"));
- // autoChooser.addOption("Left Side Lollipop", new PathPlannerAuto("Left Side Lollipop"));
- // autoChooser.addOption("Left Side Ground", new PathPlannerAuto("Left Side Ground"));
-
- // if(intake != null && indexer != null && arm != null && elevator != null){
- // autoChooser.addOption("One peice blue",
- // new SequentialCommandGroup(
- // new InstantCommand(()->{
- // drive.resetOdometry(new Pose2d(7.229,4.191, Rotation2d.fromDegrees(90.0)));
- // intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT);
- // }),
- // new DriveToPose(drive, () -> VisionConstants.REEF.BLUE_BRANCH_21_RIGHT.pose).withTimeout(8),
- // new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
- // new MoveArm(arm, ArmConstants.L4_SETPOINT),
- // new OuttakeCoral(outtake, elevator, arm),
- // new SequentialCommandGroup(new WaitCommand(0.1),
- // new MoveArm(arm, ArmConstants.INTAKE_SETPOINT),
- // new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))),
- // new InstantCommand(()-> intake.stow())
- // ));
- // autoChooser.addOption("One peice red",
- // new SequentialCommandGroup(
- // new InstantCommand(()->{
- // drive.resetOdometry(new Pose2d(FieldConstants.FIELD_LENGTH-7.229,FieldConstants.FIELD_WIDTH-4.191, Rotation2d.fromDegrees(-90.0)));
- // intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT);
- // }),
- // new DriveToPose(drive, () -> VisionConstants.REEF.RED_BRANCH_10_RIGHT.pose).withTimeout(8),
- // new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
- // new MoveArm(arm, ArmConstants.L4_SETPOINT),
- // new OuttakeCoral(outtake, elevator, arm),
- // new SequentialCommandGroup(new WaitCommand(0.1),
- // new MoveArm(arm, ArmConstants.INTAKE_SETPOINT),
- // new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))),
- // new InstantCommand(()-> intake.stow())
- // ));
- // }
- // // autoChooser.addOption("#1", new FollowPathCommand("#1", true, drive)
- // // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT))
- // // .andThen(new OuttakeCoral(outtake, elevator, arm))
- // // .andThen(new FollowPathCommand("#2", true, drive))
- // // .andThen(new FollowPathCommand("#3", true, drive))
- // // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT))
- // // .andThen(new OuttakeCoral(outtake, elevator, arm))
- // // .andThen(new FollowPathCommand("#4", true, drive))
- // // .andThen(new FollowPathCommand("#5", true, drive))
- // // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT))
- // // .andThen(new OuttakeCoral(outtake, elevator, arm)));
-
-
- // if(elevator != null && outtake != null) {
- // autoChooser.addOption("WaitTest", new FollowPathCommand("Tester", true, drive)
- // .andThen(new OuttakeCoralBasic(outtake, ()->true, ()->false))
- // .andThen(new WaitCommand(3))
- // .andThen(new FollowPathCommand("Next Tester", true, drive))
- // );
-
- // autoChooser.addOption("Center to G", new FollowPathCommand("Center to G", true, drive)
- // .andThen(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT))
- // .andThen(new OuttakeCoral(outtake, elevator, arm)));
-
- // autoChooser.addOption("Center to H", new FollowPathCommand("Center to H", true, drive)
- // .andThen(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT))
- // .andThen(new OuttakeCoral(outtake, elevator, arm)));
- // }
- // }
-
public static BooleanSupplier getAllianceColorBooleanSupplier() {
return () -> {
// Boolean supplier that controls when the path will be mirrored for the red
return auto;
}
- // Logged psoitins of subsystems
- public double elevatorHeightLogged(){
- return elevator == null ? 0 : elevator.getPosition();
- }
- public double armAngleLogged(){
- return arm == null ? 0 : arm.getAngle() + 90;
- }
- public double intakeAngleLogged(){
- return intake == null ? -90 : -intake.getPivotAngle();
- }
- public double climbAngleLogged(){
- return climb == null ? 0 : climb.getEstimatedClimbAngle();
- }
-
public void logComponents(){
if(!Constants.LOG_MECHANISMS) return;
Logger.recordOutput(
"ComponentPoses",
new Pose3d[] {
- //intake
- new Pose3d(0,-0.25,0.27, new Rotation3d(Units.degreesToRadians(intakeAngleLogged()), 0.0, 0.0)),
- //climb
- new Pose3d(0,0,0, new Rotation3d(0.0,climbAngleLogged(), 0.0)),
- //arm
- new Pose3d(0,0.110, 0.388 + elevatorHeightLogged(), new Rotation3d(Units.degreesToRadians(armAngleLogged()), 0.0, 0.0)),
- //elevator 1
- new Pose3d(0,0,0, new Rotation3d(0.0, 0.0, 0.0)),
- //elevator 2
- new Pose3d(0,0, elevatorHeightLogged()/3, new Rotation3d(0.0, 0.0, 0.0)),
- //elevator 3
- new Pose3d(0,0, elevatorHeightLogged()*2/3, new Rotation3d(0.0, 0.0, 0.0)),
- //elevator 4
- new Pose3d(0,0, elevatorHeightLogged(), new Rotation3d(0.0, 0.0, 0.0)),
- //indexer
- new Pose3d(0,0,0, new Rotation3d(0.0, 0.0, 0.0)),
+ // Subsystem Pose3ds
}
);
}
}
}
- // report the RobotId to the SmartDashboard.
- //SmartDashboard.putString("RobotID", robotId.name());
-
// return the robot identity
return robotId;
}
/**
* Runs the given command when this command is initialized, and ends when it ends.
* Useful for commands that are not created yet because the constructor parameters are not available until initialization.
+ * This is very similar to WPILib's DeferredCommand
*/
public class SupplierCommand extends Command {
* Default drive command. Drives robot using driver controls.
*/
public class DefaultDriveCommand extends Command {
- private final Drivetrain swerve;
+ protected final Drivetrain swerve;
private final BaseDriverConfig driver;
public DefaultDriveCommand(
ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
+ drive(corrected);
+ }
+
+ /**
+ * Drives the robot
+ * @param speeds The ChassisSpeeds to drive at
+ */
+ protected void drive(ChassisSpeeds speeds){
// If the driver is pressing the align button or a command set the drivetrain to
// align, then align to speaker
if (driver.getIsAlign() || swerve.getIsAlign()) {
swerve.driveHeading(
- forwardTranslation,
- sideTranslation,
- swerve.getAlignAngle(),
- true);
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ swerve.getAlignAngle(),
+ true);
} else {
swerve.drive(
- corrected.vxMetersPerSecond,
- corrected.vyMetersPerSecond,
- corrected.omegaRadiansPerSecond,
- true,
- false);
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ speeds.omegaRadiansPerSecond,
+ true,
+ false);
}
}
}
-
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.IntakeConstants;
-import frc.robot.subsystems.intake.Intake;
-
-public class IntakeAlgae extends Command {
- private final Intake intake;
-
- public IntakeAlgae(Intake intake) {
- this.intake = intake;
- addRequirements(intake);
- }
-
- @Override
- public void initialize() {
- intake.setAngle(IntakeConstants.ALGAE_SETPOINT);
- intake.setSpeed(IntakeConstants.ALGAE_INTAKE_POWER);
- }
-
- @Override
- public void end(boolean interrupted) {
- intake.deactivate();
- intake.setSpeed(-0.05);
- }
-}
\ No newline at end of file
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.outtake.Outtake;
-
-public class IntakeAlgaeArm extends Command {
- private final Outtake outtake;
-
- public IntakeAlgaeArm(Outtake outtake) {
- this.outtake = outtake;
- addRequirements(outtake);
- }
-
- @Override
- public void initialize() {
- outtake.intakeAlgaeReef();
- }
-
- @Override
- public void end(boolean interrupted) {
- outtake.setMotor(-0.1);
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.constants.ArmConstants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
-
-/**
- * Intake a coral.
- */
-public class IntakeCoral extends SequentialCommandGroup {
- public IntakeCoral(Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm) {
- addCommands(
- new ParallelCommandGroup(
- new InstantCommand(() -> {
- intake.unstow();
- elevator.setSetpoint(ElevatorConstants.INTAKE_SETPOINT);
- arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);}),
- new IntakeCoralHelper(intake, indexer, outtake, arm, elevator)));
- }
-
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.ArmConstants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
-
-/**
- * All the intake code except the elevator stuff.
- * Don't call this directly; use {@link frc.robot.commands.gpm.IntakeCoral}
- * instead.
- */
-public class IntakeCoralHelper extends Command {
- private Intake intake;
- private Indexer indexer;
- private Outtake outtake;
- private Arm arm;
- private Elevator elevator;
-
- private enum Phase {
- Acquiring, Intaking, Indexing, Detected, InOuttake, Done
- };
-
- private Phase phase;
-
- public IntakeCoralHelper(Intake intake, Indexer indexer, Outtake outtake, Arm arm, Elevator elevator) {
- this.intake = intake;
- this.indexer = indexer;
- this.outtake = outtake;
- this.arm = arm;
- this.elevator = elevator;
- addRequirements(intake, indexer, arm, elevator);
- if(outtake != null){
- addRequirements(outtake);
- }
- }
-
- @Override
- public void initialize() {
- intake.activate();
- intake.unstow();
- indexer.run();
- phase = Phase.Acquiring;
- if(outtake != null) {
- outtake.setMotor(0.7);
- }
- }
-
- @Override
- public void execute() {
- if(outtake != null && outtake.coralLoaded()){
- phase = Phase.InOuttake;
- }
- switch (phase) {
- case Acquiring:
- case Intaking:
- case Indexing:
- if (!indexer.isIndexerClear()) {
- phase = Phase.Detected;
- intake.setSpeed(0.1);
- indexer.slow();
- }
- break;
- case Detected:
- if(indexer.isIndexerClear()){
- phase = Phase.InOuttake;
- intake.deactivate();
- intake.stow();
- }
- break;
- case InOuttake:
- if(outtake == null || outtake.coralLoaded()){
- phase = Phase.Done;
- elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT);
- arm.setSetpoint(ArmConstants.STOW_SETPOINT);
- indexer.stop();
- if(outtake != null){
- outtake.setMotor(0.1);
- }
- }
- break;
- case Done:
- break;
- }
- }
-
- @Override
- public boolean isFinished() {
- return phase == Phase.Done && elevator.getPosition() > ElevatorConstants.SAFE_SETPOINT-0.025;
- }
-
- @Override
- public void end(boolean interrupted) {
- // in case it's interrupted
- intake.deactivate();
- intake.stow();
- indexer.stop();
- if(outtake != null){
- outtake.setMotor(0.02);
- }
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.arm.Arm;
-
-public class MoveArm extends Command {
- private Arm arm;
- private double setpoint;
-
- public MoveArm(Arm arm, double setpoint) {
- this.arm = arm;
- this.setpoint = setpoint;
- addRequirements(arm);
- }
-
- @Override
- public void initialize() {
- arm.setSetpoint(setpoint);
- }
-
- @Override
- public boolean isFinished() {
- return arm.atSetpoint();
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.elevator.Elevator;
-
-/**
- * Moves the elevator to a position
- */
-public class MoveElevator extends Command {
- private Elevator elevator;
- private double setpoint;
-
- /**
- * Creates a command to move the elevator to a position
- *
- * @param elevator The elevator subsystem
- * @param setpoint The setpoint to move to
- */
- public MoveElevator(Elevator elevator, double setpoint) {
- this.elevator = elevator;
- this.setpoint = setpoint;
- if(elevator != null){
- addRequirements(elevator);
- }
- }
-
- /**
- * Sets the elevator setpoint
- */
- @Override
- public void initialize() {
- if(elevator != null){
- elevator.setSetpoint(setpoint);
- }
- }
-
- /**
- * Returns whether the elevator is at the setpoint
- *
- * @return True if the elevator is within about 1 inch of the setpoint, false
- * otherwise
- */
- @Override
- public boolean isFinished() {
- return elevator == null || elevator.atSetpoint();
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.constants.ArmConstants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.elevator.Elevator;
-
-public class NetSetpoint extends SequentialCommandGroup {
- public NetSetpoint(Elevator elevator, Arm arm, Drivetrain drive){
- this(true, elevator, arm, drive);
- }
- public NetSetpoint(boolean chooseClosestSide, Elevator elevator, Arm arm, Drivetrain drive){
- addCommands(
- // new InstantCommand(()->{
- // if(chooseClosestSide){
- // drive.setAlignAngle(
- // useSetpoint1(drive, chooseClosestSide) == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2)
- // ? Math.PI / 2 : -Math.PI / 2
- // );
- // drive.setIsAlign(false);
- // }
- // }),
- new ParallelCommandGroup(
- new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT),
- new MoveElevator(elevator, ElevatorConstants.NET_SETPOINT)),
- new ConditionalCommand(
- new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_1),
- new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_2),
- ()->useSetpoint1(drive, chooseClosestSide))
- );
- }
- private static boolean useSetpoint1(Drivetrain drive, boolean chooseClosestSide){
- boolean positive = MathUtil.angleModulus(drive.getYaw().getRadians()) > 0;
- return !chooseClosestSide || positive == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2);
- }
-}
+++ /dev/null
-// 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.gpm;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
-
-// NOTE: Consider using this command inline, rather than writing a subclass. For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class OutakeMotors extends SequentialCommandGroup {
- /** Creates a new OutakeMotors. */
- public OutakeMotors(Intake intake, Outtake outtake) {
- // Add your commands in the addCommands() call, e.g.
- // addCommands(new FooCommand(), new BarCommand());
- addCommands(
- new InstantCommand(()->outtake.outtake(), outtake),
- new WaitCommand(0.5),
- new InstantCommand(()->outtake.stop(), outtake)
- );
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
-import frc.robot.constants.IntakeConstants;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
-
-public class OuttakeAlgae extends ConditionalCommand {
- public OuttakeAlgae(Outtake outtake, Intake intake){
- super(
- new OuttakeAlgaeArm(outtake),
- new OuttakeAlgaeIntake(intake),
- ()-> intake.isAtSetpoint(IntakeConstants.STOW_SETPOINT)
- );
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.outtake.Outtake;
-
-public class OuttakeAlgaeArm extends Command{
- private final Outtake outtake;
-
- private final Timer timer = new Timer();
- private final double EJECTION_TIME = 0.25;
-
- public OuttakeAlgaeArm(Outtake outtake) {
- this.outtake = outtake;
- addRequirements(outtake);
- }
-
- @Override
- public void initialize() {
- outtake.outtakeAlgae();
- timer.restart();
- }
-
- @Override
- public boolean isFinished() {
- return timer.hasElapsed(EJECTION_TIME);
- }
-
- @Override
- public void end(boolean interrupted) {
- outtake.stop();
- timer.stop();
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.IntakeConstants;
-import frc.robot.subsystems.intake.Intake;
-
-public class OuttakeAlgaeIntake extends Command {
- private Intake intake;
-
- private final Timer timer = new Timer();
- private final double EJECTION_TIME = 1;
-
- public OuttakeAlgaeIntake(Intake intake) {
- this.intake = intake;
- addRequirements(intake);
- }
-
- @Override
- public void initialize() {
- intake.setSpeed(IntakeConstants.ALGAE_OUTTAKE_POWER);
- intake.setAngle(50);
- timer.restart();
- }
-
- @Override
- public boolean isFinished() {
- return timer.hasElapsed(EJECTION_TIME);
- }
-
- @Override
- public void end(boolean interrupted) {
- intake.deactivate();
- intake.stow();
- timer.stop();
- }
-
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import java.util.function.BooleanSupplier;
-
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.outtake.Outtake;
-
-public class OuttakeCoral extends SequentialCommandGroup {
- public OuttakeCoral(Outtake outtake, Elevator elevator, Arm arm){
- BooleanSupplier l4Supplier = () -> elevator.getSetpoint() > ElevatorConstants.L3_SETPOINT + 0.001;
- BooleanSupplier l1Supplier = () -> elevator.getSetpoint() < ElevatorConstants.L1_SETPOINT + 0.001;
- addCommands(
- new ConditionalCommand(
- new ScoreL4(outtake, arm),
- new OuttakeCoralBasic(outtake, l4Supplier, l1Supplier),
- l4Supplier)
- //new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT), elevator)
- );
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-
-import java.util.function.BooleanSupplier;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.outtake.Outtake;
-
-
-/**
- * Command to eject coral.
- * Wants coral to be present.
- */
-public class OuttakeCoralBasic extends Command {
- public static final double L1_SPEED = -0.2;
- public static final double L4_SPEED = -0.8;
- public static final double OUTTAKE_SPEED = -0.45;
-
- private Outtake outtake;
-
- // counter to measure time in ticks (every 20 milliseconds);
- private Timer timer = new Timer();
-
- private BooleanSupplier l4Supplier;
- private BooleanSupplier l1Supplier;
-
- public OuttakeCoralBasic(Outtake outtake, BooleanSupplier l4Supplier, BooleanSupplier l1Supplier){
- this.outtake = outtake;
- this.l4Supplier = l4Supplier;
- this.l1Supplier = l1Supplier;
- addRequirements(outtake);
- }
-
-
- @Override
- public void initialize(){
- timer.restart();
- boolean l4 = l4Supplier.getAsBoolean();
- boolean l1 = !l4 && l1Supplier.getAsBoolean();
- outtake.setMotor(l4 ? L4_SPEED : l1 ? L1_SPEED : OUTTAKE_SPEED);
- }
-
- public boolean isFinished(){
- return timer.hasElapsed(0.25);
- }
-
-
- public void end(boolean interrupted){
- outtake.stop();
- }
-}
-
-
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.outtake.Outtake;
-
-public class RemoveAlgae extends Command {
- private Outtake outtake;
- public RemoveAlgae(Outtake outtake){
- this.outtake = outtake;
- addRequirements(outtake);
- }
-
- @Override
- public void initialize(){
- outtake.removeAlgae();
- }
-
- @Override
- public void end(boolean interrupted){
- outtake.stop();
- }
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.climb.Climb;
-
-public class ResetClimb extends Command {
- private Climb climb;
-
- public ResetClimb(Climb climb){
- this.climb = climb;
- addRequirements(climb);
- }
-
- @Override
- public void initialize(){
- climb.reset(true);
- }
- @Override
- public void end(boolean interrupted){
- climb.reset(false);
- }
-
-}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
-
-public class ReverseMotors extends Command {
- private Intake intake;
- private Indexer indexer;
- private Outtake outtake;
-
- public ReverseMotors(Intake intake, Indexer indexer, Outtake outtake) {
- this.intake = intake;
- this.indexer = indexer;
- this.outtake = outtake;
- addRequirements(intake, indexer);
- if(outtake != null){
- addRequirements(outtake);
- }
- }
-
- @Override
- public void initialize() {
- intake.setSpeed(-.5);
- indexer.reverse();
- if(outtake != null){
- outtake.reverse();
- }
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
- @Override
- public void end(boolean interrupted) {
- intake.deactivate();
- indexer.stop();
- if(outtake != null){
- outtake.stop();;
- }
- }
-}
+++ /dev/null
-// 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.gpm;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-
-// NOTE: Consider using this command inline, rather than writing a subclass. For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class RunIntakeAndIndexer extends SequentialCommandGroup {
- /** Creates a new RunIntakeAndIndexer. */
- public RunIntakeAndIndexer(Intake intake, Indexer indexer ) {
- // Add your commands in the addCommands() call, e.g.
- // addCommands(new FooCommand(), new BarCommand());
- addCommands(
- new InstantCommand(() -> intake.activate()),
- new InstantCommand(() -> indexer.run()),
- new WaitCommand(3),
- new InstantCommand(() -> intake.deactivate()),
- new InstantCommand(() -> indexer.stop())
- );
- }
-}
+++ /dev/null
-// 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.gpm;
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.outtake.Outtake;
-
-// NOTE: Consider using this command inline, rather than writing a subclass. For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class ScoreL4 extends SequentialCommandGroup {
- /** Creates a new ScoreL4. */
- public ScoreL4(Outtake outtake, Arm arm) {
- addCommands(
- new OuttakeCoralBasic(outtake, ()->true, ()->false)
- );
- }
-}
-
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.outtake.Outtake;
-
-public class StationIntake extends Command {
- private Outtake outtake;
-
- public StationIntake(Outtake outtake) {
- this.outtake = outtake;
- addRequirements(outtake);
- }
-
- @Override
- public void initialize() {
- outtake.setMotor(0.7);
- }
-
- @Override
- public boolean isFinished() {
- return outtake.coralLoaded();
- }
-
- @Override
- public void end(boolean interrupted) {
- outtake.setMotor(0.02);
- }
-
-
-}
+++ /dev/null
-package frc.robot.commands.test_comm;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.TestConstants;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-
-/**
- * Tests the odometry of the robot by driving a certain distance and calculating the error.
- */
-public class PoseTransform extends Command {
-
- private final Drivetrain drive;
-
- private double startTime;
- private Pose2d finalPose;
- private final Transform2d distanceToMove;
- private Pose2d error;
-
- public PoseTransform(Drivetrain drive, Transform2d poseTransform) {
- this.drive = drive;
- // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove
- distanceToMove = poseTransform;
-
- addRequirements(drive);
- }
-
- @Override
- public void initialize() {
- startTime = Timer.getFPGATimestamp();
- finalPose = drive.getPose().transformBy(distanceToMove);
- }
-
- @Override
- public void execute() {
- drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians());
- }
-
- @Override
- public boolean isFinished() {
- double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR;
- double errorMarginRadians = Units.degreesToRadians(10);
- error = drive.getPose().relativeTo(finalPose);
- // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is
- return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians;
- }
-
- @Override
- public void end(boolean interrupted) {
- drive.stop();
- System.out.println(Timer.getFPGATimestamp() - startTime);
- System.out.println(error.getX());
- System.out.println(error.getY());
- System.out.println(error.getRotation().getRadians());
- }
-}
\ No newline at end of file
+++ /dev/null
-package frc.robot.commands.test_comm;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.TestConstants;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-
-/**
- * Tests the odometry of the robot by driving a certain distance and calculating the error.
- */
-public class PoseTransformTest extends Command {
-
- private final Drivetrain drive;
-
- private double startTime;
- private Pose2d finalPose;
- private Pose2d error;
- private double x;
- private double y;
- private double rotation;
-
- public PoseTransformTest(Drivetrain drive, double x, double y, double rotation) {
- this.drive = drive;
- // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove
- this.x =x;
- this.y =y;
- this.rotation = rotation;
- addRequirements(drive);
- }
-
- @Override
- public void initialize() {
- startTime = Timer.getFPGATimestamp();
- finalPose = drive.getPose().transformBy(new Transform2d(
- new Translation2d(x, y),
- Rotation2d.fromDegrees(rotation)
- ));
- }
-
- @Override
- public void execute() {
- drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians());
- }
-
- @Override
- public boolean isFinished() {
- double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR;
- double errorMarginRadians = Units.degreesToRadians(10);
- error = drive.getPose().relativeTo(finalPose);
- // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is
- return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians;
- }
-
- @Override
- public void end(boolean interrupted) {
- drive.stop();
- System.out.println(Timer.getFPGATimestamp() - startTime);
- System.out.println(error.getX());
- System.out.println(error.getY());
- System.out.println(error.getRotation().getRadians());
- }
-}
\ No newline at end of file
import java.util.function.Supplier;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.commands.gpm.IntakeCoral;
-import frc.robot.subsystems.arm.Arm;
+import frc.robot.commands.DoNothing;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
import frc.robot.util.Vision.DetectedObject;
public class AcquireGamePiece extends SequentialCommandGroup {
*
* @param gamePiece The supplier for the game piece to intake
* @param drive The drivetrain
- * @param intake The intake
- * @param index The indexer
- * @param arm The arm
*/
- public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive, Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm){
- addCommands(new IntakeCoral(intake, indexer, elevator, outtake, arm).deadlineFor(new DriveToCoral(gamePiece, drive)));
+ public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive){
+ // TODO: Replace DoNothing with next year's intake command
+ addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive)));
}
}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.commands.drive_comm.DefaultDriveCommand;
+import frc.robot.constants.VisionConstants;
+import frc.robot.controls.BaseDriverConfig;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+public class AimAtGamePiece extends DefaultDriveCommand {
+ private Supplier<DetectedObject> objectSupplier;
+ private static int ticksSinceLastObject;
+ private static DetectedObject cachedObject;
+
+
+ public AimAtGamePiece(Drivetrain drive, BaseDriverConfig driver, Supplier<DetectedObject> objectSupplier){
+ super(drive, driver);
+ this.objectSupplier = objectSupplier;
+ }
+
+ @Override
+ public void initialize() {
+ cachedObject = null;
+ ticksSinceLastObject = 0;
+ super.initialize();
+ }
+
+ @Override
+ protected void drive(ChassisSpeeds speeds){
+ if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ super.drive(speeds);
+ return;
+ }
+ DetectedObject object = objectSupplier.get();
+
+ if(object == null || !object.isGamePiece()) {
+ if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
+ object = cachedObject;
+ } else {
+ super.drive(speeds);
+ return;
+ }
+ ticksSinceLastObject++;
+ } else {
+ ticksSinceLastObject = 0;
+ cachedObject = object;
+ }
+
+ // System.out.println("objangle " + object.getAngle());
+ swerve.driveHeading(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ MathUtil.angleModulus(object.getAngle()),
+ true);
+ }
+}
+++ /dev/null
-package frc.robot.commands.vision;
-
-import java.util.function.Supplier;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import frc.robot.commands.drive_comm.DriveToPose;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.util.Vision.DetectedObject;
-
-/**
- * Moves toward the detected object
- * <p>Only works with the front camera
- */
-public class DriveToCoral extends DriveToPose {
- private static final boolean constantUpdate = false;
-
- private static Pose2d tempPose;
-
- private Supplier<DetectedObject> objectSupplier;
-
- /**
- * Moves toward the detected object
- * @param detectedObject The supplier for the detected object to use
- * @param drive The drivetrain
- */
- public DriveToCoral(Supplier<DetectedObject> detectedObject, Drivetrain drive) {
- super(drive,
- constantUpdate
- ? () -> getPose(detectedObject)
- : () -> tempPose);
- objectSupplier = detectedObject;
- updateTarget = constantUpdate;
- }
-
- public static Pose2d getPose(Supplier<DetectedObject> supplier){
- DetectedObject object = supplier.get();
- if(object == null) return null;
- return new Pose2d(object.pose.toPose2d().getTranslation(), new Rotation2d(object.getAngle()+Math.PI/2));
- }
-
- @Override
- public void initialize(){
- // Set the static variable so the super class has access to it
- if(constantUpdate){
- tempPose = getPose(objectSupplier);
- }
- super.initialize();
- }
-
- @Override
- public void execute(){
- // Set the static variable so the super class has access to it
- if(constantUpdate){
- tempPose = getPose(objectSupplier);
- }
- super.execute();
- }
-}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.constants.VisionConstants;
+import frc.robot.constants.swerve.DriveConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.Vision.DetectedObject;
+
+/**
+ * Moves toward the detected object
+ */
+public class DriveToGamePiece extends DriveToPose {
+ private static boolean constantUpdate = true;
+ private static int ticksSinceLastObject;
+ private static DetectedObject cachedObject;
+
+ /**
+ * Moves toward the detected object
+ * @param detectedObject The supplier for the detected object to use
+ * @param drive The drivetrain
+ */
+ public DriveToGamePiece(Supplier<DetectedObject> detectedObject, Drivetrain drive) {
+ super(drive,
+ () -> getPose(detectedObject, drive)
+ );
+ updateTarget = constantUpdate;
+ }
+
+ @Override
+ public void initialize() {
+ cachedObject = null;
+ ticksSinceLastObject = 0;
+ super.initialize();
+ }
+
+ public static Pose2d getPose(Supplier<DetectedObject> supplier, Drivetrain drive){
+ DetectedObject object = supplier.get();
+ if(object == null || !object.isGamePiece()) {
+ if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
+ object = cachedObject;
+ } else {
+ return null;
+ }
+ ticksSinceLastObject++;
+ } else {
+ ticksSinceLastObject = 0;
+ cachedObject = object;
+ }
+ Rotation2d rotation = new Rotation2d(MathUtil.angleModulus(object.getAngle()));
+ Translation2d objectTranslation = object.pose.toPose2d().getTranslation();
+ Translation2d diff = objectTranslation.minus(drive.getPose().getTranslation());
+ Translation2d translation = objectTranslation.minus(diff.times(DriveConstants.ROBOT_WIDTH_WITH_BUMPERS/2/diff.getNorm()));
+ return new Pose2d(translation, rotation);
+ }
+}
\ No newline at end of file
--- /dev/null
+package frc.robot.commands.vision;
+
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.util.Vision.DetectedObject;
+
+public class LogVision extends Command {
+ private Supplier<DetectedObject> objectSupplier;
+ public LogVision(Supplier<DetectedObject> objectSupplier){
+ this.objectSupplier = objectSupplier;
+ }
+
+ @Override
+ public void execute() {
+ DetectedObject object = this.objectSupplier.get();
+ if (object != null) {
+ Logger.recordOutput("Vision/object_angle", object.getAngle());
+ Logger.recordOutput("Vision/object_distance", object.getDistance());
+ }
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+
+}
+++ /dev/null
-package frc.robot.constants;
-
-import edu.wpi.first.math.system.plant.DCMotor;
-
-public class ArmConstants {
- // Degrees
- public static final double START_ANGLE = -86.5;
- public static final double MIN_ANGLE = -86.5;
- public static final double MAX_ANGLE = 180;
- public static final double OFFSET = 0 - START_ANGLE;
-
- public static final double GEAR_RATIO = 29.36;
- public static final double ENCODER_GEAR_RATIO = 50.0/24.0;
- public static final DCMotor MOTOR = DCMotor.getKrakenX60(1);
-
- public static final double MASS = 2.46; // kilograms
- public static final double LENGTH = 0.138*2; // meters
- public static final double MOI = 0.0261057394; // kg*m^2
- public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters
-
- public static final double MAX_VELOCITY = 21; // rad/s
- public static final double MAX_ACCELERATION = 120; // rad/s^2
-
- public static final double INTAKE_SETPOINT = START_ANGLE;
- public static final double STATION_INTAKE_SETPOINT = 75.5;
-
- public static final double TOLERANCE = 3.0;
-
- //Dunk L4 = 6.4
- public static final double L1_SETPOINT = 50;
-
- //4 in offset
- // public static final double L4_SETPOINT = 11;
- // Original L4: 7.5 degrees
- public static final double L4_SETPOINT_RIGHT = 7.5;
- public static final double L4_SETPOINT_LEFT = 6;
- public static final double L2_L3_SETPOINT = 21.25;
-
- //touching reef
- public static final double L4_SETPOINT_ALT = 4.5;
- public static final double L2_L3_SETPOINT_ALT = 12.23;
-
- public static final double ALGAE_SETPOINT = -19.37;
- public static final double ALGAE_NET_SETPOINT_1 = 85.0;
- public static final double ALGAE_NET_SETPOINT_2 = 25;
- public static final double ALGAE_STOW_SETPOINT = 50;
- public static final double ALGAE_LOLI_SETPOINT = -19;
-
- public static final double PROCESSOR_SETPOINT = -65.0;
-
- public static final double STOW_SETPOINT = -14.0;
-}
public static final double HEADING_SLEWRATE = 10;
//Modes
- public static final Mode SIM_MODE = Mode.REPLAY;
+ public static final Mode SIM_MODE = Mode.SIM;
public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
// Enables 3D logs of mechanisms
+++ /dev/null
-// 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.constants;
-
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.util.Units;
-
-/** Stores constants for the elevator. */
-public class ElevatorConstants {
- public static final double ANGLE = Units.degreesToRadians(3.5); // radians, from vertical
- public static final DCMotor MOTOR = DCMotor.getKrakenX60(1);
- public static final int NUMBER_OF_STAGES = 3;
- public static final double GEARING = 8.333/NUMBER_OF_STAGES;
- public static final double MIN_HEIGHT = 0.0; // meters
- public static final double MAX_HEIGHT = Units.inchesToMeters(66);//Units.inchesToMeters(48); // meters
- public static final double START_HEIGHT = MIN_HEIGHT; // meters
- public static final double CARRIAGE_MASS = 3; // kilograms 2.49475803
- public static final double DRUM_RADIUS = Units.inchesToMeters(1.281/2); // meters
- public static final double SPRING_FORCE = 0; //Newtons
-
- public static final double BOTTOM_LIMIT_SWITCH_HEIGHT = 0;//0.015; // meters
- public static final double TOP_LIMIT_SWITCH_HEIGHT = MAX_HEIGHT; // meters
- public static final double SIM_LIMIT_SWITCH_TRIGGER_DISTANCE = 0.01; // meters
-
- public static final double STOW_SETPOINT = 0;
- public static final double INTAKE_SETPOINT = 0.036;
- public static final double SAFE_SETPOINT = 0.225;
- public static final double INTAKE_STOW_SETPOINT = 0.58;
-
- //4 inch offset
- public static final double L1_SETPOINT = 0.0;
- public static final double L2_SETPOINT = 0.523;
- public static final double L3_SETPOINT = 0.9192;
- // public static final double L4_SETPOINT = 1.675;
- public static final double L4_SETPOINT = 1.675;
-
- //touching reef
- public static final double L1_SETPOINT_ALT = 0.27;
- public static final double L2_SETPOINT_ALT = 0.588;
- public static final double L3_SETPOINT_ALT = 0.98-0.0254-0.01;
- public static final double L4_SETPOINT_ALT = 1.675;
- //Dunk L4 = 1.5
-
- public static final double BOTTOM_ALGAE_SETPOINT = 0.405;
- public static final double TOP_ALGAE_SETPOINT = 0.799;
-
- public static final double NET_SETPOINT = MAX_HEIGHT;
- public static final double STATION_INTAKE_SETPOINT = 0.233;
-
-
- public static final double CENTER_OF_MASS_HEIGHT_STOWED = Units.inchesToMeters(9.44);
- public static final double CENTER_OF_MASS_HEIGHT_EXTENDED = Units.inchesToMeters(10+14.767);
-
-
- // The x distance from the center of the robot to the outtake.
- public static final double OUTTAKE_X = Units.inchesToMeters(-7.25);
-}
package frc.robot.constants;
-import edu.wpi.first.wpilibj.I2C;
-
public class IdConstants {
// Drivetrain
public static final int DRIVE_FRONT_LEFT_ID = 1;
// LEDs
public static final int CANDLE_ID = 1;
-
- // Elevator
- public static final int ELEVATOR_RIGHT_MOTOR = 50;
- public static final int ELEVATOR_BOTTOM_LIMIT_SWITCH = 29;
- public static final int ELEVATOR_TOP_LIMIT_SWITCH = 30;
-
- // Indexer
- public static final int INDEXER_MOTOR = 56;
- public static final int INDEXER_SENSOR = 24;
-
- // Climb
- public static final int CLIMB_MOTOR = 31;
-
- // Intake
- public static final int INTAKE_ROLLER = 51;
- public static final int INTAKE_PIVOT = 55; //55
- public static final int INTAKE_LASER_CAN = 25;
-
- // Outtake
- public static final int OUTTAKE_MOTOR_ALPHA = 14;
- public static final int OUTTAKE_MOTOR_COMP = 30;
- public static final int OUTTAKE_DIO_EJECTING = 3;
- public static final I2C.Port i2cPort = I2C.Port.kMXP;
-
- //Arm
- public static final int ARM_MOTOR = 29;
- public static final int ARM_ABSOLUTE_ENCODER = 5;
}
+++ /dev/null
-package frc.robot.constants;
-
-public class IndexerConstants {
- public static final double SPEED = 1;
-
- public static final double MOMENT_OF_INERTIA = 0.000326;
- public static final double GEAR_RATIO = 1.0;
- public static final int MEASUREMENT_THRESHOLD = 15; // in millimeters
-
- // this stuff is for sim, all in meters
- public static final double WHEEL_CIRCUMFERENCE = Math.PI * 0.1;
- public static final double START_SIM_POS_AT = -1.0; // start coral here
- public static final double START_SIM_SENSOR_POS_AT = -0.5; // activate the sensor here
- public static final double END_SIM_SENSOR_POS_AT = 0.5; // deactivate the sensor here
-}
-
+++ /dev/null
-package frc.robot.constants;
-
-/**
- * Storage class for intake constants
- */
-public class IntakeConstants {
- public static final double PIVOT_GEAR_RATIO = 75;
- public static final double MOMENT_OFiNERTIA = 0.600984136;
- public static final double CENTER_OF_MASS_DIST = 0.265;
- public static final double MASS = 6.45688739;
- public static final double ARM_LENGTH = CENTER_OF_MASS_DIST*2;
- public static final double DETECT_CORAL_DIST = 0.49;
- public static final double INTAKE_MOTOR_POWER = 1;
- public static final double INTAKE_SETPOINT = -30.41;
- public static final double STOW_SETPOINT = 90;
- public static final double STATION_SETPOINT = 70;
- public static final double INTAKE_SAFE_POINT = 30;
-
- public static final double ALGAE_SETPOINT = 25;
- public static final double ALGAE_INTAKE_POWER = -0.5;
- public static final double ALGAE_OUTTAKE_POWER = 0.5;
-
-}
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-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.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
-import frc.robot.constants.swerve.DriveConstants;
/**
* Container class for vision constants.
*/
public static final double HIGHEST_AMBIGUITY = 0.01;
+ public static final int MAX_EMPTY_TICKS = 10;
+
/**
* The camera poses
* <p>
new Transform3d(
new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)),
new Rotation3d(0, Units.degreesToRadians(20), 0))));
-
- // Poses to potentially align to
- public static final Pose2d RED_PROCESSOR_POSE = new Pose2d(
- FieldConstants.APRIL_TAGS.get(2).pose.getX(),
- FieldConstants.APRIL_TAGS.get(2).pose.getY() - DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2,
- new Rotation2d(Math.PI / 2));
-
- public static final Pose2d BLUE_PROCESSOR_POSE = new Pose2d(
- FieldConstants.APRIL_TAGS.get(15).pose.getX(),
- FieldConstants.APRIL_TAGS.get(15).pose.getY() + DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2,
- new Rotation2d(-Math.PI / 2));
-
- public static final Pose2d BLUE_CAGE_LEFT_POSE = new Pose2d(
- FieldConstants.FIELD_LENGTH / 2,
- FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(1019.0 / 8),
- new Rotation2d());
-
- public static final Pose2d BLUE_CAGE_MIDDLE_POSE = new Pose2d(
- FieldConstants.FIELD_LENGTH / 2,
- FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(675.0 / 8),
- new Rotation2d());
-
- public static final Pose2d BLUE_CAGE_RIGHT_POSE = new Pose2d(
- FieldConstants.FIELD_LENGTH / 2,
- FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(41.5),
- new Rotation2d());
-
- public static final Pose2d RED_CAGE_RIGHT_POSE = new Pose2d(
- FieldConstants.FIELD_LENGTH / 2,
- FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(-41.5),
- new Rotation2d(Math.PI));
-
- public static final Pose2d RED_CAGE_MIDDLE_POSE = new Pose2d(
- FieldConstants.FIELD_LENGTH / 2,
- FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-675.0 / 8),
- new Rotation2d(Math.PI));
-
- public static final Pose2d RED_CAGE_LEFT_POSE = new Pose2d(
- FieldConstants.FIELD_LENGTH / 2,
- FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-1019.0 / 8),
- new Rotation2d(Math.PI));
-
- /**
- * The distance between the AprilTag and algae setpoint in the directin parallel to the face of the reef
- * Positive is to the left
- */
- public static final double ALGAE_X_OFFSET = 0.02;
-
- /**
- * Stores all of the alignment poses for both reefs.
- * Each branch is in the format (RED or BLUE)_BRANCH_(tag ID)_(LEFT or RIGHT)
- * Each algae setpoint is in the form (RED or BLUE)_ALGAE_(tag ID)
- * Left and right refer to the position of the branch when looking directly at the AprilTag
- */
- public enum REEF {
- RED_BRANCH_6_LEFT(5, 0.1651, .0587),
- RED_BRANCH_6_RIGHT(5, -0.1651, .0587),
- RED_BRANCH_7_LEFT(6, 0.1651, .0587),
- RED_BRANCH_7_RIGHT(6, -0.1651, .0587),
- RED_BRANCH_8_LEFT(7, 0.1651, .0587),
- RED_BRANCH_8_RIGHT(7, -0.1651, .0587),
- RED_BRANCH_9_LEFT(8, 0.1651, .0587),
- RED_BRANCH_9_RIGHT(8, -0.1651, .0587),
- RED_BRANCH_10_LEFT(9, 0.1651, .0587),
- RED_BRANCH_10_RIGHT(9, -0.1651, .0587),
- RED_BRANCH_11_LEFT(10, 0.1651, .0587),
- RED_BRANCH_11_RIGHT(10, -0.1651, .0587),
-
- BLUE_BRANCH_17_LEFT(16, 0.1651, .0587),
- BLUE_BRANCH_17_RIGHT(16, -0.1651, .0587),
- BLUE_BRANCH_18_LEFT(17, 0.1651, .0587),
- BLUE_BRANCH_18_RIGHT(17, -0.1651, .0587),
- BLUE_BRANCH_19_LEFT(18, 0.1651, .0587),
- BLUE_BRANCH_19_RIGHT(18, -0.1651, .0587),
- BLUE_BRANCH_20_LEFT(19, 0.1651, .0587),
- BLUE_BRANCH_20_RIGHT(19, -0.1651, .0587),
- BLUE_BRANCH_21_LEFT(20, 0.1651, .0587),
- BLUE_BRANCH_21_RIGHT(20, -0.1651, .0587),
- BLUE_BRANCH_22_LEFT(21, 0.1651, .0587),
- BLUE_BRANCH_22_RIGHT(21, -0.1651, .0587),
-
- RED_ALGAE_6(5, ALGAE_X_OFFSET, 0, true),
- RED_ALGAE_7(6, ALGAE_X_OFFSET, 0, true),
- RED_ALGAE_8(7, ALGAE_X_OFFSET, 0, true),
- RED_ALGAE_9(8, ALGAE_X_OFFSET, 0, true),
- RED_ALGAE_10(9, ALGAE_X_OFFSET, 0, true),
- RED_ALGAE_11(10, ALGAE_X_OFFSET, 0, true),
- BLUE_ALGAE_17(16, ALGAE_X_OFFSET, 0, true),
- BLUE_ALGAE_18(17, ALGAE_X_OFFSET, 0, true),
- BLUE_ALGAE_19(18, ALGAE_X_OFFSET, 0, true),
- BLUE_ALGAE_20(19, ALGAE_X_OFFSET, 0, true),
- BLUE_ALGAE_21(20, ALGAE_X_OFFSET, 0, true),
- BLUE_ALGAE_22(21, ALGAE_X_OFFSET, 0, true);
-
- /**
- * The pose to align to for scoring on this branch or intaking this algae
- */
- public final Pose2d pose;
- /**
- * The pose to align to for scoring on L4, null for algae
- */
- public final Pose2d l4Pose;
- /**
- * The pose to align to for scoring on L1, null for algae
- */
- public final Pose2d l1Pose;
- /**
- * The ID of the AprilTag on the smae side of hte reef ast his branch
- */
- public final int aprilTagId;
- private final int aprilTagIndex;
- /**
- * The horizontal (parallel to the closest face of the reef) distance, in meters, between the AprilTag and branch
- */
- public final double xOffset;
- /**
- * The y (normal to the closest face of the reef) distance, in meters, between the AprilTag and branch
- */
- public final double yOffset;
-
- public final boolean isAlgae;
-
- private REEF(int aprilTagIndex, double xOffset, double yOffset) {
- this(aprilTagIndex, xOffset, yOffset, false);
- }
- private REEF(int aprilTagIndex, double xOffset, double yOffset, boolean isAlgae) {
- this.aprilTagIndex = aprilTagIndex;
- aprilTagId = aprilTagIndex+1;
- this.xOffset = xOffset;
- this.yOffset = yOffset;
- this.isAlgae = isAlgae;
- pose = getPose();
- if(isAlgae){
- l4Pose = null;
- l1Pose = null;
- }else{
- l4Pose = pose.transformBy(new Transform2d(0, Units.inchesToMeters(2), new Rotation2d()));
- l1Pose = getL1Pose();
- }
- }
-
- /**
- * Calculates the Pose2d to align to the branch based on the AprilTag's pose and
- * offsets.
- *
- * @return The calculated Pose2d for this reef branch.
- */
- private Pose2d getPose() {
- Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose;
- double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0 + (isAlgae ? 0 : Units.inchesToMeters(4.5));
-
- // Apply both X and Y offsets to calculate the reef branch pose
- Transform3d transform = new Transform3d(adjustedYOffset, -xOffset, 0, new Rotation3d(0, 0, 0));
-
- Pose3d branchPose3d = basePose3d.plus(transform);
-
- // Convert the calculated branch Pose3d to Pose2d
- return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2)));
- }
- /**
- * Calculates the Pose2d to align to for scoring on L1 near this branch based on the AprilTag's pose and
- * offsets.
- *
- * @return The calculated Pose2d for this reef branch.
- */
- private Pose2d getL1Pose() {
- Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose;
- double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0;
-
- // Apply both X and Y offsets to calculate the reef branch pose
- Transform3d transform = new Transform3d(adjustedYOffset, -Math.signum(xOffset)*Units.inchesToMeters(37.04/2-1), 0, new Rotation3d(0, 0, 0));
-
- Pose3d branchPose3d = basePose3d.plus(transform);
-
- // Convert the calculated branch Pose3d to Pose2d
- return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2)));
- }
-
- /**
- * Finds the appropriate reef branch based on the AprilTag ID and whether the
- * pose is left or right.
- *
- * @param aprilTagId The ID of the AprilTag.
- * @param isLeftPose True if the pose is left, false if it's right.
- * @return The matching REEF, or null if no match is found.
- */
- public static REEF fromAprilTagIdAndPose(int aprilTagId, boolean isLeftPose) {
- for (REEF branch : values()) {
- if (!branch.isAlgae && branch.aprilTagId == aprilTagId &&
- ((isLeftPose && branch.xOffset > 0) || (!isLeftPose && branch.xOffset < 0))) {
- return branch;
- }
- }
- return null;
- }
-
- /**
- * Finds the appropriate reef algae position based on the AprilTag ID
- *
- * @param aprilTagId The ID of the AprilTag.
- * @return The matching REEF, or null if no match is found.
- */
- public static REEF fromAprilTagIdAlgae(int aprilTagId) {
- for (REEF branch : values()) {
- if (branch.isAlgae && branch.aprilTagId == aprilTagId) {
- return branch;
- }
- }
- return null;
- }
- }
}
\ No newline at end of file
import java.util.function.BooleanSupplier;
-import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Robot;
-import frc.robot.commands.drive_comm.DriveToPose;
-import frc.robot.commands.gpm.IntakeAlgae;
-import frc.robot.commands.gpm.IntakeAlgaeArm;
-import frc.robot.commands.gpm.IntakeCoral;
-import frc.robot.commands.gpm.MoveArm;
-import frc.robot.commands.gpm.MoveElevator;
-import frc.robot.commands.gpm.NetSetpoint;
-import frc.robot.commands.gpm.OuttakeAlgae;
-import frc.robot.commands.gpm.OuttakeCoral;
-import frc.robot.commands.gpm.ResetClimb;
-import frc.robot.commands.gpm.ReverseMotors;
-import frc.robot.commands.gpm.StationIntake;
-import frc.robot.constants.ArmConstants;
import frc.robot.constants.Constants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.constants.VisionConstants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.climb.Climb;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
import lib.controllers.GameController;
import lib.controllers.GameController.Axis;
import lib.controllers.GameController.Button;
-import lib.controllers.GameController.DPad;
/**
* Driver controls for the generic game controller.
*/
public class GameControllerDriverConfig extends BaseDriverConfig {
- public static final boolean singleAlignmentButton = true;
-
private final GameController driver = new GameController(Constants.DRIVER_JOY);
private final BooleanSupplier slowModeSupplier = driver.get(Button.RIGHT_JOY);
- private final Elevator elevator;
- private final Intake intake;
- private final Indexer indexer;
- private final Outtake outtake;
- private final Climb climb;
- private final Arm arm;
- private int alignmentDirection = 0;
- private Pose2d alignmentPose = null;
- public GameControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer,
- Outtake outtake, Climb climb, Arm arm) {
+ public GameControllerDriverConfig(Drivetrain drive) {
super(drive);
- this.elevator = elevator;
- this.intake = intake;
- this.indexer = indexer;
- this.outtake = outtake;
- this.climb = climb;
- this.arm = arm;
}
@Override
public void configureControls() {
- Trigger menu = driver.get(Button.LEFT_JOY);
-
- // Elevator setpoints
- if (elevator != null && arm != null) {
- driver.get(Button.BACK).and(menu.negate()).onTrue(
- new SequentialCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT),
- new MoveArm(arm, ArmConstants.L1_SETPOINT)));
-
- driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue(
- new SequentialCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
- new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)));
-
- Command l2Coral = new SequentialCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT));
- Command l3Coral = new SequentialCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT));
- Command l2Algae = new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT),
- new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
- Command l3Algae = new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT),
- new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
- driver.get(Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu));
- driver.get(Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu));
-
- // Processor setpoint
- driver.get(Button.Y).and(menu.negate()).onTrue(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001),
- new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT)));
- driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain()));
- }
- // Intake/outtake
- Trigger r3 = driver.get(Button.RIGHT_JOY);
-
- if (intake != null && indexer != null && elevator != null && outtake != null && arm != null) {// && elevator != null){
- boolean toggle = true;
- Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm);
- Command intakeAlgae = new IntakeAlgae(intake);
- driver.get(Button.A).onTrue(new InstantCommand(() -> {
- if (r3.getAsBoolean())
- return;
- if (menu.getAsBoolean()) {
- intakeAlgae.schedule();
- } else {
- if (toggle) {
- if (intakeCoral.isScheduled()) {
- intakeCoral.cancel();
- } else {
- intakeCoral.schedule();
- }
- } else {
- intakeCoral.schedule();
- }
- }
- })).onFalse(new InstantCommand(() -> {
- if (!toggle) {
- intakeCoral.cancel();
- }
- intakeAlgae.cancel();
- }));
- // On true, run the command to start intaking
- // On false, run the command to finish intaking if it has a coral
- Command startIntake = new StationIntake(outtake);
- // Command finishIntake = new FinishStationIntake(intake, indexer, elevator,
- // outtake);
- driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake)
- .onFalse(new InstantCommand(() -> {
- if (!startIntake.isScheduled()) {
- // finishIntake.schedule();
- } else {
- startIntake.cancel();
- }
- }));
- }
-
- if (intake != null && outtake != null && arm != null && elevator != null) {
- driver.get(DPad.DOWN).and(menu).onTrue(new SequentialCommandGroup(
- new OuttakeAlgae(outtake, intake),
- new InstantCommand(() -> {
- arm.setSetpoint(ArmConstants.START_ANGLE);
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- getDrivetrain().setIsAlign(false);
- }, elevator)));
- }
-
- if (outtake != null && elevator != null && arm != null) {
- driver.get(DPad.DOWN).and(menu.negate())
- .onTrue(new OuttakeCoral(outtake, elevator, arm)
- .alongWith(new InstantCommand(() -> getDrivetrain().setDesiredPose(() -> null)))
- .andThen(new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT),
- new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT))));
- }
- driver.get(DPad.DOWN).and(menu.negate()).onTrue(new InstantCommand(() -> {
- }, getDrivetrain()));
- if (intake != null && indexer != null && outtake != null) {
- driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake));
- }
-
- // Climb
- if (climb != null) {
- driver.get(Button.X).and(menu.negate())
- .toggleOnTrue(new StartEndCommand(() -> climb.extend(), () -> climb.climb(), climb));
- if (intake != null) {
- driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(() -> intake.setAngle(65), intake));
- }
- driver.get(Button.X).and(menu).whileTrue(new ResetClimb(climb));
- driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu).onTrue(new InstantCommand(() -> climb.stow(), climb));
- }
-
- // Alignment
- if (singleAlignmentButton) {
- driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(() -> {
- setAlignmentDirection();
- setAlignmentPose(false, true);
- }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
- driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(() -> {
- setAlignmentDirection();
- setAlignmentPose(false, false);
- }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
- } else {
- driver.get(DPad.LEFT).onTrue(new InstantCommand(() -> setAlignmentPose(false, true))
- .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
- driver.get(DPad.RIGHT).onTrue(new InstantCommand(() -> setAlignmentPose(false, false))
- .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose)));
- }
-
// Reset yaw to be away from driver
driver.get(Button.START).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(
new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu.negate()).onTrue(new InstantCommand(() -> {
- if (elevator != null) {
- if (outtake != null && outtake.coralLoaded()) {
- elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT);
- } else {
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- }
- }
- if (outtake != null) {
- outtake.stop();
- }
- if (intake != null) {
- intake.stow();
- intake.deactivate();
- }
- if (indexer != null) {
- indexer.stop();
- }
- if (climb != null) {
- climb.stow();
- }
- if (arm != null) {
- if (outtake != null && outtake.coralLoaded()) {
- arm.setSetpoint(ArmConstants.STOW_SETPOINT);
- } else {
- arm.setSetpoint(ArmConstants.START_ANGLE);
- }
- }
+ driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
getDrivetrain().setIsAlign(false);
getDrivetrain().setDesiredPose(() -> null);
CommandScheduler.getInstance().cancelAll();
}));
}
- private void setAlignmentDirection() {
- Translation2d drivePose = getDrivetrain().getPose().getTranslation();
- int closestDirection = 0;
- double closestDist = 20;
- boolean isRed = Robot.getAlliance() == Alliance.Red;
- int start = isRed ? 5 : 16;
- for (int i = 0; i < 6; i++) {
- double dist = FieldConstants.APRIL_TAGS.get(start + i).pose.toPose2d().getTranslation().getDistance(drivePose);
- if (dist < closestDist) {
- closestDist = dist;
- closestDirection = i;
- }
- }
- if (isRed) {
- closestDirection = (8 - closestDirection) % 6;
- }
- alignmentDirection = closestDirection;
- }
-
- /**
- * Sets the drivetrain's alignment pose to the selected position
- *
- * @param isAlgae True for algae, false for branches
- * @param isLeft True for left branch, false for right, ignored for algae
- */
- private void setAlignmentPose(boolean isAlgae, boolean isLeft) {
- int id = Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17
- : (8 - alignmentDirection) % 6 + 6;
- if (isAlgae) {
- alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(id).pose;
- } else {
- alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(id, isLeft).pose;
- }
- }
-
@Override
public double getRawForwardTranslation() {
return driver.get(Axis.LEFT_Y);
package frc.robot.controls;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Robot;
-import frc.robot.commands.DoNothing;
-import frc.robot.commands.gpm.IntakeAlgae;
-import frc.robot.commands.gpm.IntakeCoral;
-import frc.robot.commands.gpm.MoveElevator;
-import frc.robot.commands.gpm.OuttakeAlgaeIntake;
-import frc.robot.commands.gpm.OuttakeCoral;
-import frc.robot.commands.gpm.RemoveAlgae;
-import frc.robot.commands.gpm.ReverseMotors;
-import frc.robot.commands.gpm.StationIntake;
import frc.robot.constants.Constants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.IntakeConstants;
-import frc.robot.constants.VisionConstants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.climb.Climb;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
import lib.controllers.GameController;
-import lib.controllers.GameController.Button;
-import lib.controllers.GameController.DPad;
/**
* Controls for the operator, which are almost a duplicate of most of the driver's controls
private final GameController driver = new GameController(Constants.OPERATOR_JOY);
private final Drivetrain drive;
- private final Elevator elevator;
- private final Intake intake;
- private final Indexer indexer;
- private final Outtake outtake;
- private final Climb climb;
- private final Arm arm;
- private int alignmentDirection = 0;
-
- public Operator(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) {
+
+ public Operator(Drivetrain drive) {
this.drive = drive;
- this.elevator = elevator;
- this.intake = intake;
- this.indexer = indexer;
- this.outtake = outtake;
- this.climb = climb;
- this.arm = arm;
}
public void configureControls() {
- // TODO: Update to match new driver controls, except the ones that move the drivetrain
- Trigger menu = driver.get(Button.LEFT_JOY);
-
- // Elevator setpoints
- if(elevator != null && outtake != null) {
- driver.get(Button.BACK).onTrue(new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
- driver.get(Button.LB).onTrue(new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
- driver.get(Button.RB).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
- driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT).deadlineFor(new RemoveAlgae(outtake)));
- driver.get(Button.Y).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT));
- }
-
- // Intake/outtake
- Trigger r3 = driver.get(Button.RIGHT_JOY);
- if(intake != null && indexer != null){// && elevator != null){
- driver.get(Button.A).and(menu.negate()).and(r3.negate()).whileTrue(new IntakeCoral(intake, indexer, elevator, outtake, arm));
- // On true, run the command to start intaking
- // On false, run the command to finish intaking if it has a coral
- Command startIntake = new StationIntake(outtake);
- Command finishIntake = new DoNothing();
- driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake)
- .onFalse(new InstantCommand(()->{
- if(!startIntake.isScheduled()){
- finishIntake.schedule();
- }else{
- startIntake.cancel();
- }
- }));
- }
- if(intake != null){
- driver.get(Button.A).and(menu).whileTrue(new IntakeAlgae(intake));
- driver.get(DPad.DOWN).and(menu).onTrue(new OuttakeAlgaeIntake(intake));
- }
- if(outtake != null && elevator != null){
- driver.get(DPad.DOWN).and(menu.negate()).onTrue(new OuttakeCoral(outtake, elevator, arm));
- }
- if(intake != null && indexer != null){
- driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake));
- }
-
- // Climb
- if(climb != null){
- driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->climb.extend(), climb))
- .onFalse(new InstantCommand(()->climb.climb(), climb));
- if(intake != null){
- driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(IntakeConstants.ALGAE_SETPOINT), intake));
- }
- }
-
- // Alignment
- driver.get(Button.B).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 0));
- driver.get(Button.Y).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 2));
- driver.get(Button.X).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 3));
- driver.get(Button.RB).onTrue(new InstantCommand(()->alignmentDirection = 4));
- driver.get(DPad.UP).onTrue(new InstantCommand(()->alignmentDirection = 5));
- driver.get(DPad.LEFT).onTrue(new InstantCommand(()->setAlignmentPose(true)));
- driver.get(DPad.RIGHT).onTrue(new InstantCommand(()->setAlignmentPose(false)));
-
- // Cancel commands
- driver.get(Button.START).onTrue(new InstantCommand(()->{
- if(elevator != null){
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- }
- if(outtake != null){
- outtake.stop();
- }
- if(intake != null){
- intake.stow();
- intake.deactivate();
- }
- if(indexer != null){
- indexer.stop();
- }
- if(climb != null){
- climb.stow();
- }
- drive.setDesiredPose(()->null);
+ // Cancel commands, could be removed if the operator doesn't need this button
+ driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
+ drive.setIsAlign(false);
+ drive.setDesiredPose(() -> null);
CommandScheduler.getInstance().cancelAll();
}));
}
- /**
- * Sets the drivetrain's alignmetn pose to the selected position
- * @param isLeft True for left branch, false for right
- */
- private void setAlignmentPose(boolean isLeft){
- Pose2d pose = VisionConstants.REEF.fromAprilTagIdAndPose(
- Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17
- : (8-alignmentDirection) % 6 + 6,
- isLeft).pose;
- drive.setDesiredPose(pose);
- }
public Trigger getRightTrigger(){
return new Trigger(driver.RIGHT_TRIGGER_BUTTON);
import java.util.function.BooleanSupplier;
-import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Robot;
-import frc.robot.commands.DoNothing;
-import frc.robot.commands.drive_comm.DriveToPose;
-import frc.robot.commands.gpm.IntakeAlgae;
-import frc.robot.commands.gpm.IntakeAlgaeArm;
-import frc.robot.commands.gpm.IntakeCoral;
-import frc.robot.commands.gpm.MoveArm;
-import frc.robot.commands.gpm.MoveElevator;
-import frc.robot.commands.gpm.OuttakeAlgae;
-import frc.robot.commands.gpm.NetSetpoint;
-import frc.robot.commands.gpm.OuttakeCoral;
-import frc.robot.commands.gpm.ResetClimb;
-import frc.robot.commands.gpm.ReverseMotors;
-import frc.robot.commands.gpm.StationIntake;
-import frc.robot.constants.ArmConstants;
import frc.robot.constants.Constants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.constants.VisionConstants;
-import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.climb.Climb;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.indexer.Indexer;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.outtake.Outtake;
import lib.controllers.PS5Controller;
-import lib.controllers.PS5Controller.DPad;
import lib.controllers.PS5Controller.PS5Axis;
import lib.controllers.PS5Controller.PS5Button;
* Driver controls for the PS5 controller
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
- private final boolean autoOuttake = true;
-
private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
- private final Elevator elevator;
- private final Intake intake;
- private final Indexer indexer;
- private final Outtake outtake;
- private final Climb climb;
- private final Arm arm;
private final BooleanSupplier slowModeSupplier = ()->false;
- private Pose2d alignmentPose = null;
- // 0 == not selected, -1 == left, 1 == right
- private byte selectedDirection = 0;
- public PS5ControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) {
+ public PS5ControllerDriverConfig(Drivetrain drive) {
super(drive);
- this.elevator = elevator;
- this.intake = intake;
- this.indexer = indexer;
- this.outtake = outtake;
- this.climb = climb;
- this.arm = arm;
}
public void configureControls() {
- Trigger menu = driver.get(PS5Button.LEFT_JOY);
-
- // Elevator setpoints
- if(elevator != null && arm != null && outtake != null) {
- driver.get(PS5Button.OPTIONS).and(menu.negate()).onTrue(
- new SequentialCommandGroup(
- new InstantCommand(()->setAlignmentPose(false, true)),
- new ConditionalCommand(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT),
- new MoveArm(arm, ArmConstants.L1_SETPOINT),
- new DriveToPose(getDrivetrain(), ()->alignmentPose)
- ),
- // This is instant so it doesn't requre the drivetrain for more than 1 frame
- new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.L1_SETPOINT);
- arm.setSetpoint(ArmConstants.L1_SETPOINT);
- }),
- ()->selectedDirection != 0
- ),
- new ConditionalCommand(
- new SequentialCommandGroup(
- new OuttakeCoral(outtake, elevator, arm),
- new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
- new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
- alignmentPose = null;
- selectedDirection = 0;
- }, elevator, arm)
- ),
- new DoNothing(),
- () -> selectedDirection != 0 && autoOuttake
- )
- )
- );
-
- driver.get(PS5Button.LEFT_TRIGGER).onTrue(
- new SequentialCommandGroup(
- new InstantCommand(()->setAlignmentPose(true)),
- new ConditionalCommand(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
- new ConditionalCommand(
- new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT),
- new MoveArm(arm, ArmConstants.L4_SETPOINT_LEFT),
- () -> selectedDirection >= 0
- ),
- new DriveToPose(getDrivetrain(), ()->alignmentPose)
- ),
- // This is instant so it doesn't requre the drivetrain for more than 1 frame
- new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.L4_SETPOINT);
- arm.setSetpoint(ArmConstants.L4_SETPOINT_RIGHT);
- }),
- ()->selectedDirection != 0
- ),
- new ConditionalCommand(
- new WaitCommand(0.5),
- new DoNothing(),
- () -> selectedDirection < 0
- ),
- new ConditionalCommand(
- new OuttakeCoral(outtake, elevator, arm)
- .andThen(new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
- alignmentPose = null;
- selectedDirection = 0;
- }, elevator, arm)),
- new DoNothing(),
- () -> selectedDirection != 0 && autoOuttake
- )
- )
- );
-
- Command l2Coral = new SequentialCommandGroup(
- new SequentialCommandGroup(
- new InstantCommand(()->setAlignmentPose(false)),
- new ConditionalCommand(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT),
- new DriveToPose(getDrivetrain(), ()->alignmentPose)
- ),
- // This is instant so it doesn't requre the drivetrain for more than 1 frame
- new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.L2_SETPOINT);
- arm.setSetpoint(ArmConstants.L2_L3_SETPOINT);
- }),
- ()->selectedDirection != 0
- ),
- new ConditionalCommand(
- new OuttakeCoral(outtake, elevator, arm)
- .andThen(new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
- alignmentPose = null;
- selectedDirection = 0;
- }, elevator, arm)),
- new DoNothing(),
- () -> selectedDirection != 0 && autoOuttake
- )
- )
- );
- Command l3Coral = new SequentialCommandGroup(
- new SequentialCommandGroup(
- new InstantCommand(()->setAlignmentPose(false)),
- new ConditionalCommand(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT),
- new MoveArm(arm, ArmConstants.L2_L3_SETPOINT),
- new DriveToPose(getDrivetrain(), ()->alignmentPose)
- ),
- // This is instant so it doesn't requre the drivetrain for more than 1 frame
- new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.L3_SETPOINT);
- arm.setSetpoint(ArmConstants.L2_L3_SETPOINT);
- }),
- ()->selectedDirection != 0
- ),
- new ConditionalCommand(
- new OuttakeCoral(outtake, elevator, arm)
- .andThen(new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
- alignmentPose = null;
- selectedDirection = 0;
- }, elevator, arm)),
- new DoNothing(),
- () -> selectedDirection != 0 && autoOuttake
- )
- )
- );
- Command l2Algae = new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT),
- new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
- Command l3Algae = new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT),
- new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake));
- driver.get(PS5Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu));
- driver.get(PS5Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu));
-
- //Processor setpoint
- driver.get(PS5Button.TRIANGLE).and(menu.negate()).onTrue(
- new ParallelCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001),
- new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT)
- )
- );
- driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain()));
- }
-
- // Intake/outtake
- Trigger r3 = driver.get(PS5Button.RIGHT_JOY);
-
- if(intake != null && indexer != null && elevator != null && outtake != null && arm != null){
- boolean toggle = true;
- Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm);
- Command intakeAlgae = new IntakeAlgae(intake);
- driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
- if(r3.getAsBoolean()) return;
- if(menu.getAsBoolean()){
- intakeAlgae.schedule();
- }else{
- if(toggle){
- if(intakeCoral.isScheduled()){
- intakeCoral.cancel();
- }else{
- intakeCoral.schedule();
- }
- }else{
- intakeCoral.schedule();
- }
- }
- })).onFalse(new InstantCommand(()->{
- if(!toggle){
- intakeCoral.cancel();
- }
- intakeAlgae.cancel();
- }));
- // On true, run the command to start intaking
- // On false, run the command to finish intaking if it has a coral
- Command startIntake = new StationIntake(outtake);
- // Command finishIn6take = new FinishStationIntake(intake, indexer, elevator, outtake);
- // driver.get(PS5Button.CROSS).and(r3).and(menu.negate()).onTrue(startIntake)
- // .onFalse(new InstantCommand(()->{
- // if(!startIntake.isScheduled()){
- // // finishIntake.schedule();
- // }else{
- // startIntake.cancel();
- // }
- // }));
- driver.get(PS5Button.CROSS).and(r3).onTrue(
- new SequentialCommandGroup(
- new MoveElevator(elevator, ElevatorConstants.STATION_INTAKE_SETPOINT),
- new MoveArm(arm, ArmConstants.STATION_INTAKE_SETPOINT)).
- andThen(startIntake));
- }
-
- if(intake != null && outtake != null && arm != null && elevator != null){
- Command algae = new SequentialCommandGroup(
- new OuttakeAlgae(outtake, intake),
- // Only move the arm and elevator in sequence when scoring in the net
- new ConditionalCommand(
- new SequentialCommandGroup(
- new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT),
- new InstantCommand(()-> elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT)),
- new WaitCommand(0.25),
- new InstantCommand(()-> arm.setSetpoint(ArmConstants.INTAKE_SETPOINT))
- ),
- new InstantCommand(()->{
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);
- }),
- // 1 meter is in between L3 and net setpoints
- () -> elevator.getSetpoint() > 1
- )
- );
- Command coral = new OuttakeCoral(outtake, elevator, arm).alongWith(new InstantCommand(()->getDrivetrain().setDesiredPose(()->null)))
- .andThen(
- new ConditionalCommand(
- new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT), new InstantCommand(()->selectedDirection = 0)),
- new DoNothing(),
- ()->!arm.canMoveElevator()
- ));
- Command cancelAlign = new InstantCommand(()->{}, getDrivetrain());
-
- driver.get(DPad.DOWN).onTrue(new InstantCommand(()->{
- if(menu.getAsBoolean()){
- algae.schedule();
- }else{
- coral.schedule();
- }
- cancelAlign.schedule();
- }));
- }
- if(intake != null && indexer != null && outtake != null){
- driver.get(PS5Button.CIRCLE).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake));
- }
-
- // Climb
- if(climb != null){
- driver.get(PS5Button.SQUARE).and(menu.negate()).toggleOnTrue(new StartEndCommand(()->climb.extend(), ()->climb.climb(), climb));
- if(intake != null){
- driver.get(PS5Button.SQUARE).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(65), intake));
- }
- driver.get(PS5Button.PS).and(menu).whileTrue(new ResetClimb(climb));
- driver.get(PS5Button.RIGHT_TRIGGER).and(menu).onTrue(new InstantCommand(()->climb.stow(), climb));
- }
-
- // Alignment
- driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(()->{
- selectedDirection = -1;
- }));
- driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(()->{
- selectedDirection = 1;
- }));
- driver.get(PS5Button.TOUCHPAD).toggleOnTrue(new InstantCommand(()->{
- setAlignmentPose(true, false, false, false);
- }).andThen(new DriveToPose(getDrivetrain(), ()->alignmentPose)));
-
// Reset the yaw. Mainly useful for testing/driver practice
- driver.get(PS5Button.CREATE).and(menu.negate()).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
- )));
- driver.get(PS5Button.CREATE).and(menu).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? Math.PI*5/6 : -Math.PI/6)
+ driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
)));
// Cancel commands
- driver.get(PS5Button.RIGHT_TRIGGER).and(menu.negate()).onTrue(new InstantCommand(()->{
- if(elevator != null){
- if(outtake != null && outtake.coralLoaded()){
- elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT);
- }else{
- elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT);
- }
- }
- if(outtake != null){
- outtake.stop();
- }
- if(intake != null){
- intake.stow();
- intake.deactivate();
- }
- if(indexer != null){
- indexer.stop();
- }
- if(climb != null){
- climb.stow();
- }
- if(arm != null){
- if(outtake != null && outtake.coralLoaded()){
- arm.setSetpoint(ArmConstants.STOW_SETPOINT);
- }else{
- arm.setSetpoint(ArmConstants.START_ANGLE);
- }
- }
+ driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{
getDrivetrain().setIsAlign(false);
getDrivetrain().setDesiredPose(()->null);
- alignmentPose = null;
- selectedDirection = 0;
CommandScheduler.getInstance().cancelAll();
}));
- driver.get(PS5Button.MUTE).and(menu).onTrue(new FunctionalCommand(
+ // Align wheels
+ driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand(
()->getDrivetrain().setStateDeadband(false),
getDrivetrain()::alignWheels,
interrupted->getDrivetrain().setStateDeadband(true),
()->false, getDrivetrain()).withTimeout(2));
}
-
- /**
- * Sets the drivetrain's alignmetn pose to the nearest reef branch or algae location
- * @param isAlgae True for algae, false for branches
- * @param isLeft True for left branch, false for right, ignored for algae
- * @param l4 If the robot should align to the L4 scoring pose
- */
- private void setAlignmentPose(boolean isAlgae, boolean isLeft, boolean l4, boolean l1){
- Translation2d drivePose = getDrivetrain().getPose().getTranslation();
- int closestId = 0;
- double closestDist = 20;
- boolean isRed = Robot.getAlliance() == Alliance.Red;
- int start = isRed ? 5 : 16;
- for(int i = 0; i < 6; i++){
- double dist = FieldConstants.APRIL_TAGS.get(start+i).pose.toPose2d().getTranslation().getDistance(drivePose);
- if(dist < closestDist){
- closestDist = dist;
- closestId = start+i+1;
- }
- }
- if(isAlgae){
- alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(closestId).pose;
- }else{
- if(l4){
- alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l4Pose;
- }else if(l1){
- alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l1Pose;
- }else{
- alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).pose;
- }
- }
- }
-
- /**
- * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction
- * @param l4 If the robot should align to the L4 scoring pose
- * @param l1 If the robot should align to the L1 scoring pose
- */
- private void setAlignmentPose(boolean l4, boolean l1){
- if(selectedDirection == 0){
- alignmentPose = null;
- return;
- }
- setAlignmentPose(false, selectedDirection < 0, l4, l1);
- }
- /**
- * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction
- * @param l4 If the robot should align to the L4 scoring pose
- */
- private void setAlignmentPose(boolean l4){
- setAlignmentPose(l4, false);
- }
@Override
public double getRawSideTranslation() {
+++ /dev/null
-package frc.robot.subsystems.arm;
-
-import java.util.function.BooleanSupplier;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.sim.TalonFXSimState;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj.util.Color8Bit;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.ArmConstants;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-import frc.robot.util.PhoenixUtil;
-
-public class Arm extends SubsystemBase implements ArmIO {
- //motor
- private TalonFX motor = new TalonFX(IdConstants.ARM_MOTOR);
- private TalonFXSimState encoderSim;
-
- // Mechism2d display
- private Mechanism2d simulationMechanism;
- private MechanismLigament2d simLigament;
- private SingleJointedArmSim armSim;
-
- private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(IdConstants.ARM_ABSOLUTE_ENCODER);
-
- private double setpoint = ArmConstants.START_ANGLE;
-
- private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
-
- private final ArmFeedforward feedforward = new ArmFeedforward(0, ArmConstants.MASS*ArmConstants.CENTER_OF_MASS_LENGTH/ArmConstants.GEAR_RATIO/ArmConstants.MOTOR.KtNMPerAmp*ArmConstants.MOTOR.rOhms, 0);
-
- private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
-
- private BooleanSupplier elevatorStowed;
-
- public Arm() {
- if (RobotBase.isSimulation()) {
- encoderSim = motor.getSimState();
- encoderSim.setRawRotorPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO);
- armSim = new SingleJointedArmSim(
- ArmConstants.MOTOR,
- ArmConstants.GEAR_RATIO,
- ArmConstants.MOI,
- ArmConstants.LENGTH,
- Units.degreesToRadians(ArmConstants.MIN_ANGLE), //min angle
- Units.degreesToRadians(ArmConstants.MAX_ANGLE), //max angle
- true,
- Units.degreesToRadians(ArmConstants.START_ANGLE));
- simulationMechanism = new Mechanism2d(3, 3);
- MechanismRoot2d root = simulationMechanism.getRoot("Arm", 1.5, 1.5);
- simLigament = root.append(
- new MechanismLigament2d("angle", 1, ArmConstants.START_ANGLE, 4, new Color8Bit(Color.kAliceBlue))
- );
- SmartDashboard.putData("Arm Display", simulationMechanism);
- }
-
- // resetAbsolute();
- motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO);
-
- var talonFXConfigs = new TalonFXConfiguration();
-
- var slot0Configs = talonFXConfigs.Slot0;
- slot0Configs.kS = 0.1; // Static friction compensation (should be >0 if friction exists)
- slot0Configs.kG = ArmConstants.MASS * 9.81 * ArmConstants.CENTER_OF_MASS_LENGTH / ArmConstants.GEAR_RATIO; // Gravity compensation
- slot0Configs.kV = 0.12; // Velocity gain: 1 rps -> 0.12V
- slot0Configs.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters)
- slot0Configs.kP = 0.85; // If position error is 2.5 rotations, apply 12V (0.5 * 2.5 * 12V)
- slot0Configs.kI = 0.; // Integral term (usually left at 0 for MotionMagic)
- slot0Configs.kD = 0; // Derivative term (used to dampen oscillations)
-
- // set Motion Magic settings
- var motionMagicConfigs = talonFXConfigs.MotionMagic;
- motionMagicConfigs.MotionMagicCruiseVelocity = ArmConstants.MAX_VELOCITY * ArmConstants.GEAR_RATIO/Math.PI/2;
- motionMagicConfigs.MotionMagicAcceleration = ArmConstants.MAX_ACCELERATION * ArmConstants.GEAR_RATIO/Math.PI/2;
- talonFXConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
- motor.getConfigurator().apply(talonFXConfigs);
- updateInputs();
- PhoenixUtil.tryUntilOk(100, ()->motor.setNeutralMode(NeutralModeValue.Brake));
-
- }
-
- public void setElevatorStowed(BooleanSupplier elevatorStowed){
- this.elevatorStowed = elevatorStowed;
- }
-
- @Override
- public void periodic() {
- double setpoint2 = setpoint;
- if(elevatorStowed == null || elevatorStowed.getAsBoolean() && Math.abs(setpoint2-ArmConstants.L1_SETPOINT) > 0.0001){
- setpoint2 = ArmConstants.START_ANGLE;
- }
- double setpointRotations = Units.degreesToRotations(setpoint2) * ArmConstants.GEAR_RATIO;
- motor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0)));
- updateInputs();
- Logger.recordOutput("Arm/Atsetpoint",atSetpoint());
- }
-
- @Override
- public void simulationPeriodic() {
- armSim.setInputVoltage(getAppliedVoltage());
- armSim.update(Constants.LOOP_TIME);
-
- double armRotations = Units.radiansToRotations(armSim.getAngleRads());
- encoderSim.setRawRotorPosition(armRotations * ArmConstants.GEAR_RATIO);
- simLigament.setAngle(getAngle());
- }
-
- public void setSetpoint(double setpoint) {
-
- this.setpoint = MathUtil.clamp(setpoint, ArmConstants.MIN_ANGLE, ArmConstants.MAX_ANGLE);
- }
-
- public double getAppliedVoltage() {
- return motor.getMotorVoltage().getValueAsDouble();
- }
-
- /**
- * Gets the angle of the arm
- * @return The angle in degrees
- */
- public double getAngle() {
- return inputs.measuredAngle;
- }
-
- public void resetAbsolute(){
- if(RobotBase.isSimulation()){
- motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO);
- }else{
- double absolutePosition = absoluteEncoder.get() / ArmConstants.ENCODER_GEAR_RATIO;
- motor.setPosition(MathUtil.inputModulus(absolutePosition - Units.degreesToRotations(ArmConstants.OFFSET), -0.5, 0.5)*ArmConstants.GEAR_RATIO);
- }
- }
-
- public boolean atSetpoint() {
- return Math.abs(getAngle() - setpoint) < ArmConstants.TOLERANCE;
- }
-
- public boolean canMoveElevator() {
- return Math.abs(getAngle() - ArmConstants.START_ANGLE) < 5 || Math.abs(getAngle() - ArmConstants.L1_SETPOINT) < 5;
- }
-
- @Override
- public void updateInputs(){
- inputs.measuredAngle = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / ArmConstants.GEAR_RATIO;
- inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble();
-
- Logger.processInputs("Arm", inputs);
- Logger.recordOutput("Arm/setpointDeg", setpoint);
- }
-}
+++ /dev/null
-package frc.robot.subsystems.arm;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface ArmIO {
- @AutoLog
- public static class ArmIOInputs{
- public double measuredAngle = 0.0;
- public double currentAmps = 0.0;
- }
-
- public void updateInputs();
-}
+++ /dev/null
-package frc.robot.subsystems.climb;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.sim.TalonFXSimState;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj.util.Color8Bit;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-import frc.robot.util.ClimbArmSim;
-
-public class Climb extends SubsystemBase {
-
- private static final double startingPosition = 0;
- private static final double extendPosition = 2;
- private static final double climbPosition = -0.83;
-
- //Motors
- private final PIDController pid = new PIDController(2.5, 0, 0.0);
-
- private TalonFX motor = new TalonFX(IdConstants.CLIMB_MOTOR, Constants.CANIVORE_CAN);
- private final DCMotor climbGearBox = DCMotor.getKrakenX60(1);
- private TalonFXSimState encoderSim;
-
- //Mechism2d display
- private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3);
- private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5);
- private final MechanismLigament2d simLigament = mechanismRoot.append(
- new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite))
- );
-
- private final double versaPlanetaryGearRatio = 1.0;
- private final double climbGearRatio = 60.0/1.0;
- private final double totalGearRatio = versaPlanetaryGearRatio * climbGearRatio;
-
- private ClimbArmSim climbSim;
-
- private double power;
-
- private boolean resetting = false;
-
- private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged();
-
- public Climb() {
- if (RobotBase.isSimulation()) {
- encoderSim = motor.getSimState();
- encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*totalGearRatio);
-
- climbSim = new ClimbArmSim(
- climbGearBox,
- totalGearRatio,
- 0.1,
- 0.127,
- 0, //min angle
- Units.degreesToRadians(90), //max angle
- true,
- Units.degreesToRadians(startingPosition),
- 60
- );
-
- climbSim.setIsClimbing(true);
- SmartDashboard.putData("Climb Display", simulationMechanism);
- }
-
- pid.setIZone(1);
-
- pid.setSetpoint(Units.degreesToRadians(startingPosition));
-
- motor.setPosition(Units.degreesToRotations(startingPosition)*totalGearRatio);
- motor.setNeutralMode(NeutralModeValue.Brake);
- //SmartDashboard.putData("Climb PID", pid);
- }
-
- @Override
- public void periodic() {
-
- inputs.measuredPositionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble() / totalGearRatio);
- inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble();
- Logger.processInputs("Climb", inputs);
-
- double motorPosition = motor.getPosition().getValueAsDouble();
- double currentPosition = Units.rotationsToRadians(motorPosition/totalGearRatio);
- power = pid.calculate(currentPosition);
-
- if(resetting){
- power = -0.1;
- }
-
- Logger.recordOutput("Climb/Motor Power", power);
- Logger.recordOutput("Climb/setpointDeg", motorPosition/totalGearRatio);
-
- motor.set(MathUtil.clamp(power, -1, 1));
- }
-
-
- @Override
- public void simulationPeriodic() {
- climbSim.setInput(power * Constants.ROBOT_VOLTAGE);
- climbSim.update(Constants.LOOP_TIME);
-
- double climbRotations = Units.radiansToRotations(climbSim.getAngleRads());
- encoderSim.setRawRotorPosition(climbRotations * totalGearRatio);
-
- simLigament.setAngle(Units.radiansToDegrees(getAngle()));
- }
-
- /**
- * Sets the motor to an angle from 0-90 deg
- * @param angle in degrees
- */
- public void setAngle(double angle) {
- pid.reset();
- pid.setSetpoint(Units.degreesToRadians(angle));
- }
-
- /**
- * Gets the current position of the motor in degrees
- * @return The angle in degrees
- */
- public double getAngle() {
- return inputs.measuredPositionDeg;
- }
-
- /**
- * Turns the motor to 90 degrees (extended positiion)
- */
- public void extend(){
- double extendAngle = Units.rotationsToDegrees(extendPosition);
- setAngle(extendAngle);
- }
-
- /**
- * Turns the motor to 0 degrees (climb position)
- */
- public void climb(){
- setAngle(Units.rotationsToDegrees(climbPosition));
- }
-
- /**
- * Turns the motor to 0 degrees (climb position)
- */
- public void stow(){
- setAngle(startingPosition);
- }
-
- public void reset(boolean resetting){
- this.resetting = resetting;
- if(!resetting){
- motor.setPosition(climbPosition*totalGearRatio);
- pid.setSetpoint(Units.degreesToRadians(startingPosition));
- pid.reset();
- }
- }
-
- public double getCurrent(){
- return motor.getStatorCurrent().getValueAsDouble();
- }
-
- //not working
- public Mechanism2d getMech2d() {
- return simulationMechanism;
- }
-
- /**
- * Gets the estimated angle of the climb
- * This is slightly inaccurate since it assumes the climb rotates exactly 90 degrees and the motor position is proportional to the climb position
- * Used only for 3D robot display
- */
- public double getEstimatedClimbAngle(){
- return Units.degreesToRotations(getAngle())/(extendPosition-climbPosition)*Math.PI/2;
- }
-}
\ No newline at end of file
+++ /dev/null
-package frc.robot.subsystems.climb;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface ClimbIO {
- @AutoLog
- public static class ClimbIOInputs{
- public double measuredPositionDeg = 0.0;
- public double currentAmps = 0.0;
- }
-}
double[] sampleTimestamps =
gyroInputs.odometryYawTimestamps; // All signals are sampled together
int sampleCount = sampleTimestamps.length;
+ SwerveModulePosition[][] positions = new SwerveModulePosition[4][];
+ for(int i = 0; i < modules.length; i++){
+ positions[i] = modules[i].getOdometryPositions();
+ sampleCount = Math.min(sampleCount, positions[i].length);
+ }
for (int i = 0; i < sampleCount; i++) {
// Read wheel positions and deltas from each module
SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
- modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i];
+ modulePositions[moduleIndex] = positions[moduleIndex][i];
}
// Use the real gyro angle
rawGyroRotation = gyroInputs.odometryYawPositions[i];
// Even if vision is disabled, it should still update inputs
// This prevents it from storing a lot of unread results, and it could be useful for replays
- vision.updateInputs();
+ if(vision != null){
+ vision.updateInputs();
+ }
if(VisionConstants.ENABLED){
if(vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)){
}
public void alignWheels(){
- SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(1.465));
+ SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
setModuleStates(new SwerveModuleState[]{
state, state, state, state
}, false);
+++ /dev/null
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.subsystems.elevator;
-
-import java.util.function.BooleanSupplier;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
-import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.IdConstants;
-import frc.robot.util.AngledElevatorSim;
-import frc.robot.util.PhoenixUtil;
-
-public class Elevator extends SubsystemBase {
- private TalonFX rightMotor = new TalonFX(IdConstants.ELEVATOR_RIGHT_MOTOR, Constants.CANIVORE_CAN);
-
- private double setpoint = ElevatorConstants.INTAKE_SETPOINT;
-
- private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
-
- private double maxVelocity = 3.6; // m/s 3.68
- private double maxAcceleration = 14; // m/s 8
-
- // Sim variables
- private AngledElevatorSim sim;
- private Mechanism2d mechanism;
- private MechanismLigament2d ligament;
- private double voltage;
-
- private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
-
- private BooleanSupplier armStowed;
-
- public Elevator() {
- // This increases both the time and memory efficiency of the code when running
- // on a real robot; do not remove this if statement
- if (RobotBase.isSimulation()) {
- sim = new AngledElevatorSim(ElevatorConstants.MOTOR, ElevatorConstants.GEARING, ElevatorConstants.CARRIAGE_MASS,
- ElevatorConstants.DRUM_RADIUS, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT, true,
- ElevatorConstants.START_HEIGHT, ElevatorConstants.ANGLE, ElevatorConstants.SPRING_FORCE);
- double width = ElevatorConstants.MAX_HEIGHT * Math.sin(ElevatorConstants.ANGLE);
- double height = ElevatorConstants.MAX_HEIGHT * Math.cos(ElevatorConstants.ANGLE);
- double size = Math.max(width, height);
- mechanism = new Mechanism2d(size, size);
- ligament = mechanism.getRoot("base", size / 2 - width / 2, size / 2 - height / 2).append(new MechanismLigament2d(
- "elevator", ElevatorConstants.START_HEIGHT, 90 - Units.radiansToDegrees(Math.abs(ElevatorConstants.ANGLE))));
- SmartDashboard.putData("elevator", mechanism);
- }
- Timer.delay(1.0);
-
- //m_lastProfiledReference = new ExponentialProfile.State(getPosition(),0);
- resetEncoder(ElevatorConstants.START_HEIGHT);
-
- var talonFXConfigs = new TalonFXConfiguration();
-
- // set slot 0 gains
- var slot0Configs = talonFXConfigs.Slot0;
- slot0Configs.kS = 0.15; // Add 0.25 V output to overcome static friction
- slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output
- slot0Configs.kA = 0; // An acceleration of 1 rps/s requires 0.01 V output
- slot0Configs.kP = 0.75; // A position error of 2.5 rotations results in 12 V output
- slot0Configs.kI = 0; // no output for integrated error
- slot0Configs.kD = 0; // A velocity error of 1 rps results in 0.1 V output
-
- // set Motion Magic settings
- var motionMagicConfigs = talonFXConfigs.MotionMagic;
- motionMagicConfigs.MotionMagicCruiseVelocity = ElevatorConstants.GEARING * maxVelocity/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target cruise velocity
- motionMagicConfigs.MotionMagicAcceleration = ElevatorConstants.GEARING * maxAcceleration/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target acceleration
- rightMotor.getConfigurator().apply(talonFXConfigs);
- rightMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive));
- updateInputs();
- PhoenixUtil.tryUntilOk(100, ()-> rightMotor.setNeutralMode(NeutralModeValue.Brake));
- }
-
- public void setArmStowed(BooleanSupplier armStowed){
- this.armStowed = armStowed;
- }
-
- @Override
- public void periodic() {
- double setpoint2 = setpoint;
- if(setpoint2 < ElevatorConstants.SAFE_SETPOINT && (armStowed == null || !armStowed.getAsBoolean())){
- setpoint2 = ElevatorConstants.SAFE_SETPOINT;
- }
- double setpointRotations = ElevatorConstants.GEARING * setpoint2 / ElevatorConstants.DRUM_RADIUS/Math.PI/2;
- rightMotor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(0.4));
- updateInputs();
- Logger.processInputs("Elevator", inputs);
- Logger.recordOutput("Elevator/Setpoint", getSetpoint());
- Logger.recordOutput("Elevator/AtSetpoint", atSetpoint());
- }
-
- @Override
- public void simulationPeriodic() {
- sim.setInputVoltage(0);
- sim.update(Constants.LOOP_TIME);
- ligament.setLength(sim.getPositionMeters());
- rightMotor.getSimState().setRawRotorPosition(
- sim.getPositionMeters() / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING);
- }
-
- public void resetEncoder(double height) {
- // Without the if statement, this causes loop overruns in simulation, and this
- // code does nothing anyway on sim (it sets the position to itself)
- if (RobotBase.isReal()) {
- rightMotor.setPosition(height / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING);
- }
- }
-
- public void updateInputs(){
- inputs.measuredPosition = rightMotor.getPosition().getValueAsDouble() / ElevatorConstants.GEARING
- * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS);
- inputs.velocity = rightMotor.getVelocity().getValueAsDouble()/ ElevatorConstants.GEARING
- * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS);
- inputs.currentAmps = rightMotor.getStatorCurrent().getValueAsDouble();
- }
-
- /**
- * Get the position of the elevator in meters.
- */
- public double getPosition() {
- return inputs.measuredPosition;
- }
-
- /**
- * Get the velocity of the elevator in m/s.
- */
- public double getVelocity(){
- return inputs.velocity;
- }
-
- public double getVoltage(){
- return voltage;
- }
-
- /**
- * Method to set the setpoint of the elevator. Clamped between min and max height.
- * @param setpoint The setpoint in meters.
- */
- public void setSetpoint(double setpoint) {
- this.setpoint = MathUtil.clamp(setpoint, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT);
- }
-
- /**
- * Get the velocity of the elevator in meters.
- */
- public double getSetpoint() {
- return setpoint;
- }
-
- public Mechanism2d getMechanism2d() {
- return mechanism;
- }
-
- public boolean atSetpoint(){
- return Math.abs(getPosition() - setpoint) < (0.025+0.0125);
- }
-
- /**
- * Get the COM at the current elevater height.
- */
- public double getCenterOfMassHeight(){
- return (getPosition()-ElevatorConstants.MIN_HEIGHT)/(ElevatorConstants.MAX_HEIGHT-ElevatorConstants.MIN_HEIGHT)*(ElevatorConstants.CENTER_OF_MASS_HEIGHT_EXTENDED-ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED)+ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED;
- }
-}
+++ /dev/null
-package frc.robot.subsystems.elevator;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface ElevatorIO {
- @AutoLog
- public static class ElevatorIOInputs {
- public double measuredPosition = 0.0;
- public double velocity = 0.0;
- public double currentAmps = 0.0;
- }
-}
+++ /dev/null
-package frc.robot.subsystems.indexer;
-
-import org.littletonrobotics.junction.AutoLogOutput;
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.hardware.TalonFX;
-
-import au.grapplerobotics.ConfigurationFailedException;
-import au.grapplerobotics.LaserCan;
-import au.grapplerobotics.interfaces.LaserCanInterface;
-import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
-import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest;
-import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget;
-import au.grapplerobotics.simulation.MockLaserCan;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Robot;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-import frc.robot.constants.IndexerConstants;
-
-public class Indexer extends SubsystemBase {
- private TalonFX motor;
- private MockLaserCan simSensor;
- private LaserCanInterface sensor;
-
- private FlywheelSim flywheelSim;
-
- // where the coral is for simulation
- // in meters
- private double simCoralPos;
-
- private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
-
- public Indexer() {
- motor = new TalonFX(IdConstants.INDEXER_MOTOR);
- if (Robot.isSimulation()) {
- flywheelSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getNEO(1),
- IndexerConstants.MOMENT_OF_INERTIA, IndexerConstants.GEAR_RATIO), DCMotor.getNEO(1));
-
- // have both interfaces availible
- simSensor = new MockLaserCan();
- sensor = simSensor;
- } else {
- sensor = new LaserCan(IdConstants.INDEXER_SENSOR);
- try {
- sensor.setRangingMode(RangingMode.SHORT);
- sensor.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS);
- sensor.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8));
- } catch (ConfigurationFailedException e) {
- DriverStation.reportError("Indexer LaserCan configuration error", true);
- }
- }
- simCoralPos = IndexerConstants.START_SIM_POS_AT; // initialize it anyway, it's easier
- }
-
- /** Runs the indexer. */
- public void run() {
- motor.set(IndexerConstants.SPEED);
- simCoralPos = IndexerConstants.START_SIM_POS_AT;
- }
-
- public void slow(){
- motor.set(0.6);
- }
-
- /** Reverses the indexer. */
- public void reverse() {
- motor.set(-IndexerConstants.SPEED);
- simCoralPos = IndexerConstants.END_SIM_SENSOR_POS_AT;
- }
-
- /** Stops the indexer */
- public void stop() {
- motor.stopMotor();
- }
-
- /**
- * @return the motor velocity in rotations per minute
- */
- public double getMotor() {
- return inputs.velocity;
- }
-
- /**
- * Gets the LaserCAN's distance reading.
- * If the distance is null, return 314,159
- *
- * @return the distance, in millimeters
- */
- public int getSensorValue() {
- return inputs.sensorDistance;
- }
-
- /**
- * Checks whether a coral is in the indexer.
- * True means nothing is there, false means something is there
- *
- * @return the sensor's state
- */
- @AutoLogOutput(key = "Intake/isIndexerClear")
- public boolean isIndexerClear() {
- return true;
- }
-
- @Override
- public void periodic() {
- //SmartDashboard.putBoolean("Indexer has coral ", isIndexerClear());
-
- if (Robot.isReal()) {
- inputs.velocity = motor.getVelocity().getValueAsDouble() / IndexerConstants.GEAR_RATIO;
- } else {
- inputs.velocity = flywheelSim.getAngularVelocityRPM();
- }
- var measurement = sensor.getMeasurement();
- inputs.sensorDistance = (measurement == null || measurement.status > 0) ? 314159 : measurement.distance_mm;
- Logger.processInputs("Indexer", inputs);
- Logger.recordOutput("Indexer/indexer coral",isIndexerClear());
- }
-
- @Override
- public void simulationPeriodic() {
- flywheelSim.setInput(motor.get() * Constants.ROBOT_VOLTAGE);
- flywheelSim.update(Constants.LOOP_TIME);
-
- // pretend we have a fake coral
- simCoralPos += flywheelSim.getAngularVelocityRPM() / 60. * Constants.LOOP_TIME
- * IndexerConstants.WHEEL_CIRCUMFERENCE;
-
- // toggle the sensor (values are backwards because that's how the sensor works)
- simSensor.setMeasurementPartialSim(0, // 0 == valid measurement
- (simCoralPos < IndexerConstants.START_SIM_SENSOR_POS_AT
- || simCoralPos > IndexerConstants.END_SIM_SENSOR_POS_AT)
- ? IndexerConstants.MEASUREMENT_THRESHOLD * 2
- : 0,
- 1000 // IDK what this is exactly, but 1000 seems good
- );
- }
-}
+++ /dev/null
-package frc.robot.subsystems.indexer;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface IndexerIO {
- @AutoLog
- public class IndexerIOInputs{
- public double velocity = 0.0;
- public int sensorDistance = 0;
- }
-}
+++ /dev/null
-package frc.robot.subsystems.intake;
-
-import org.littletonrobotics.junction.AutoLogOutput;
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-
-import au.grapplerobotics.LaserCan;
-import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-import frc.robot.constants.IntakeConstants;
-
-
-public class Intake extends SubsystemBase {
- private final TalonFX rollerMotor = new TalonFX(IdConstants.INTAKE_ROLLER);
- private final TalonFX pivotMotor = new TalonFX(IdConstants.INTAKE_PIVOT, Constants.CANIVORE_CAN);
-
- private SingleJointedArmSim stowArmSim;
- private Mechanism2d stowMechanism2d;
- private MechanismLigament2d stowWheelLigament;
-
- private final double positionTolerance = 5;
-
- private final PIDController stowPID = new PIDController(0.015, 0, 0);
- private double power;
-
- private LaserCan laserCan;
- private boolean hasCoral = false;
- private boolean isMoving = false;
- private Timer laserCanSimTimer;
- private DCMotor dcMotor = DCMotor.getKrakenX60(1);
- private ArmFeedforward feedforward = new ArmFeedforward(0,
- Constants.GRAVITY_ACCELERATION * IntakeConstants.CENTER_OF_MASS_DIST * IntakeConstants.MASS
- / IntakeConstants.PIVOT_GEAR_RATIO * dcMotor.rOhms / dcMotor.KtNMPerAmp / Constants.ROBOT_VOLTAGE,
- 0);
- private double startPosition = 90;
-
- private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
-
- public Intake() {
- if (RobotBase.isSimulation()) {
- stowMechanism2d = new Mechanism2d(10, 10);
- stowWheelLigament = stowMechanism2d.getRoot("Root", 5, 5)
- .append(new MechanismLigament2d("Intake", 4, startPosition));
- SmartDashboard.putData("Intake pivot", stowMechanism2d);
- stowArmSim = new SingleJointedArmSim(
- dcMotor,
- IntakeConstants.PIVOT_GEAR_RATIO,
- IntakeConstants.MOMENT_OFiNERTIA,
- IntakeConstants.ARM_LENGTH,
- Math.toRadians(0),
- Math.toRadians(90),
- true,
- Units.degreesToRadians(startPosition));
- laserCanSimTimer = new Timer();
- } else {
- // laserCan = new LaserCan(IdConstants.INTAKE_LASER_CAN);
- // try {
- // laserCan.setRangingMode(RangingMode.SHORT);
- // laserCan.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS);
- // laserCan.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8));
- // } catch (ConfigurationFailedException e) {
- // DriverStation.reportError("Intake LaserCan configuration error", true);
- // }
- }
- rollerMotor.getConfigurator().apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake).withInverted(InvertedValue.CounterClockwise_Positive));
- pivotMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive));
- pivotMotor.setPosition(Units.degreesToRotations(startPosition) * IntakeConstants.PIVOT_GEAR_RATIO);
- pivotMotor.setNeutralMode(NeutralModeValue.Coast);
- stowPID.setTolerance(positionTolerance);
-
- setAngle(startPosition);
-
- rollerMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(40).withStatorCurrentLimitEnable(true));
- }
-
- /**
- * publishes stuff to smartdashboard
- */
- @SuppressWarnings("unused")
- private void publish() {
- SmartDashboard.putNumber("Stow Motor Position", getPivotAngle());
- SmartDashboard.putNumber("Target Angle", stowPID.getSetpoint());
- SmartDashboard.putNumber("Roller Motor Power", power);
-
- SmartDashboard.putBoolean("Has Coral", hasCoral());
- SmartDashboard.putBoolean("Stow Arm Sim - Is Stowed", isAtSetpoint(90));
- SmartDashboard.putBoolean("Stow Arm Sim - Is Unstowed", isAtSetpoint(0));
- SmartDashboard.putBoolean("Is Roller Active", rollerMotor.get() > 0);
- }
-
- @Override
- public void periodic() {
- //publish();
- // SmartDashboard.putNumber("angle", getPivotAngle());
- // SmartDashboard.putBoolean("Intake has coral", hasCoral());
-
- double position = getPivotAngle();
- power = stowPID.calculate(position) + feedforward.calculate(Units.degreesToRadians(position), 0);
- power = MathUtil.clamp(power, -1, 1);
- pivotMotor.set(power);
- if (laserCan != null) {
- Measurement measurement = laserCan.getMeasurement();
- hasCoral = measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT
- && measurement.distance_mm <= 1000 * IntakeConstants.DETECT_CORAL_DIST;
- }
- if(RobotBase.isSimulation()) {
- inputs.measuredPivotPosition = Units.radiansToDegrees(stowArmSim.getAngleRads());
- } else {
- inputs.measuredPivotPosition = Units.rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble()) / IntakeConstants.PIVOT_GEAR_RATIO;
- }
- inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble();
- Logger.processInputs("Intake", inputs);
-
- Logger.recordOutput("Intake/PivotSetpoint", stowPID.getSetpoint());
- }
-
- @Override
- public void simulationPeriodic() {
- stowArmSim.setInputVoltage(power * Constants.ROBOT_VOLTAGE);
- stowArmSim.update(Constants.LOOP_TIME);
- stowWheelLigament.setAngle(Units.radiansToDegrees(stowArmSim.getAngleRads()));
- if (!isMoving) {
- laserCanSimTimer.reset();
- laserCanSimTimer.start();
- hasCoral = false;
- } else {
- if (isAtSetpoint()) {
- laserCanSimTimer.start();
- }
- hasCoral = laserCanSimTimer.hasElapsed(0.5) && !laserCanSimTimer.hasElapsed(1);
- }
- }
-
- /**
- * Gets the rotation of the intake.
- *
- * @return the rotation of the intake (in degrees).
- */
- public double getPivotAngle() {
- return inputs.measuredPivotPosition;
- }
-
- public PIDController getPID() {
- return stowPID;
- }
-
- /**
- * Checks if intake has coral.
- *
- * @return Boolean (True if has Coral, False otherwise)
- */
- @AutoLogOutput(key = "Intake/HasCoral")
- public boolean hasCoral() {
- return hasCoral;
- }
-
- /**
- * Checks if motor is at current setpoint.
- *
- * @return Boolean (True if at setpoint, False otherwise)
- */
- public boolean isAtSetpoint(double setpoint) {
- return Math.abs(getPivotAngle() - setpoint) < positionTolerance;
- }
-
- /**
- * Returns whether or not the intake is at its setpoint
- *
- * @return True if it is at the setpoint, false otherwise
- */
- @AutoLogOutput(key = "Intake/AtSetPoint")
- public boolean isAtSetpoint() {
- return stowPID.atSetpoint();
- }
-
- /**
- * Sets the desired angle of the intake, mostly to stow or unstow.
- *
- * @param angle desired angle of the intake in degrees
- */
- public void setAngle(double angle) {
- stowPID.setSetpoint(angle);
- }
-
- /**
- * Sets the speed of the roller motor.
- *
- * @param power The desired speed of the roller, between 0 and 1.
- */
- public void setSpeed(double power) {
- rollerMotor.set(power);
- isMoving = Math.abs(power) < 0.01;
-
- }
-
- /**
- * Moves the intake up, doesn't stop it.
- */
- public void stow() {
- stowPID.setSetpoint(IntakeConstants.STOW_SETPOINT);
- }
-
- /**
- * Moves the intake down.
- */
- public void unstow() {
- stowPID.setSetpoint(IntakeConstants.INTAKE_SETPOINT);
- }
-
- /**
- * Stops the motor.
- */
- public void deactivate() {
- rollerMotor.set(0);
- }
-
- /**
- * Starts the motor.
- */
- public void activate(){
- rollerMotor.set(IntakeConstants.INTAKE_MOTOR_POWER);
- }
-}
-
+++ /dev/null
-package frc.robot.subsystems.intake;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface IntakeIO {
- @AutoLog
- public static class IntakeIOInputs {
- public double rollerVelocity = 0.0;
- public double measuredPivotPosition = 0.0;
- }
-}
+++ /dev/null
-package frc.robot.subsystems.outtake;
-
-import edu.wpi.first.wpilibj.simulation.DIOSim;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-/**
- * Abstract class for the outtake. All commands should use this subsystem
- */
-public abstract class Outtake extends SubsystemBase {
- /** Coral detected before the rollers */
- protected DIOSim dioInputLoaded;
- /** Coral detected after the rollers */
- protected DIOSim dioInputEjecting;
-
- protected abstract double getMotorSpeed();
-
- public void simulationPeriodic(){}
-
- /** Set the motor power to move the coral */
- public abstract void setMotor(double power);
-
- /** stop the coral motor */
- public void stop() {
- setMotor(0);
- }
-
- /** start spinning the rollers to eject the coral */
- public abstract void outtake();
-
- public abstract boolean coralLoaded();
-
- /**
- * Coral is at the ejecting beam break sensor.
- * @return coral is interrupting the beam breaker.
- */
- public abstract boolean coralEjecting();
-
- public abstract void reverse();
-
- public void removeAlgae(){
- setMotor(-0.6);
- }
-
- public void intakeAlgaeReef() {
- setMotor(-0.6);
- }
-
- public void outtakeAlgae() {
- setMotor(0.9);
- }
-}
+++ /dev/null
-package frc.robot.subsystems.outtake;
-
-
-import com.revrobotics.ColorSensorV3;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkFlex;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.config.SparkFlexConfig;
-
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.simulation.DIOSim;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.constants.IdConstants;
-
-
-public class OuttakeAlpha extends Outtake {
-
- private SparkFlex motor = new SparkFlex(IdConstants.OUTTAKE_MOTOR_ALPHA, MotorType.kBrushless);
- private double power;
-
-
- /** Coral detected before the rollers */
- private final ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort);
- /** Coral detected after the rollers */
- private DigitalInput digitalInputEjecting = new DigitalInput(IdConstants.OUTTAKE_DIO_EJECTING);
-
-
-
- public OuttakeAlpha(){
- motor.configure(new SparkFlexConfig()
- .inverted(true)
- .idleMode(IdleMode.kBrake),
- ResetMode.kResetSafeParameters,
- PersistMode.kNoPersistParameters
- );
- if (RobotBase.isSimulation()){
- // object that will control the ejecting sensor
- dioInputEjecting = new DIOSim(digitalInputEjecting);
- // assume coral is loaded
- dioInputLoaded.setValue(false);
- // we are not ejecting
- dioInputEjecting.setValue(true);
- }
- }
-
- @Override
- protected double getMotorSpeed(){
- return motor.get();
- }
-
- @Override
- public void periodic(){
- motor.set(power);
- // SmartDashboard.putBoolean("Coral loaded", coralLoaded());
- // SmartDashboard.putBoolean("Coral ejected", coralEjecting());
-
- }
-
- /** Set the motor power to move the coral */
- public void setMotor(double power){
- this.power = power;
- }
-
-
- /** start spinning the rollers to eject the coral */
- public void outtake(){
- // assumes the coral is present
- // if the coral is not present, we should not bother to spin the rollers
- setMotor(SmartDashboard.getNumber("wheel speed", 0.2));
- // this starts the motor... what needs to be done later?
- }
-
- /**
- * Coral is at the ejecting beam break sensor.
- * @return coral is interrupting the beam breaker.
- */
- public boolean coralEjecting() {
- return !digitalInputEjecting.get();
- }
-
-
- public void reverse(){
- setMotor(-0.2);
- }
-
-
- public boolean isSimulation(){
- return RobotBase.isSimulation();
- }
-
- public int getProximity() {
- return colorSensor.getProximity(); // Returns 0 (far) to ~2047 (very close)
- }
-
- // coral detection from color sensor
- public boolean coralLoaded() {
- //this is about 1/2inch away -- might have to change based on placement
- if (getProximity() > 800) {
- return true;
- }
- return false;
- }
-}
+++ /dev/null
-package frc.robot.subsystems.outtake;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.revrobotics.ColorSensorV3;
-
-import edu.wpi.first.hal.I2CJNI;
-import frc.robot.constants.IdConstants;
-
-public class OuttakeComp extends Outtake {
-
- private TalonFX motor = new TalonFX(IdConstants.OUTTAKE_MOTOR_COMP);
- private double power;
-
- /** Coral detected before the rollers */
- private ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort);
-
- OuttakeIOIntakesAutoLogged inputs = new OuttakeIOIntakesAutoLogged();
-
- public OuttakeComp(){
- motor.getConfigurator().apply(new MotorOutputConfigs()
- .withInverted(InvertedValue.Clockwise_Positive)
- .withNeutralMode(NeutralModeValue.Brake)
- );
- }
-
- @Override
- protected double getMotorSpeed() {
- return power;
- }
-
- @Override
- public void periodic(){
- motor.set(power);
- // SmartDashboard.putBoolean("Coral loaded", coralLoaded());
- // SmartDashboard.putBoolean("Coral ejected", coralEjecting());
-
- inputs.motorVelocity = motor.getVelocity().getValueAsDouble();
- Logger.processInputs("Outtake", inputs);
- //Logger.recordOutput("Outtake/Sensor", getProximity());
- //Logger.recordOutput("Outtake/SensorConnected", colorSensor.isConnected());
- }
-
- /** Set the motor power to move the coral */
- public void setMotor(double power){
- this.power = power;
- }
-
- /** start spinning the rollers to eject the coral */
- public void outtake(){
- setMotor(-0.3);
- }
-
- /**
- * Coral is in the outtake.
- * @return The same thing as coralLoaded(), for compatibility with previous code
- */
- public boolean coralEjecting() {
- return coralLoaded();
- }
-
-
- public void reverse(){
- setMotor(0.2);
- }
-
- public int getProximity() {
- inputs.proximity = colorSensor.getProximity();
- if (inputs.proximity > 0){
- return inputs.proximity;
- }
- else{
- I2CJNI.i2CClose(1);
- colorSensor = new ColorSensorV3(IdConstants.i2cPort);
- return inputs.proximity = colorSensor.getProximity(); // Returns 0 (far) to ~2047 (very close)
- }
- }
-
- // coral detection from color sensor
- public boolean coralLoaded() {
- return getProximity() > 2000;
- }
-}
+++ /dev/null
-package frc.robot.subsystems.outtake;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface OuttakeIO {
- @AutoLog
- public static class OuttakeIOIntakes {
- public double motorVelocity = 0.0;
- public int proximity = 0;
- }
-}