diff --git a/src/main/deploy/pathplanner/autos/Center Field Limelight(No Preload).auto b/src/main/deploy/pathplanner/autos/Center Field Limelight(No Preload).auto index ff797653..74c6cdce 100644 --- a/src/main/deploy/pathplanner/autos/Center Field Limelight(No Preload).auto +++ b/src/main/deploy/pathplanner/autos/Center Field Limelight(No Preload).auto @@ -24,22 +24,9 @@ } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Field Limelight 2" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CW" } }, { @@ -48,54 +35,22 @@ "pathName": "Center Field Limelight 3" } }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Center Field Limelight 4" } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Field Limelight 4" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CW" } }, { @@ -104,41 +59,22 @@ "pathName": "Center Field Limelight 5" } }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Center Field Limelight 6" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Center Field Limelight 6" + "name": "Limelight Intake CW" } } ] diff --git a/src/main/deploy/pathplanner/autos/Center Field Limelight.auto b/src/main/deploy/pathplanner/autos/Center Field Limelight.auto index efc39c42..1034fc2e 100644 --- a/src/main/deploy/pathplanner/autos/Center Field Limelight.auto +++ b/src/main/deploy/pathplanner/autos/Center Field Limelight.auto @@ -11,35 +11,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "PassToOuttake" - } - }, { "type": "named", "data": { - "name": "ArmToGround" + "name": "Shoot" } }, { @@ -49,22 +24,9 @@ } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Field Limelight 2" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CW" } }, { @@ -73,54 +35,22 @@ "pathName": "Center Field Limelight 3" } }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Center Field Limelight 4" } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Field Limelight 4" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CW" } }, { @@ -129,41 +59,22 @@ "pathName": "Center Field Limelight 5" } }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Center Field Limelight 6" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Center Field Limelight 6" + "name": "Limelight Intake CW" } } ] diff --git a/src/main/deploy/pathplanner/autos/Center Forward.auto b/src/main/deploy/pathplanner/autos/Center Forward.auto new file mode 100644 index 00000000..54e89612 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Forward.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.335176614373586, + "y": 5.535530663936646 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Forward" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Limelight 4 Piece.auto b/src/main/deploy/pathplanner/autos/Center Limelight 4 Piece.auto index b535d0e4..83de7c0c 100644 --- a/src/main/deploy/pathplanner/autos/Center Limelight 4 Piece.auto +++ b/src/main/deploy/pathplanner/autos/Center Limelight 4 Piece.auto @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "Limelight Intake CCW" + "name": "Limelight Intake Straight" } }, { @@ -44,7 +44,7 @@ { "type": "named", "data": { - "name": "Limelight Intake CW" + "name": "Limelight Intake CCW" } }, { @@ -62,7 +62,7 @@ { "type": "named", "data": { - "name": "Limelight Intake CCW" + "name": "Limelight Intake CW" } }, { @@ -104,7 +104,7 @@ { "type": "named", "data": { - "name": "Limelight Intake CCW" + "name": "Limelight Intake Straight" } } ] diff --git a/src/main/deploy/pathplanner/autos/Left Forward.auto b/src/main/deploy/pathplanner/autos/Left Forward.auto new file mode 100644 index 00000000..b11a61e3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Forward.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6842044526319189, + "y": 6.6998765551505 + }, + "rotation": 63.19916505304543 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Forward" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Limelight 4 Piece.auto b/src/main/deploy/pathplanner/autos/Left Limelight 4 Piece.auto index c4731918..37109844 100644 --- a/src/main/deploy/pathplanner/autos/Left Limelight 4 Piece.auto +++ b/src/main/deploy/pathplanner/autos/Left Limelight 4 Piece.auto @@ -11,204 +11,76 @@ "type": "sequential", "data": { "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "PassToOuttake" - } - }, { "type": "named", "data": { - "name": "ArmToGround" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Limelight 1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Shoot" } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "pathName": "Left Limelight 1" } }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Limelight Intake Straight" } }, { "type": "named", "data": { - "name": "ArmToGround" + "name": "Shoot" } }, { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Limelight 2" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "pathName": "Left Limelight 2" } }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Limelight Intake Straight" } }, { "type": "named", "data": { - "name": "ArmToGround" + "name": "Shoot" } }, { - "type": "deadline", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Limelight 3" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "pathName": "Left Limelight 3" } }, { - "type": "parallel", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "name": "Limelight Intake Straight" } }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Left Limelight 4" } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Limelight 4" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CCW" } }, { @@ -217,54 +89,22 @@ "pathName": "Left Limelight 5" } }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Left Limelight 6" } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Limelight 6" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CCW" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Forward.auto b/src/main/deploy/pathplanner/autos/Right Forward.auto new file mode 100644 index 00000000..afbb601a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Forward.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6842044526319189, + "y": 6.6998765551505 + }, + "rotation": -67.01128319791938 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Forward" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Limelight 1 Piece.auto b/src/main/deploy/pathplanner/autos/Right Limelight 1 Piece.auto index 0a803d43..ac8eadd5 100644 --- a/src/main/deploy/pathplanner/autos/Right Limelight 1 Piece.auto +++ b/src/main/deploy/pathplanner/autos/Right Limelight 1 Piece.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.4048307100172925, - "y": 5.570246205735591 + "x": 0.7134190306340287, + "y": 4.411401278318571 }, "rotation": -61.557071375636745 }, @@ -12,84 +12,39 @@ "data": { "commands": [ { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "pathName": "Right Limelight 1" } }, { - "type": "named", + "type": "path", "data": { - "name": "PassToOuttake" + "pathName": "Right Limelight 2" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Right Limelight 3" } }, { - "type": "deadline", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Limelight 1 Piece" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "pathName": "Right Limelight 4" } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "pathName": "Right Limelight 5" } }, { - "type": "named", + "type": "path", "data": { - "name": "PassToOuttake" - } - }, - { - "type": "named", - "data": { - "name": "ArmToGround" + "pathName": "Right Limelight 6" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Limelight 4 Piece.auto b/src/main/deploy/pathplanner/autos/Right Limelight 4 Piece.auto index dfa0eb3a..bd95a568 100644 --- a/src/main/deploy/pathplanner/autos/Right Limelight 4 Piece.auto +++ b/src/main/deploy/pathplanner/autos/Right Limelight 4 Piece.auto @@ -11,204 +11,76 @@ "type": "sequential", "data": { "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "PassToOuttake" - } - }, { "type": "named", "data": { - "name": "ArmToGround" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Limelight 1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Shoot" } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "pathName": "Right Limelight 1" } }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Limelight Intake Straight" } }, { "type": "named", "data": { - "name": "ArmToGround" + "name": "Shoot" } }, { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Limelight 2" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "pathName": "Right Limelight 2" } }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Limelight Intake Straight" } }, { "type": "named", "data": { - "name": "ArmToGround" + "name": "Shoot" } }, { - "type": "deadline", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Limelight 3" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "pathName": "Right Limelight 3" } }, { - "type": "parallel", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] + "name": "Limelight Intake Straight" } }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Right Limelight 4" } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Limelight 4" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CCW" } }, { @@ -217,54 +89,22 @@ "pathName": "Right Limelight 5" } }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToAprilTagMegaTag2" - } - }, - { - "type": "named", - "data": { - "name": "AimArmSpeakerMT2" - } - } - ] - } - }, { "type": "named", "data": { - "name": "PassToOuttake" + "name": "Shoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "ArmToGround" + "pathName": "Right Limelight 6" } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Limelight 6" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] + "name": "Limelight Intake CCW" } } ] diff --git a/src/main/deploy/pathplanner/paths/Center Field Limelight 1.path b/src/main/deploy/pathplanner/paths/Center Field Limelight 1.path index c1073fe9..69196ddb 100644 --- a/src/main/deploy/pathplanner/paths/Center Field Limelight 1.path +++ b/src/main/deploy/pathplanner/paths/Center Field Limelight 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.358727567327909, - "y": 6.694858784119604 + "x": 2.3796163622861606, + "y": 6.92463552866037 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.6904903631690225, - "y": 6.496415232016215 + "x": 7.372038357308267, + "y": 6.0055285504973055 }, "prevControl": { - "x": 3.298723340449227, - "y": 6.569526014370095 + "x": 5.001160129546722, + "y": 6.903746733702119 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Field Limelight 4.path b/src/main/deploy/pathplanner/paths/Center Field Limelight 4.path index fd315072..5b389ff3 100644 --- a/src/main/deploy/pathplanner/paths/Center Field Limelight 4.path +++ b/src/main/deploy/pathplanner/paths/Center Field Limelight 4.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.894258231264555, - "y": 4.073315016859041 + "x": 7.403371549745644, + "y": 4.219536581566802 }, "prevControl": { - "x": 7.100484022850999, - "y": 4.1777589916502995 + "x": 6.609597341332088, + "y": 4.32398055635806 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Field Limelight 6.path b/src/main/deploy/pathplanner/paths/Center Field Limelight 6.path index f76dfe9a..70123210 100644 --- a/src/main/deploy/pathplanner/paths/Center Field Limelight 6.path +++ b/src/main/deploy/pathplanner/paths/Center Field Limelight 6.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.862925038827178, - "y": 2.736432139530946 + "x": 7.466037934620398, + "y": 2.9662088840717122 }, "prevControl": { - "x": 7.069150830413621, - "y": 3.5302063479445027 + "x": 6.672263726206841, + "y": 3.759983092485269 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Forward.path b/src/main/deploy/pathplanner/paths/Center Forward.path new file mode 100644 index 00000000..e9bca659 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Forward.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.335176614373586, + "y": 5.535530663936646 + }, + "prevControl": null, + "nextControl": { + "x": 5.126492899296232, + "y": 7.687076544636549 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.766904366548946, + "y": 6.8946404084978985 + }, + "prevControl": { + "x": 7.766904366548946, + "y": 6.8946404084978985 + }, + "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": 50.355825042855095, + "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/Center Limelight 1.path b/src/main/deploy/pathplanner/paths/Center Limelight 1.path index 0bc9a9ce..46e87732 100644 --- a/src/main/deploy/pathplanner/paths/Center Limelight 1.path +++ b/src/main/deploy/pathplanner/paths/Center Limelight 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.3561397466804435, - "y": 5.531293435066112 + "x": 2.0245068479958848, + "y": 5.525086266457521 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.909024481556072, + "x": 2.0453956429541362, "y": 5.531293435066112 }, "prevControl": { - "x": 0.909024481556072, - "y": 5.531293435066112 + "x": 1.4709537816022202, + "y": 5.525086266457521 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 2.path b/src/main/deploy/pathplanner/paths/Center Limelight 2.path index 71eb0f9a..4b955bee 100644 --- a/src/main/deploy/pathplanner/paths/Center Limelight 2.path +++ b/src/main/deploy/pathplanner/paths/Center Limelight 2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.8701178634584756, - "y": 6.195878932042224 + "x": 2.2438391950575256, + "y": 6.485970834537088 }, "prevControl": { - "x": 1.198978701274938, - "y": 5.991619187029844 + "x": 1.7320637185803638, + "y": 6.193527705121568 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 3.path b/src/main/deploy/pathplanner/paths/Center Limelight 3.path index 4fd2b525..58ba045b 100644 --- a/src/main/deploy/pathplanner/paths/Center Limelight 3.path +++ b/src/main/deploy/pathplanner/paths/Center Limelight 3.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.0475514260637064, - "y": 6.368778004459924 + "x": 2.4527271446400407, + "y": 6.3188604748710775 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.899297827031673, - "y": 4.960593807443538 + "x": 2.108062027828891, + "y": 4.3344249538371855 }, "prevControl": { - "x": 1.3247444596568483, - "y": 5.272215972799375 + "x": 2.1602840152245197, + "y": 4.96108880258473 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -43.36342295838326, + "rotation": -12.804266065286738, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 4.path b/src/main/deploy/pathplanner/paths/Center Limelight 4.path index f5f7661f..ff245aae 100644 --- a/src/main/deploy/pathplanner/paths/Center Limelight 4.path +++ b/src/main/deploy/pathplanner/paths/Center Limelight 4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.064151379487593, - "y": 2.344123740380181 + "x": 7.413815947224771, + "y": 2.391767022719796 }, "prevControl": { - "x": 5.759233562060025, - "y": 1.3995190516453007 + "x": 6.108898129797203, + "y": 1.4471623339849158 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 6.path b/src/main/deploy/pathplanner/paths/Center Limelight 6.path index b20b0955..25c3ba2b 100644 --- a/src/main/deploy/pathplanner/paths/Center Limelight 6.path +++ b/src/main/deploy/pathplanner/paths/Center Limelight 6.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.696383923573536, - "y": 3.063896175185714 + "x": 7.915147026222807, + "y": 3.3839847832367425 }, "prevControl": { - "x": 7.297118024211369, - "y": 1.690811009086556 + "x": 7.5158811268606405, + "y": 2.0108996171375844 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Forward.path b/src/main/deploy/pathplanner/paths/Left Forward.path new file mode 100644 index 00000000..084fe010 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Forward.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6842044526319189, + "y": 6.6998765551505 + }, + "prevControl": null, + "nextControl": { + "x": 5.00796199694416, + "y": 6.865425830495789 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.766904366548946, + "y": 6.8946404084978985 + }, + "prevControl": { + "x": 7.766904366548946, + "y": 6.8946404084978985 + }, + "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": 50.355825042855095, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 59.470294100065814, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 1.path b/src/main/deploy/pathplanner/paths/Left Limelight 1.path index d23205ec..27bcf5cc 100644 --- a/src/main/deploy/pathplanner/paths/Left Limelight 1.path +++ b/src/main/deploy/pathplanner/paths/Left Limelight 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.7067146827057598, - "y": 6.836211252493681 + "x": 1.564953358914352, + "y": 6.976857516056 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.583152022769052, - "y": 6.9920223351715975 + "x": 2.0036180530376333, + "y": 7.008190708493376 }, "prevControl": { - "x": 1.5411654073604717, - "y": 6.875164023163159 + "x": 0.959178305125059, + "y": 6.809747156384766 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 2.path b/src/main/deploy/pathplanner/paths/Left Limelight 2.path index c5addb31..20c763cd 100644 --- a/src/main/deploy/pathplanner/paths/Left Limelight 2.path +++ b/src/main/deploy/pathplanner/paths/Left Limelight 2.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.463259866021512, - "y": 6.485636316468363 + "x": 2.1707284127036455, + "y": 6.015972947981653 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.583152022769052, - "y": 5.502078857064002 + "x": 2.0036180530376333, + "y": 5.598197048816623 }, "prevControl": { - "x": 2.144933352737406, - "y": 5.443649701059782 + "x": 2.0976176303497653, + "y": 5.974195358065149 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 3.path b/src/main/deploy/pathplanner/paths/Left Limelight 3.path index 518b26d1..162e3dfd 100644 --- a/src/main/deploy/pathplanner/paths/Left Limelight 3.path +++ b/src/main/deploy/pathplanner/paths/Left Limelight 3.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.3464015540130732, - "y": 4.771714407011259 + "x": 2.118506425308017, + "y": 4.773089647960467 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.583152022769052, - "y": 4.0413499569585145 + "x": 2.097617630349765, + "y": 4.031537426942538 }, "prevControl": { - "x": 2.144933352737406, - "y": 3.982920800954295 + "x": 2.181172810182771, + "y": 4.61642368577358 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 4.path b/src/main/deploy/pathplanner/paths/Left Limelight 4.path index d0b3e0fa..421a19c9 100644 --- a/src/main/deploy/pathplanner/paths/Left Limelight 4.path +++ b/src/main/deploy/pathplanner/paths/Left Limelight 4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.017063531161464, - "y": 2.356642625503519 + "x": 7.549593114453406, + "y": 2.036657508429521 }, "prevControl": { - "x": 6.7608366770707455, - "y": 1.6360163681181459 + "x": 6.293366260362688, + "y": 1.3160312510441479 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 5.path b/src/main/deploy/pathplanner/paths/Left Limelight 5.path index 60e7e539..33c66352 100644 --- a/src/main/deploy/pathplanner/paths/Left Limelight 5.path +++ b/src/main/deploy/pathplanner/paths/Left Limelight 5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.1090144268009174, - "y": 2.3176898548345597 + "x": 2.5258379269939204, + "y": 2.736432139530946 }, "prevControl": { - "x": 3.644615023506263, - "y": 1.7723510654618457 + "x": 3.061438523699266, + "y": 2.191093350158232 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -29.054604099077075, + "rotation": -44.06080905426434, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 6.path b/src/main/deploy/pathplanner/paths/Left Limelight 6.path index 6bb7a742..e2a5cfac 100644 --- a/src/main/deploy/pathplanner/paths/Left Limelight 6.path +++ b/src/main/deploy/pathplanner/paths/Left Limelight 6.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.1090144268009174, - "y": 2.3176898548345597 + "x": 2.442282747160915, + "y": 2.778209729447448 }, "prevControl": null, "nextControl": { - "x": 4.452885014904075, - "y": 3.3402000849078815 + "x": 3.9044983942385194, + "y": 3.4466511681114964 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.144296694287339, - "y": 3.768680562272157 + "x": 4.844494167359836, + "y": 3.7182055025687664 }, "prevControl": { - "x": 4.480206191370604, - "y": 3.540399451894529 + "x": 4.3358036908498185, + "y": 2.9157338243683215 }, "nextControl": { - "x": 6.079163190354851, - "y": 4.0900409202953645 + "x": 5.3737648046055755, + "y": 4.553142888082103 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.958634375157244, - "y": 4.148470076299583 + "x": 7.434704742183022, + "y": 4.135981401733796 }, "prevControl": { - "x": 6.906909567081294, - "y": 4.051088149625883 + "x": 6.382979934107071, + "y": 4.038599475060096 }, "nextControl": null, "isLocked": false, @@ -61,7 +61,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -43.806510575683916, + "rotation": 6.340191745910007, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Left-Auto Ruiner.path b/src/main/deploy/pathplanner/paths/Left-Auto Ruiner.path index 15394ae1..2c4a6d6e 100644 --- a/src/main/deploy/pathplanner/paths/Left-Auto Ruiner.path +++ b/src/main/deploy/pathplanner/paths/Left-Auto Ruiner.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 8.139789061985532, - "y": 7.441753332580739 + "x": 8.31894750384993, + "y": 7.430241005203245 }, "prevControl": { - "x": 8.17020615010322, - "y": 7.531391482794168 + "x": 8.34936459196762, + "y": 7.519879155416674 }, "nextControl": { - "x": 8.109371973867844, - "y": 7.35211518236731 + "x": 8.288530415732243, + "y": 7.3406028549898155 }, "isLocked": false, "linkedName": null @@ -36,28 +36,28 @@ "y": 5.807970324273982 }, "prevControl": { - "x": 7.05254675155435, - "y": 6.028968188110407 + "x": 7.0127401698482945, + "y": 5.799347290818089 }, "nextControl": { - "x": 9.20116605048025, - "y": 5.227725566562245 + "x": 10.208156881319693, + "y": 5.842915600421949 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.645176577390192, - "y": 4.1023916029313545 + "x": 7.900205219153025, + "y": 4.197161039636433 }, "prevControl": { - "x": 6.762130876113604, - "y": 4.479779325299392 + "x": 6.234974273032769, + "y": 4.664594287670188 }, "nextControl": { - "x": 8.973712184377149, - "y": 3.534614616892208 + "x": 9.928006485127504, + "y": 3.627953666731317 }, "isLocked": false, "linkedName": null @@ -65,15 +65,15 @@ { "anchor": { "x": 7.645176577390192, - "y": 2.4015636318309674 + "y": 2.5806210568530275 }, "prevControl": { - "x": 6.7860051744406125, - "y": 2.6430837194342613 + "x": 6.410261741045428, + "y": 2.837584023214047 }, "nextControl": { - "x": 9.16280374227415, - "y": 1.9749462600777052 + "x": 10.663278521681567, + "y": 1.9526097919785999 }, "isLocked": false, "linkedName": null @@ -84,12 +84,12 @@ "y": 0.7790554133895939 }, "prevControl": { - "x": 5.601463207556044, - "y": 0.7317343060612052 + "x": 6.0304722270180005, + "y": 1.9184239554718743 }, "nextControl": { - "x": 8.81887171296734, - "y": 0.7877608793928265 + "x": 8.766541381364528, + "y": 0.5562108020312624 }, "isLocked": false, "linkedName": null @@ -115,7 +115,22 @@ "rotateFast": false }, { - "waypointRelativePos": 0.5, + "waypointRelativePos": 2.05, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 4.050000000000001, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 5.0, "rotationDegrees": 0, "rotateFast": false } @@ -130,7 +145,7 @@ }, "goalEndState": { "velocity": 100.0, - "rotation": 0.5926760044409363, + "rotation": 146.84146577760032, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right Forward.path b/src/main/deploy/pathplanner/paths/Right Forward.path new file mode 100644 index 00000000..fe62d665 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Forward.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7815863793056181, + "y": 4.450354048988052 + }, + "prevControl": null, + "nextControl": { + "x": 1.2977105906762236, + "y": 1.2367504687559792 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.893500871224756, + "y": 1.3049178174275693 + }, + "prevControl": { + "x": 7.893500871224756, + "y": 1.3049178174275693 + }, + "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": 50.355825042855095, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -57.46493284206497, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Limelight 1.path b/src/main/deploy/pathplanner/paths/Right Limelight 1.path index 94d67d3d..8b841430 100644 --- a/src/main/deploy/pathplanner/paths/Right Limelight 1.path +++ b/src/main/deploy/pathplanner/paths/Right Limelight 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.7134190306340287, - "y": 4.411401278318571 + "x": 1.4500649866439688, + "y": 4.303091761399808 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5052464814300928, - "y": 4.0900409202953645 + "x": 2.0453956429541362, + "y": 4.135981401733796 }, "prevControl": { - "x": 1.5052464814300928, - "y": 4.0900409202953645 + "x": 1.0218446899998135, + "y": 4.376202543753688 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Limelight 2.path b/src/main/deploy/pathplanner/paths/Right Limelight 2.path index 6ccfa7cb..25eae395 100644 --- a/src/main/deploy/pathplanner/paths/Right Limelight 2.path +++ b/src/main/deploy/pathplanner/paths/Right Limelight 2.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.3171869760109631, - "y": 4.294542966310132 + "x": 2.2647279900157775, + "y": 4.699978865606586 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5052464814300928, - "y": 5.531293435066112 + "x": 1.9513960656420046, + "y": 5.566863856374023 }, "prevControl": { - "x": 1.5052464814300928, - "y": 5.531293435066112 + "x": 2.170728412703646, + "y": 4.710423263085711 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Limelight 3.path b/src/main/deploy/pathplanner/paths/Right Limelight 3.path index 7328a0d1..1c04842e 100644 --- a/src/main/deploy/pathplanner/paths/Right Limelight 3.path +++ b/src/main/deploy/pathplanner/paths/Right Limelight 3.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.3171869760109631, - "y": 5.7357954810808796 + "x": 2.2542835925366513, + "y": 6.2457496925171965 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5052464814300928, - "y": 6.962807757169489 + "x": 1.9513960656420046, + "y": 7.008190708493376 }, "prevControl": { - "x": 1.5052464814300928, - "y": 6.962807757169489 + "x": 2.3691719648070344, + "y": 5.94286216562255 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Limelight 4.path b/src/main/deploy/pathplanner/paths/Right Limelight 4.path index 81c68e07..79b2940e 100644 --- a/src/main/deploy/pathplanner/paths/Right Limelight 4.path +++ b/src/main/deploy/pathplanner/paths/Right Limelight 4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.5052464814300928, - "y": 6.962807757169489 + "x": 3.225612558095346, + "y": 7.0186351059725025 }, "prevControl": null, "nextControl": { - "x": 3.7809497208555514, - "y": 7.371811849199025 + "x": 4.501315797520807, + "y": 7.427639198002039 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.812561485146696, - "y": 5.794224637085098 + "x": 7.455593537141273, + "y": 5.786196203435664 }, "prevControl": { - "x": 6.8874331817465535, - "y": 6.174014151112526 + "x": 6.530465233741131, + "y": 6.165985717463092 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Limelight 5.path b/src/main/deploy/pathplanner/paths/Right Limelight 5.path index 0f6295fc..ba0165e5 100644 --- a/src/main/deploy/pathplanner/paths/Right Limelight 5.path +++ b/src/main/deploy/pathplanner/paths/Right Limelight 5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.849117069527141, - "y": 6.641447399146282 + "x": 3.0271690059919565, + "y": 6.14130571772594 }, "prevControl": { - "x": 4.472361400238816, - "y": 6.75830571115472 + "x": 3.650413336703628, + "y": 6.258164029734378 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 13.706961004079655, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right Limelight 6.path b/src/main/deploy/pathplanner/paths/Right Limelight 6.path index b5bff3fd..aef4c03f 100644 --- a/src/main/deploy/pathplanner/paths/Right Limelight 6.path +++ b/src/main/deploy/pathplanner/paths/Right Limelight 6.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.849117069527141, - "y": 6.641447399146282 + "x": 3.1316129807832143, + "y": 6.06819493537206 }, "prevControl": null, "nextControl": { - "x": 4.186497126174914, - "y": 6.01597294797643 + "x": 3.643388457260376, + "y": 5.504197471499269 }, "isLocked": false, "linkedName": null @@ -20,24 +20,24 @@ "y": 4.135981401733796 }, "prevControl": { - "x": 3.8764479639557194, - "y": 4.221287541874861 + "x": 4.228274716091418, + "y": 5.086421572334239 }, "nextControl": { - "x": 6.954262458143235, - "y": 4.062870619379916 + "x": 5.855356212845234, + "y": 3.901906242697459 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.946480218660184, + "x": 7.424260344703896, "y": 4.135981401733796 }, "prevControl": { - "x": 7.413815947224771, - "y": 4.115092606775545 + "x": 7.445149139662148, + "y": 4.1777589916502995 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right-Auto Ruiner.path b/src/main/deploy/pathplanner/paths/Right-Auto Ruiner.path index 3c66c3dd..a4a23e22 100644 --- a/src/main/deploy/pathplanner/paths/Right-Auto Ruiner.path +++ b/src/main/deploy/pathplanner/paths/Right-Auto Ruiner.path @@ -16,48 +16,48 @@ }, { "anchor": { - "x": 8.116213463908409, - "y": 0.886175532730662 + "x": 8.34816208185204, + "y": 0.8277463767264434 }, "prevControl": { - "x": 7.685460491538737, - "y": 0.7277380121519171 + "x": 7.917409109482368, + "y": 0.6693088561476985 }, "nextControl": { - "x": 9.394501771274664, - "y": 1.3563494908806235 + "x": 9.626450389218295, + "y": 1.297920334876405 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.873369436306303, - "y": 2.3079516621666696 + "x": 8.116213463908409, + "y": 2.2982134694993004 }, "prevControl": { - "x": 6.12222005788186, - "y": 1.299856480100275 + "x": 6.365064085483965, + "y": 1.2901182874329058 }, "nextControl": { - "x": 9.175908458578483, - "y": 3.057792497554153 + "x": 9.41875248618059, + "y": 3.048054304886784 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.873369436306303, - "y": 4.0803006143049325 + "x": 7.987848953159354, + "y": 4.070564534960625 }, "prevControl": { - "x": 5.942839192834933, - "y": 3.539283038444537 + "x": 5.989185465829067, + "y": 3.9124781574316763 }, "nextControl": { - "x": 9.020097375900564, - "y": 4.401663085651201 + "x": 9.711509055283829, + "y": 4.206899232303804 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index d9ddb660..37ee2f9d 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -361,7 +361,10 @@ private void registerAutoCommands() { NamedCommands.registerCommand("AlignToAprilTagMegaTag2", new AlignToApriltag(drivetrain, limelight)); NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( - new ParallelCommandGroup(new AlignToApriltag(drivetrain, limelight), + new ParallelCommandGroup( + // TODO: Use Align To Drivetrain + new AlignDrivetrain(drivetrain), + // new AlignToApriltag(drivetrain, limelight), new AimArmSpeaker(arm, limelight), new RampRPMAuton(intakeShooter)), new PassToOuttake(intakeShooter), @@ -375,6 +378,11 @@ private void registerAutoCommands() { new AutoMATICALLYGetNote(drivetrain, limelight, intakeShooter, -1))); + NamedCommands.registerCommand("Limelight Intake Straight", + new ParallelCommandGroup(new Intake(intakeShooter), + new AutoMATICALLYGetNote(drivetrain, limelight, + intakeShooter, 0))); + NamedCommands.registerCommand("StopIntake", new InstantCommand(intakeShooter::stopIntake)); NamedCommands.registerCommand("StopOutake", diff --git a/src/main/java/org/carlmontrobotics/commands/AlignDrivetrain.java b/src/main/java/org/carlmontrobotics/commands/AlignDrivetrain.java new file mode 100644 index 00000000..4718754c --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AlignDrivetrain.java @@ -0,0 +1,47 @@ +// 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 org.carlmontrobotics.commands; + +import java.util.Optional; + +import org.carlmontrobotics.subsystems.Drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ProxyCommand; +import edu.wpi.first.wpilibj.DriverStation; + +/** Add your docs here. */ +public class AlignDrivetrain extends ProxyCommand { + static double blueSpeakerX = 0.14; + static double blueSpeakerY = 5.54; + + static double redSpeakerX = 16.36; + static double redSpeakerY = 5.51; + + public AlignDrivetrain(Drivetrain dt) { + super(() -> { + Optional allianceSide = DriverStation.getAlliance(); + if (allianceSide.get() == Alliance.Red) { + // double redAngle = Math.atan2(redSpeakerY-dt.getPose().getY(), + // dt.getPose().getX()); + return new RotateToFieldRelativeAngle(new Rotation2d( + dt.getPose().getX(), blueSpeakerY - dt.getPose().getY()), dt); + + } else if (allianceSide.get() == Alliance.Blue) { + // double blueAngle = Math.atan2(blueSpeakerY-dt.getPose().getY(), + // dt.getPose().getX()); + return new RotateToFieldRelativeAngle( + new Rotation2d(redSpeakerX - dt.getPose().getX(), + redSpeakerY - dt.getPose().getY()), + dt); + + } + // create an if statement based on alliance side + return new InstantCommand(); + }); + } +}