diff --git a/logs/Log_24-03-10_13-36-48.wpilog b/logs/Log_24-03-10_13-36-48.wpilog new file mode 100644 index 00000000..360c422a Binary files /dev/null and b/logs/Log_24-03-10_13-36-48.wpilog differ diff --git a/logs/Log_c4c28accb5c6554e.wpilog b/logs/Log_c4c28accb5c6554e.wpilog new file mode 100644 index 00000000..23519fd8 Binary files /dev/null and b/logs/Log_c4c28accb5c6554e.wpilog differ diff --git a/src/main/deploy/pathplanner/autos/3 Piece Under Stage.auto b/src/main/deploy/pathplanner/autos/3 Piece Under Stage.auto new file mode 100644 index 00000000..6278b0fb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Piece Under Stage.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.51, + "y": 5.56 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shooting_From_Subwoofer" + } + }, + { + "type": "path", + "data": { + "pathName": "(M3A) Shoot first note, move to second note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + }, + { + "type": "path", + "data": { + "pathName": "(M) Going under stage" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "path", + "data": { + "pathName": "(M2) Under stage reverse" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece Auto.auto new file mode 100644 index 00000000..24b43bf6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece Auto.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 6.82 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "(L1) Shoot first note, exit wing" + } + } + ] + } + }, + "folder": "Lit Left Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece Auto.auto new file mode 100644 index 00000000..a80e345f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece Auto.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 6.82 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shooting_From_Subwoofer" + } + }, + { + "type": "path", + "data": { + "pathName": "(L3A) Shoot 1st note, 2nd note transit" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + }, + { + "type": "path", + "data": { + "pathName": "(L3B) After shooting, move to third note pickup" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece Auto.auto new file mode 100644 index 00000000..918d3b4d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece Auto.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6654451313653237, + "y": 6.639585345679005 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "(L3A) Shoot 1st note, 2nd note transit" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + }, + { + "type": "path", + "data": { + "pathName": "(L3B) After shooting, move to third note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "path", + "data": { + "pathName": "(L3C) Move back, shoot third note" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + } + ] + } + }, + "folder": "Lit Left Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Middle 1 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Middle 1 Piece Auto.auto new file mode 100644 index 00000000..2204f87b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Middle 1 Piece Auto.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.51, + "y": 5.56 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shooting_From_Subwoofer" + } + }, + { + "type": "path", + "data": { + "pathName": "(M1) Shoot first note, exit wing" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Middle 2 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Middle 2 Piece Auto.auto new file mode 100644 index 00000000..71b4fa0d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Middle 2 Piece Auto.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.51, + "y": 5.56 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shooting_From_Subwoofer" + } + }, + { + "type": "path", + "data": { + "pathName": "(M3A) Shoot first note, move to second note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + }, + { + "type": "path", + "data": { + "pathName": "(M) Going under stage" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Middle 3 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Middle 3 Piece Auto.auto new file mode 100644 index 00000000..cc1487cb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Middle 3 Piece Auto.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.56 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "(M3A) Shoot first note, move to second note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + }, + { + "type": "path", + "data": { + "pathName": "(M3B) After shooting, move to third note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "path", + "data": { + "pathName": "(M3C) Move back, shoot third note" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + } + ] + } + }, + "folder": "Macho Middle Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece Auto.auto new file mode 100644 index 00000000..a1060869 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece Auto.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.76, + "y": 4.24 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "(R1) Shoot first note, exit wing" + } + } + ] + } + }, + "folder": "Rad Right Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece Auto.auto new file mode 100644 index 00000000..9e202701 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece Auto.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.76, + "y": 4.24 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "(R3A) Shoot first note, move to second note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + }, + { + "type": "path", + "data": { + "pathName": "(R3B) After shooting, move to third note pickup" + } + } + ] + } + }, + "folder": "Rad Right Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece Auto.auto new file mode 100644 index 00000000..750ed649 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece Auto.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.64, + "y": 4.39 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "(R3A) Shoot first note, move to second note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + }, + { + "type": "path", + "data": { + "pathName": "(R3B) After shooting, move to third note pickup" + } + }, + { + "type": "named", + "data": { + "name": "PickUpNoteWithLimelight" + } + }, + { + "type": "path", + "data": { + "pathName": "(R3C) Move back, shoot third note" + } + }, + { + "type": "named", + "data": { + "name": "AutoShootWithAutoAlign" + } + } + ] + } + }, + "folder": "Rad Right Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Two Piece Start Middle.auto b/src/main/deploy/pathplanner/autos/Two Piece Start Middle.auto new file mode 100644 index 00000000..9fc42605 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Two Piece Start Middle.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2437546130441752, + "y": 5.410406484151056 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shooting_From_Subwoofer" + } + }, + { + "type": "path", + "data": { + "pathName": "(M3A) Shoot first note, move to second note pickup" + } + }, + { + "type": "named", + "data": { + "name": "Automated_Shooting_With_Automatic_Alignment" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(L1) Shoot first note, exit wing.path b/src/main/deploy/pathplanner/paths/(L1) Shoot first note, exit wing.path new file mode 100644 index 00000000..4095f8c4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(L1) Shoot first note, exit wing.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.72394368066646, + "y": 6.8150809935824155 + }, + "prevControl": null, + "nextControl": { + "x": 2.7616431479893855, + "y": 7.039325432570105 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.5835483689969765, + "y": 7.497564068762342 + }, + "prevControl": { + "x": 3.24913105883219, + "y": 7.097823981871242 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Left Ring Autos", + "previewStartingState": { + "rotation": 60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(L3A) Shoot 1st note, 2nd note transit.path b/src/main/deploy/pathplanner/paths/(L3A) Shoot 1st note, 2nd note transit.path new file mode 100644 index 00000000..6ed3f852 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(L3A) Shoot 1st note, 2nd note transit.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6654451313653237, + "y": 6.639585345679005 + }, + "prevControl": null, + "nextControl": { + "x": 1.3966769976295312, + "y": 6.844330268232984 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0011620070746092, + "y": 6.990576641485824 + }, + "prevControl": { + "x": 1.3771774811958186, + "y": 6.844330268232984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Left Ring Autos", + "previewStartingState": { + "rotation": 60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(L3B) After shooting, move to third note pickup.path b/src/main/deploy/pathplanner/paths/(L3B) After shooting, move to third note pickup.path new file mode 100644 index 00000000..7139402f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(L3B) After shooting, move to third note pickup.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.0429695047424277, + "y": 6.993104356579084 + }, + "prevControl": null, + "nextControl": { + "x": 4.680151126684019, + "y": 7.180210827658122 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.264559258463246, + "y": 7.414093916506922 + }, + "prevControl": { + "x": 4.890645906647938, + "y": 7.191904982100564 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Left Ring Autos", + "previewStartingState": { + "rotation": -151.92751306414704, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(L3C) Move back, shoot third note.path b/src/main/deploy/pathplanner/paths/(L3C) Move back, shoot third note.path new file mode 100644 index 00000000..88cc2a41 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(L3C) Move back, shoot third note.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.656163637113599, + "y": 7.449176379834242 + }, + "prevControl": null, + "nextControl": { + "x": 6.937122934074928, + "y": 7.098351746561043 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.826477852385904, + "y": 6.4902557155541665 + }, + "prevControl": { + "x": 5.639071790964095, + "y": 6.864468657712245 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -165.25643716352937, + "rotateFast": false + }, + "reversed": false, + "folder": "Left Ring Autos", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(M1) Shoot first note, exit wing.path b/src/main/deploy/pathplanner/paths/(M1) Shoot first note, exit wing.path new file mode 100644 index 00000000..02d23283 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(M1) Shoot first note, exit wing.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.51, + "y": 5.56 + }, + "prevControl": null, + "nextControl": { + "x": 4.311854704469505, + "y": 5.781606622595669 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.954039181237508, + "y": 6.015600819800215 + }, + "prevControl": { + "x": 5.013837296083145, + "y": 7.439065519461206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Ring Autos", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(M2) Under stage reverse.path b/src/main/deploy/pathplanner/paths/(M2) Under stage reverse.path new file mode 100644 index 00000000..627aed7f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(M2) Under stage reverse.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.718745418488464, + "y": 4.114397967513275 + }, + "prevControl": null, + "nextControl": { + "x": 6.408052721093567, + "y": 3.617160298453614 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.697619936807571, + "y": 5.294118711752863 + }, + "prevControl": { + "x": 5.19908270220341, + "y": 4.20214579146498 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -4.014175695410979, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Ring Autos", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(M3A) Shoot first note, move to second note pickup.path b/src/main/deploy/pathplanner/paths/(M3A) Shoot first note, move to second note pickup.path new file mode 100644 index 00000000..7b3dca0d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(M3A) Shoot first note, move to second note pickup.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5136740962318043, + "y": 5.557362183607978 + }, + "prevControl": null, + "nextControl": { + "x": 1.8451658756049116, + "y": 5.557362183607978 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.784236337703446, + "y": 5.557362183607978 + }, + "prevControl": { + "x": 2.462494316547194, + "y": 5.557362183607978 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Pickup_Note_Without_Limelight" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(M3B) After shooting, move to third note pickup.path b/src/main/deploy/pathplanner/paths/(M3B) After shooting, move to third note pickup.path new file mode 100644 index 00000000..b22dd307 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(M3B) After shooting, move to third note pickup.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.148216894724387, + "y": 5.55472336015897 + }, + "prevControl": null, + "nextControl": { + "x": 5.580601018751895, + "y": 7.086657592118603 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.627078046178885, + "y": 5.765218140122891 + }, + "prevControl": { + "x": 6.960511242959807, + "y": 6.104348618953648 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Ring Autos", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(M3C) Move back, shoot third note.path b/src/main/deploy/pathplanner/paths/(M3C) Move back, shoot third note.path new file mode 100644 index 00000000..3da47d4c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(M3C) Move back, shoot third note.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.621081173786278, + "y": 5.788606449007769 + }, + "prevControl": null, + "nextControl": { + "x": 6.235473667528531, + "y": 6.583808951093687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.259161566756181, + "y": 5.788606449007769 + }, + "prevControl": { + "x": 5.849566570928014, + "y": 6.876162812154685 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Ring Autos", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(R1) Shoot first note, exit wing.path b/src/main/deploy/pathplanner/paths/(R1) Shoot first note, exit wing.path new file mode 100644 index 00000000..8bfe731a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(R1) Shoot first note, exit wing.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.76, + "y": 4.24 + }, + "prevControl": null, + "nextControl": { + "x": 1.26, + "y": 3.373974596215562 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.7492942586835305, + "y": 0.7702308991316326 + }, + "prevControl": { + "x": 5.7492942586835305, + "y": 0.7702308991316326 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Right Ring Autos", + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(R3A) Shoot first note, move to second note pickup.path b/src/main/deploy/pathplanner/paths/(R3A) Shoot first note, move to second note pickup.path new file mode 100644 index 00000000..c4e39192 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(R3A) Shoot first note, move to second note pickup.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7629427135338847, + "y": 4.241144824332404 + }, + "prevControl": null, + "nextControl": { + "x": 1.4649253051475228, + "y": 4.075398934645851 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9914122488577533, + "y": 4.085148692862707 + }, + "prevControl": { + "x": 1.4759528094554764, + "y": 4.186507290393498 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Right Ring Autos", + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(R3B) After shooting, move to third note pickup.path b/src/main/deploy/pathplanner/paths/(R3B) After shooting, move to third note pickup.path new file mode 100644 index 00000000..318af703 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/(R3B) After shooting, move to third note pickup.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9026396514331485, + "y": 4.128036518181299 + }, + "prevControl": null, + "nextControl": { + "x": 2.4231793192931113, + "y": 2.479160741797268 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.428277420657405, + "y": 2.4440782784699477 + }, + "prevControl": { + "x": 5.299941312133336, + "y": 1.0407797453771557 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "DeployIntake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Right Ring Autos", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fbe7cda1..7f5052c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,10 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.RealClimberIO; +import frc.robot.subsystems.climber.commands.CmdClimberMove; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.intake.Intake; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3f45b7ad..f5c0a183 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -14,6 +14,11 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.*; + +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -25,15 +30,21 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.HardwareConstants; import frc.robot.subsystems.vision.VisionIOInputsAutoLogged; +import frc.robot.subsystems.visiondrive.VisionDriveIO; +import frc.robot.subsystems.multisubsystemcommands.CmdShootOnTheMove; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; import frc.robot.subsystems.vision.VisionIO; import frc.robot.util.autonomous.AutonomousPathGenerator; @@ -41,6 +52,8 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + public class Drive extends SubsystemBase { private final GyroIO _gyroIO; @@ -55,6 +68,7 @@ public class Drive extends SubsystemBase { private SwerveDriveKinematics _kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d _rawGyroRotation = new Rotation2d(); + private Rotation2d _shootOnMoveGyro = new Rotation2d(); private SwerveModulePosition[] _lastModulePositions = // For delta tracking new SwerveModulePosition[] { new SwerveModulePosition(), @@ -70,6 +84,9 @@ public class Drive extends SubsystemBase { private Translation2d _centerOfRotation; public Field2d _field; + private BooleanSubscriber _isCmdShootOnMoveRunning; + private NetworkTable _adkitNetworkTable; + public Drive( GyroIO gyroIO, VisionIO visionIO, @@ -79,6 +96,12 @@ public Drive( ModuleIO brModuleIO) { this._gyroIO = gyroIO; this._visionIO = visionIO; + // this._visionDriveIO = visionDriveIO; + + _isCmdShootOnMoveRunning = _adkitNetworkTable.getBooleanTopic("RealOutputs/Drive/shootOnTheMove/isRunning") + .subscribe(false); + + _modules[0] = new Module(flModuleIO, 0); _modules[1] = new Module(frModuleIO, 1); @@ -87,6 +110,8 @@ public Drive( AutonomousPathGenerator.configure(this, _kinematics, getModuleStates()); + PPHolonomicDriveController.setRotationTargetOverride(this::getRotationTargetOverride); + // =============== START CONFIGURATION FOR SYSID =============== _sysId = new SysIdRoutine( new SysIdRoutine.Config( @@ -265,6 +290,27 @@ public void stopWithX() { stop(); } + public void setShootOnMoveGyro(Rotation2d futureGyro){ + _shootOnMoveGyro = futureGyro; + } + + public Rotation2d getShootOnMoveGyro(){ + return _shootOnMoveGyro; + } + + public Optional getRotationTargetOverride(){ + // Some condition that should decide if we want to override rotation + if(_isCmdShootOnMoveRunning.get()){ + return Optional.of(_shootOnMoveGyro); + } else if(Shooter.isAutoShootEnabled()) { + // Return an optional containing the rotation override (this should be a field relative rotation) + return Optional.of(this.getRotation2dToSpeaker(this.getPose().getTranslation())); + } else { + // return an empty optional when we don't want to override the path's rotation + return Optional.empty(); + } +} + /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return _sysId.quasistatic(direction); diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java index 356f00aa..84ff010f 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java @@ -38,6 +38,7 @@ public void execute() { if (_intakeSubsystem.hasNote()) { double radius = _drive.getRadiusToSpeakerInMeters(); _shooterSubsystem.runShooterForRadius(radius); + Logger.recordOutput("Shooter/RadiusToSpeaker", radius); } else { // We aren't holding a note, so stay in unknown for now _shooterSubsystem.runShooterForZone(ShooterZone.Unknown); diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdShootOnTheMove.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdShootOnTheMove.java index 40e20fce..505be09e 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdShootOnTheMove.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdShootOnTheMove.java @@ -12,6 +12,8 @@ import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; @@ -39,6 +41,7 @@ public class CmdShootOnTheMove extends Command { private Command _autoAdjustCommand; private boolean _isFinished; + private boolean _cmdIsRunning; private Translation2d _currentRobotTranslation; private double _currentAngleRadians; @@ -86,6 +89,8 @@ public CmdShootOnTheMove(Drive drivetrainSubsystem, _trigger = triggerAxis; _translationXSupplier = translationXSupplier; _translationYSupplier = translationYSupplier; + //_autoAdjustCommand = autoAdjustCommand; + _cmdIsRunning = true; _rotationPID = new PIDController( DriveConstants.SHOOT_ON_THE_MOVE_P, @@ -106,6 +111,7 @@ public CmdShootOnTheMove(Drive drivetrainSubsystem, @Override public void initialize() { _isFinished = false; + _cmdIsRunning = true; // Cancel CmdAdjustShooterAutomatically while this command runs; reenable when it finishes. _shooter.setAutoShootEnabled(false); } @@ -120,6 +126,7 @@ public void execute() { } } + // Get *translation only* of the robot _currentRobotTranslation = _drive.getPose().getTranslation(); @@ -146,10 +153,14 @@ public void execute() { // Angle to the speaker from the future position as a Rotation2d. _futureAngleToSpeaker = _drive.getRotation2dToSpeaker(_futureRobotTranslation); + // + _drive.setShootOnMoveGyro(_futureRobotTranslation.getAngle()); + // All PID calculations are done in radians, so convert our setpoint from a // Rotation2d to radians. _rotationPID.setSetpoint(_futureAngleToSpeaker.getRadians()); + // Angle in radians (omegaRadiansPerSecond) to pass to the drivetrain later in a // ChassisSpeeds object _correctedRotation = _rotationPID.calculate( @@ -181,6 +192,9 @@ public void execute() { } } } + + // Adds a boolean that tells if this command is running to NetworkTables. + Logger.recordOutput("Drive/shootOnTheMove/isRunning", _cmdIsRunning); } // Called once the command ends or is interrupted. @@ -195,6 +209,7 @@ public void end(boolean interrupted) { _shotTimer.stop(); _shotTimer.reset(); _hasRunOnce = false; + _cmdIsRunning = false; // Reenable CmdAdjustShooterAutomatically because this command is finished. _shooter.setAutoShootEnabled(true); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b93a65a6..208f26d8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -248,11 +248,11 @@ public ShooterZone getZoneFromRadius(double radius) { return ShooterZone.Unknown; } - public boolean isAutoShootEnabled() { + public static boolean isAutoShootEnabled() { return _autoShootEnabled; } - public void setAutoShootEnabled(boolean enabled) { + public static void setAutoShootEnabled(boolean enabled) { _autoShootEnabled = enabled; } diff --git a/src/main/java/frc/robot/subsystems/visiondrive/RealVisionDriveIO.java b/src/main/java/frc/robot/subsystems/visiondrive/RealVisionDriveIO.java index 42a0c96f..d76a3ada 100644 --- a/src/main/java/frc/robot/subsystems/visiondrive/RealVisionDriveIO.java +++ b/src/main/java/frc/robot/subsystems/visiondrive/RealVisionDriveIO.java @@ -1,7 +1,15 @@ package frc.robot.subsystems.visiondrive; +import java.util.Optional; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.multisubsystemcommands.CmdShootOnTheMove; public class RealVisionDriveIO implements VisionDriveIO { private NetworkTable _table; diff --git a/src/main/java/frc/robot/util/autonomous/AutonomousPathGenerator.java b/src/main/java/frc/robot/util/autonomous/AutonomousPathGenerator.java index 712c7b22..791cbb2f 100644 --- a/src/main/java/frc/robot/util/autonomous/AutonomousPathGenerator.java +++ b/src/main/java/frc/robot/util/autonomous/AutonomousPathGenerator.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; @@ -37,6 +38,8 @@ public static void configure(Drive drive, SwerveDriveKinematics kinematics, Swer && DriverStation.getAlliance().get() == Alliance.Red, drive); + PPHolonomicDriveController.setRotationTargetOverride(drive::getRotationTargetOverride); + Pathfinding.setPathfinder(new LocalADStar()); PathPlannerLogging.setLogActivePathCallback(