diff --git a/simgui.json b/simgui.json index 81e3dad2..de436924 100644 --- a/simgui.json +++ b/simgui.json @@ -43,6 +43,7 @@ "/Shuffleboard/arm SysID/quasistatic backward": "Command", "/Shuffleboard/arm SysID/quasistatic forward": "Command", "/SmartDashboard/Arm": "Subsystem", + "/SmartDashboard/Drivetrain": "Subsystem", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/Intake Shooter": "Subsystem", "/SmartDashboard/SendableChooser[0]": "String Chooser", diff --git a/src/main/deploy/pathplanner/autos/Center Limelight 4 Piece.auto b/src/main/deploy/pathplanner/autos/Center Limelight 4 Piece.auto new file mode 100644 index 00000000..2ea1ec90 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Limelight 4 Piece.auto @@ -0,0 +1,275 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.356139746680443, + "y": 5.531293435066112 + }, + "rotation": -1.9026452141219743 + }, + "command": { + "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": "Center Limelight 1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "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": "Center Limelight 2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "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": "Center Limelight 3" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "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": "Center Limelight 4" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Center Limelight 5" + } + }, + { + "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": "Center Limelight 6" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center-Auto Ruiner.auto b/src/main/deploy/pathplanner/autos/Center-Auto Ruiner.auto index 98788499..98dddd82 100644 --- a/src/main/deploy/pathplanner/autos/Center-Auto Ruiner.auto +++ b/src/main/deploy/pathplanner/autos/Center-Auto Ruiner.auto @@ -20,12 +20,6 @@ "data": { "pathName": "Center-Auto Ruiner" } - }, - { - "type": "named", - "data": { - "name": "AutonRuinerShootAndIntake" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Left Limelight 4 Piece.auto b/src/main/deploy/pathplanner/autos/Left Limelight 4 Piece.auto new file mode 100644 index 00000000..c4731918 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Limelight 4 Piece.auto @@ -0,0 +1,275 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6842044526319189, + "y": 6.719352940485241 + }, + "rotation": 60.60256482955589 + }, + "command": { + "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" + } + } + ] + } + }, + { + "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 2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "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 3" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "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 4" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Left Limelight 5" + } + }, + { + "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 6" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Limelight 4 Piece.auto b/src/main/deploy/pathplanner/autos/Right Limelight 4 Piece.auto new file mode 100644 index 00000000..dfa0eb3a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Limelight 4 Piece.auto @@ -0,0 +1,275 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7134190306340287, + "y": 4.411401278318571 + }, + "rotation": -61.80131808467152 + }, + "command": { + "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" + } + } + ] + } + }, + { + "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 2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "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 3" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "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 4" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Right Limelight 5" + } + }, + { + "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 6" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ 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 new file mode 100644 index 00000000..adb58337 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Limelight 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.356139746680443, + "y": 5.531293435066112 + }, + "prevControl": null, + "nextControl": { + "x": 2.3561397466804435, + "y": 5.531293435066112 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6707957567753815, + "y": 5.531293435066112 + }, + "prevControl": { + "x": 1.6707957567753815, + "y": 5.531293435066112 + }, + "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": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 2.path b/src/main/deploy/pathplanner/paths/Center Limelight 2.path new file mode 100644 index 00000000..949cdab4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Limelight 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6707957567753815, + "y": 5.531293435066112 + }, + "prevControl": null, + "nextControl": { + "x": 2.144933352737406, + "y": 5.998726683099867 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6707957567753815, + "y": 6.9433313718347485 + }, + "prevControl": { + "x": 1.9793840773921174, + "y": 6.70961474781787 + }, + "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.398705354995547, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 3.path b/src/main/deploy/pathplanner/paths/Center Limelight 3.path new file mode 100644 index 00000000..a89e2cfa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Limelight 3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6707957567753815, + "y": 6.9433313718347485 + }, + "prevControl": null, + "nextControl": { + "x": 2.0475514260637064, + "y": 6.368778004459924 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.544199252099572, + "y": 4.158208268966954 + }, + "prevControl": { + "x": 1.9696458847247476, + "y": 4.469830434322791 + }, + "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": -16.38954033403484, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 4.763641690726263, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 4.path b/src/main/deploy/pathplanner/paths/Center Limelight 4.path new file mode 100644 index 00000000..fe093a15 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Limelight 4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.544199252099572, + "y": 4.158208268966954 + }, + "prevControl": null, + "nextControl": { + "x": 1.4048307100172925, + "y": 1.1198921567475397 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.929419797155134, + "y": 2.395595396172999 + }, + "prevControl": { + "x": 6.624501979727566, + "y": 1.4509907074381188 + }, + "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.014175695411015, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -17.102728969052393, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 5.path b/src/main/deploy/pathplanner/paths/Center Limelight 5.path new file mode 100644 index 00000000..8d7aad6d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Limelight 5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.929419797155134, + "y": 2.395595396172999 + }, + "prevControl": null, + "nextControl": { + "x": 7.413295585784529, + "y": 1.2854414320928298 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5149846740974624, + "y": 2.7559085248656867 + }, + "prevControl": { + "x": 2.9337269587943684, + "y": 1.3341323954296802 + }, + "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": -35.83765295427821, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 3.1798301198642935, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Limelight 6.path b/src/main/deploy/pathplanner/paths/Center Limelight 6.path new file mode 100644 index 00000000..2a50df08 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Limelight 6.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5149846740974624, + "y": 2.7559085248656867 + }, + "prevControl": null, + "nextControl": { + "x": 5.670159098325315, + "y": 0.08764373400632962 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.153398228504642, + "y": 3.846586103611116 + }, + "prevControl": { + "x": 7.754132329142475, + "y": 2.473500937511958 + }, + "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": 64.98310652189994, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -37.40535663140861, + "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 new file mode 100644 index 00000000..d23205ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Limelight 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6842044526319189, + "y": 6.719352940485241 + }, + "prevControl": null, + "nextControl": { + "x": 1.7067146827057598, + "y": 6.836211252493681 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.583152022769052, + "y": 6.9920223351715975 + }, + "prevControl": { + "x": 1.5411654073604717, + "y": 6.875164023163159 + }, + "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": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.75117366345306, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 2.path b/src/main/deploy/pathplanner/paths/Left Limelight 2.path new file mode 100644 index 00000000..c5addb31 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Limelight 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.583152022769052, + "y": 6.972545949836858 + }, + "prevControl": null, + "nextControl": { + "x": 1.463259866021512, + "y": 6.485636316468363 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.583152022769052, + "y": 5.502078857064002 + }, + "prevControl": { + "x": 2.144933352737406, + "y": 5.443649701059782 + }, + "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.89909245378778, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 1.3019526725789563, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 3.path b/src/main/deploy/pathplanner/paths/Left Limelight 3.path new file mode 100644 index 00000000..518b26d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Limelight 3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.583152022769052, + "y": 5.502078857064002 + }, + "prevControl": null, + "nextControl": { + "x": 1.3464015540130732, + "y": 4.771714407011259 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.583152022769052, + "y": 4.0413499569585145 + }, + "prevControl": { + "x": 2.144933352737406, + "y": 3.982920800954295 + }, + "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.89909245378778, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 4.9697407281102794, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 4.path b/src/main/deploy/pathplanner/paths/Left Limelight 4.path new file mode 100644 index 00000000..d0b3e0fa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Limelight 4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.583152022769052, + "y": 4.0413499569585145 + }, + "prevControl": null, + "nextControl": { + "x": 2.446817325425873, + "y": 1.0127720374064717 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.017063531161464, + "y": 2.356642625503519 + }, + "prevControl": { + "x": 6.7608366770707455, + "y": 1.6360163681181459 + }, + "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": 29.74488129694226, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 4.9697407281102794, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 5.path b/src/main/deploy/pathplanner/paths/Left Limelight 5.path new file mode 100644 index 00000000..60e7e539 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Limelight 5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.017063531161464, + "y": 2.3176898548345597 + }, + "prevControl": null, + "nextControl": { + "x": 7.4035573931171585, + "y": 1.4607289001060078 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1090144268009174, + "y": 2.3176898548345597 + }, + "prevControl": { + "x": 3.644615023506263, + "y": 1.7723510654618457 + }, + "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": -29.054604099077075, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 18.86899945523534, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Limelight 6.path b/src/main/deploy/pathplanner/paths/Left Limelight 6.path new file mode 100644 index 00000000..6bb7a742 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Limelight 6.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1090144268009174, + "y": 2.3176898548345597 + }, + "prevControl": null, + "nextControl": { + "x": 4.452885014904075, + "y": 3.3402000849078815 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.144296694287339, + "y": 3.768680562272157 + }, + "prevControl": { + "x": 4.480206191370604, + "y": 3.540399451894529 + }, + "nextControl": { + "x": 6.079163190354851, + "y": 4.0900409202953645 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.958634375157244, + "y": 4.148470076299583 + }, + "prevControl": { + "x": 6.906909567081294, + "y": 4.051088149625883 + }, + "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": -3.0664855011258916, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -43.806510575683916, + "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 new file mode 100644 index 00000000..94d67d3d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Limelight 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7134190306340287, + "y": 4.411401278318571 + }, + "prevControl": null, + "nextControl": { + "x": 1.7134190306340287, + "y": 4.411401278318571 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5052464814300928, + "y": 4.0900409202953645 + }, + "prevControl": { + "x": 1.5052464814300928, + "y": 4.0900409202953645 + }, + "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": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.23728046576109, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Limelight 2.path b/src/main/deploy/pathplanner/paths/Right Limelight 2.path new file mode 100644 index 00000000..6ccfa7cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Limelight 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5052464814300928, + "y": 4.0900409202953645 + }, + "prevControl": null, + "nextControl": { + "x": 1.3171869760109631, + "y": 4.294542966310132 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5052464814300928, + "y": 5.531293435066112 + }, + "prevControl": { + "x": 1.5052464814300928, + "y": 5.531293435066112 + }, + "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": 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/Right Limelight 3.path b/src/main/deploy/pathplanner/paths/Right Limelight 3.path new file mode 100644 index 00000000..7328a0d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Limelight 3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5052464814300928, + "y": 5.531293435066112 + }, + "prevControl": null, + "nextControl": { + "x": 1.3171869760109631, + "y": 5.7357954810808796 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5052464814300928, + "y": 6.962807757169489 + }, + "prevControl": { + "x": 1.5052464814300928, + "y": 6.962807757169489 + }, + "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": 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/Right Limelight 4.path b/src/main/deploy/pathplanner/paths/Right Limelight 4.path new file mode 100644 index 00000000..81c68e07 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Limelight 4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5052464814300928, + "y": 6.962807757169489 + }, + "prevControl": null, + "nextControl": { + "x": 3.7809497208555514, + "y": 7.371811849199025 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.812561485146696, + "y": 5.794224637085098 + }, + "prevControl": { + "x": 6.8874331817465535, + "y": 6.174014151112526 + }, + "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": 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/Right Limelight 5.path b/src/main/deploy/pathplanner/paths/Right Limelight 5.path new file mode 100644 index 00000000..0f6295fc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Limelight 5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.812561485146696, + "y": 5.794224637085098 + }, + "prevControl": null, + "nextControl": { + "x": 6.867956796411813, + "y": 6.534327279805212 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.849117069527141, + "y": 6.641447399146282 + }, + "prevControl": { + "x": 4.472361400238816, + "y": 6.75830571115472 + }, + "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": 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/Right Limelight 6.path b/src/main/deploy/pathplanner/paths/Right Limelight 6.path new file mode 100644 index 00000000..b5bff3fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Limelight 6.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.849117069527141, + "y": 6.641447399146282 + }, + "prevControl": null, + "nextControl": { + "x": 4.186497126174914, + "y": 6.01597294797643 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.533824400982135, + "y": 4.135981401733796 + }, + "prevControl": { + "x": 3.8764479639557194, + "y": 4.221287541874861 + }, + "nextControl": { + "x": 6.954262458143235, + "y": 4.062870619379916 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.946480218660184, + "y": 4.135981401733796 + }, + "prevControl": { + "x": 7.413815947224771, + "y": 4.115092606775545 + }, + "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": 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/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index be54556c..c5d2573c 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -108,6 +108,8 @@ public class RobotContainer { "Preloaded Right Auton 3 piece total", // "Preloaded Left-Pickup Subwoofer", // "Preloaded Right-Pickup Subwoofer", + "Right Limelight 4 Piece", "Left Limelight 4 Piece", + "Center Limelight 4 Piece", "Left-Amp", @@ -343,6 +345,12 @@ private void registerAutoCommands() { NamedCommands.registerCommand("PassToOuttake", new PassToOuttake(intakeShooter)); + NamedCommands.registerCommand("AimArmSpeakerMT2", + new AimArmSpeakerMT2(arm, limelight)); + NamedCommands.registerCommand("AlignToAprilTagMegaTag2", + new AlignToApriltagMegatag2(drivetrain, limelight)); + + NamedCommands.registerCommand("StopIntake", new InstantCommand(intakeShooter::stopIntake)); diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java index a46f1a1f..4feb506b 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java @@ -62,7 +62,7 @@ public void execute() { @Override public boolean isFinished() { - return false; + return rotationPID.atSetpoint(); // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); // return rotationPID.atSetpoint(); diff --git a/src/main/java/org/carlmontrobotics/commands/PassToOuttake.java b/src/main/java/org/carlmontrobotics/commands/PassToOuttake.java index 8107a9c4..fcafbbfb 100644 --- a/src/main/java/org/carlmontrobotics/commands/PassToOuttake.java +++ b/src/main/java/org/carlmontrobotics/commands/PassToOuttake.java @@ -1,22 +1,26 @@ package org.carlmontrobotics.commands; import org.carlmontrobotics.subsystems.IntakeShooter; +import org.carlmontrobotics.subsystems.IntakeShooter.*; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; public class PassToOuttake extends Command { - private IntakeShooter intakeShooter; + private final IntakeShooter intakeShooter; + public PassToOuttake(IntakeShooter intakeShooter) { - this.intakeShooter = intakeShooter; + addRequirements(this.intakeShooter = intakeShooter); } @Override public void initialize() { - + if (intakeShooter.getOuttakeRPM() >= 4000 + && intakeShooter.getOuttakeRPM() <= 4250) { intakeShooter.motorSetIntake(0.6); } + } @Override public void execute() {