diff --git a/shuffleboard.json b/shuffleboard.json index ae53c04c..d609aa6f 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -165,6 +165,19 @@ "imageWidth": 0, "imageHeight": 0 } + }, + "6,3": { + "size": [ + 4, + 1 + ], + "content": { + "_type": "Toggle Button", + "_source0": "network_table:///SmartDashboard/Amper/Reset Minimum Lift Height", + "_title": "RESET SAFE HEIGHT", + "_glyph": 148, + "_showGlyph": false + } } } } @@ -174,6 +187,6 @@ "x": -7.199999809265137, "y": -7.199999809265137, "width": 1550.4000244140625, - "height": 838.4000244140625 + "height": 830.4000244140625 } } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path new file mode 100644 index 00000000..280a89d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B to F.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 5.27 + }, + "prevControl": null, + "nextControl": { + "x": 4.511078009317217, + "y": 5.211322262872771 + }, + "isLocked": false, + "linkedName": "B" + }, + { + "anchor": { + "x": 8.141437338509483, + "y": 4.0 + }, + "prevControl": { + "x": 4.991222307694258, + "y": 3.72404602155852 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to Shoot M120.path b/src/main/deploy/pathplanner/paths/E to Shoot M120.path new file mode 100644 index 00000000..73b67ace --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E to Shoot M120.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.07, + "y": 5.84 + }, + "prevControl": null, + "nextControl": { + "x": 6.2677034911844425, + "y": 6.979658581285779 + }, + "isLocked": false, + "linkedName": "E" + }, + { + "anchor": { + "x": 3.0355126045487455, + "y": 5.141057243598081 + }, + "prevControl": { + "x": 4.651608047866594, + "y": 7.108477783289376 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -4.763641690726144, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to Shoot (TopFerry).path b/src/main/deploy/pathplanner/paths/F to Shoot (TopFerry).path index a9932d2f..59ce95c2 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot (TopFerry).path +++ b/src/main/deploy/pathplanner/paths/F to Shoot (TopFerry).path @@ -4,12 +4,12 @@ { "anchor": { "x": 8.16, - "y": 3.993395262111494 + "y": 4.2 }, "prevControl": null, "nextControl": { "x": 5.1946410831710566, - "y": 3.433578730441394 + "y": 3.6401834683299 }, "isLocked": false, "linkedName": "F Top Ferry" diff --git a/src/main/deploy/pathplanner/paths/Ferry Shot to F.path b/src/main/deploy/pathplanner/paths/Ferry Shot to F.path index 8f0716cb..2235d652 100644 --- a/src/main/deploy/pathplanner/paths/Ferry Shot to F.path +++ b/src/main/deploy/pathplanner/paths/Ferry Shot to F.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.430631429227235, - "y": 6.476519652113379 + "x": 7.017173635968985, + "y": 6.710606894650749 }, "isLocked": false, "linkedName": "Ferry Shot E" @@ -17,11 +17,11 @@ { "anchor": { "x": 8.16, - "y": 3.993395262111494 + "y": 4.2 }, "prevControl": { - "x": 8.144797599224828, - "y": 3.9781928613363213 + "x": 6.4641411145582985, + "y": 4.318586005365013 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -47.12109639666155, + "rotation": -9.990487112475767, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/M76 E to Midline.path b/src/main/deploy/pathplanner/paths/M76 E to Midline.path new file mode 100644 index 00000000..01a3f6ce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M76 E to Midline.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.07, + "y": 5.84 + }, + "prevControl": null, + "nextControl": { + "x": 8.164859011601047, + "y": 4.286166175756032 + }, + "isLocked": false, + "linkedName": "E" + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 0.9837102698456457 + }, + "prevControl": { + "x": 8.258545703967298, + "y": 3.5952268195549233 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.0908475670037, + "rotateFast": true + }, + "reversed": false, + "folder": "Top Ferry", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M76 Midline.path b/src/main/deploy/pathplanner/paths/M76 Midline.path new file mode 100644 index 00000000..30f03201 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M76 Midline.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.15, + "y": 6.563657142562062 + }, + "prevControl": null, + "nextControl": { + "x": 8.621581636886527, + "y": 6.241875878901544 + }, + "isLocked": false, + "linkedName": "Ferry Shot E" + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 0.9837102698456457 + }, + "prevControl": { + "x": 8.258545703967298, + "y": 6.347273407813577 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.42948668810303, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.0908475670037, + "rotateFast": false + }, + "reversed": false, + "folder": "Top Ferry", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Preload to C.path b/src/main/deploy/pathplanner/paths/Preload to C.path index 9ccfa9b6..49a35bbf 100644 --- a/src/main/deploy/pathplanner/paths/Preload to C.path +++ b/src/main/deploy/pathplanner/paths/Preload to C.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.493344245947751, + "x": 2.55, "y": 4.163118981508619 }, "prevControl": { - "x": 2.4591143420431036, + "x": 2.5157700960953524, "y": 4.3868765258208615 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Rerouted Ferry Shot to F.path b/src/main/deploy/pathplanner/paths/Rerouted Ferry Shot to F.path index 8f0716cb..7a0a5ad6 100644 --- a/src/main/deploy/pathplanner/paths/Rerouted Ferry Shot to F.path +++ b/src/main/deploy/pathplanner/paths/Rerouted Ferry Shot to F.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.430631429227235, - "y": 6.476519652113379 + "x": 7.01423541408095, + "y": 6.710375651573131 }, "isLocked": false, "linkedName": "Ferry Shot E" @@ -17,11 +17,11 @@ { "anchor": { "x": 8.16, - "y": 3.993395262111494 + "y": 4.2 }, "prevControl": { - "x": 8.144797599224828, - "y": 3.9781928613363213 + "x": 6.4641411145582985, + "y": 4.318586005365013 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -47.12109639666155, + "rotation": -10.607457530402133, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Shoot to C.path b/src/main/deploy/pathplanner/paths/Shoot to C.path new file mode 100644 index 00000000..9a2cb4a0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to C.path @@ -0,0 +1,66 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5050384003901907, + "y": 3.180810008343664 + }, + "prevControl": null, + "nextControl": { + "x": 1.6536338921465275, + "y": 3.1385041942694443 + }, + "isLocked": false, + "linkedName": "HShoot" + }, + { + "anchor": { + "x": 2.55, + "y": 4.163118981508619 + }, + "prevControl": { + "x": 1.3842846515935527, + "y": 4.356431195030721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": -12.222196131805264, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "IntakeToShooter", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -14.987676790851811, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to F (CBAEF).path b/src/main/deploy/pathplanner/paths/Shoot to F (CBAEF).path new file mode 100644 index 00000000..1189ca9b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot to F (CBAEF).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.73, + "y": 6.34 + }, + "prevControl": null, + "nextControl": { + "x": 4.582632369440863, + "y": 4.487367630559136 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.153314996088682, + "y": 3.8006001937929805 + }, + "prevControl": { + "x": 5.153314996088682, + "y": 3.8006001937929805 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 654e6e04..67c36bb9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -116,10 +116,11 @@ private void configureDriverBindings() { // intaking driver.getRightTriggerButton() .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.INTAKE)) - .andThen(new BuzzController(driver) - .alongWith(new LEDSet(LEDInstructions.PICKUP) - .withTimeout(3.0)))); + .andThen(new BuzzController(driver))) + .whileTrue(new WaitUntilCommand(Intake.getInstance()::hasNote) + .deadlineWith(new LEDSet(LEDInstructions.INTAKE)) + .andThen(new LEDSet(LEDInstructions.PICKUP) + .withTimeout(3.0))); // intaking (also robot relative swerve) driver.getLeftTriggerButton() @@ -331,6 +332,12 @@ public void configureAutons() { AutonConfig CBA_RED = new AutonConfig("4 CBA", FourPieceCBA::new, "Preload to C", "C to B Red", "B to A Red"); + AutonConfig CBF = new AutonConfig("4 CBF", FourPieceCBF::new, + "Preload to C", "C to B", "B to F", "F to Shoot (TopFerry)"); + + AutonConfig ReroutableCBAEF = new AutonConfig("5 CBAE Reroute Right", ReroutableSixPieceCBAEF::new, + "Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to F (CBAEF)", "F to Shoot (TopFerry)", "Rerouted E To F"); + // AutonConfig CHGF = new AutonConfig("4.5 Piece CHGF", FivePieceCHGF::new, // "Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)"); @@ -340,11 +347,27 @@ public void configureAutons() { AutonConfig TopFerry = new AutonConfig("Top Ferry", TopFerry::new, "NTF Start To D", "D to Ferry Shot", "Ferry Shot to E", "E to Ferry Shot", "Ferry Shot to F", "F to Shoot (TopFerry)"); + AutonConfig TopFerryM120 = new AutonConfig("Top Ferry M120", TopFerryM120::new, + "NTF Start To D", "D to Ferry Shot", "Ferry Shot to E", "E to Shoot M120", "Rerouted D To E"); + AutonConfig ReroutableTopFerry = new AutonConfig("Top Ferry", ReroutableTopFerry::new, "NTF Start To D", "D to Ferry Shot", "Ferry Shot to E", "E to Ferry Shot", "Ferry Shot to F", "F to Shoot (TopFerry)", "Rerouted D To E", "Rerouted E To F", "Rerouted E to Ferry Shot"); + AutonConfig M76 = new AutonConfig("M76", ReroutableTopFerryM76::new, + "NTF Start To D", "D to Ferry Shot", "Ferry Shot to E", "E to Ferry Shot", "M76 Midline", "F to Shoot (TopFerry)", "Rerouted D To E", "Rerouted E To F", "Rerouted E to Ferry Shot", "M76 E to Midline"); + + AutonConfig M94 = new AutonConfig("M94", ReroutableFourPieceHG::new, + "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "Rerouted H To G"); + + AutonConfig GC = new AutonConfig("3 GC", ThreePieceGC::new, + "HShoot to G (HGF)", "G to Shoot (HGF)", "Shoot to C"); + AutonConfig GC_RED = new AutonConfig("3 GC", ThreePieceGC::new, + "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "Shoot to C"); + + // TODO: automatically choose red/blue // TopFerry + // .registerBlue(autonChooser) // .registerRed(autonChooser); @@ -361,12 +384,21 @@ public void configureAutons() { // CBAED.registerBlue(autonChooser); // CBAED_RED.registerRed(autonChooser); - ReroutableCBAED.registerDefaultBlue(autonChooser); + ReroutableCBAED.registerBlue(autonChooser); ReroutableCBAED_RED.registerRed(autonChooser); - CBA.registerBlue(autonChooser); + CBA.registerDefaultBlue(autonChooser); CBA_RED.registerRed(autonChooser); - + + CBF.registerBlue(autonChooser) + .registerRed(autonChooser); + + ReroutableCBAEF.registerBlue(autonChooser) + .registerRed(autonChooser); + + GC.registerBlue(autonChooser); + GC_RED.registerRed(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBA.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBA.java index f0dd920f..264b7a5c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBA.java @@ -27,7 +27,7 @@ public FourPieceCBA(PathPlannerPath... paths) { .withTolerance(0.03, 0.03, 3) ), - new ConveyorShootRoutine(0.8), + new ConveyorShootRoutine(0.7), new FollowPathAndIntake(paths[0]), new SwerveDriveToShoot() @@ -42,7 +42,7 @@ public FourPieceCBA(PathPlannerPath... paths) { new FollowPathAndIntake(paths[2]), new SwerveDriveToShoot() .withTolerance(0.03, 3), - new ConveyorShootRoutine() + new ConveyorShootRoutine(0.7) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBF.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBF.java new file mode 100644 index 00000000..3e40ec8e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FourPieceCBF.java @@ -0,0 +1,48 @@ +package com.stuypulse.robot.commands.auton.CBADE; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings.Auton; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +/** + * Shoots second shot at podium, shoots first shot between C and B + */ +public class FourPieceCBF extends SequentialCommandGroup { + + public FourPieceCBF(PathPlannerPath... paths) { + addCommands( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterPodiumShot()), + + SwerveDriveToPose.speakerRelative(-15) + .withTolerance(0.03, 0.03, 3) + ), + + new ConveyorShootRoutine(0.8), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new ConveyorShootRoutine(), + + new FollowPathAndIntake(paths[1]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new ConveyorShootRoutine(), + + new FollowPathAndIntake(paths[2]), + new FollowPathAlignAndShoot(paths[3], new SwerveDriveToShoot()) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java index 5031ee1a..45529a54 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java @@ -28,7 +28,7 @@ public ReroutableSixPieceCBAED(PathPlannerPath... paths) { .andThen(new ShooterPodiumShot()), SwerveDriveToPose.speakerRelative(-15) - .withTolerance(0.03, 0.03, 3) + .withTolerance(0.06, 0.06, 3) ), // shoot preload @@ -59,13 +59,13 @@ public ReroutableSixPieceCBAED(PathPlannerPath... paths) { // shoot E, intake D, shoot D new SequentialCommandGroup( new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot() - .withTolerance(0.033, 7)), + .withTolerance(0.06, 7)), new FollowPathAndIntake(paths[5]), new ConditionalCommand( new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot() - .withTolerance(0.033, 7)), + .withTolerance(0.06, 7)), new DoNothingCommand(), Intake.getInstance()::hasNote)), @@ -74,8 +74,8 @@ public ReroutableSixPieceCBAED(PathPlannerPath... paths) { new FollowPathAndIntake(E_TO_D), new ConditionalCommand( - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot() - .withTolerance(0.033, 7)), + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot() + .withTolerance(0.06, 7)), new DoNothingCommand(), Intake.getInstance()::hasNote) ), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAEF.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAEF.java new file mode 100644 index 00000000..a7bf9cc6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAEF.java @@ -0,0 +1,87 @@ +package com.stuypulse.robot.commands.auton.CBADE; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.DoNothingCommand; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class ReroutableSixPieceCBAEF extends SequentialCommandGroup { + + public ReroutableSixPieceCBAEF(PathPlannerPath... paths) { + + PathPlannerPath E_TO_F = paths[7]; + + addCommands( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterPodiumShot()), + + SwerveDriveToPose.speakerRelative(-15) + .withTolerance(0.06, 0.06, 3) + ), + + // shoot preload + new ConveyorShootRoutine(0.55), + + // intake C, shoot C + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot() + .withTolerance(0.03, 7), + new ConveyorShootRoutine(), + + // intake B, shoot B + new FollowPathAndIntake(paths[1]), + new SwerveDriveToShoot() + .withTolerance(0.05, 7), + new ConveyorShootRoutine(), + + // intake A, shoot A + new FollowPathAndIntake(paths[2]), + new SwerveDriveToShoot() + .withTolerance(0.05, 5), + new ConveyorShootRoutine(0.6), + + // intake E + new FollowPathAndIntake(paths[3]), + + new ConditionalCommand( + // shoot E, intake F, shoot F + new SequentialCommandGroup( + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot() + .withTolerance(0.06, 7)), + + new FollowPathAndIntake(paths[5]), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot() + .withTolerance(0.06, 7)), + new DoNothingCommand(), + Intake.getInstance()::hasNote)), + + // intake F, shoot F + new SequentialCommandGroup( + new FollowPathAndIntake(E_TO_F), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot() + .withTolerance(0.06, 7)), + new DoNothingCommand(), + Intake.getInstance()::hasNote) + ), + Intake.getInstance()::hasNote) + ); + } + + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java index 6e3163e9..c41dcdb2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java @@ -23,7 +23,7 @@ public SixPieceCBAED(PathPlannerPath... paths) { .andThen(new ShooterPodiumShot()), SwerveDriveToPose.speakerRelative(-15) - .withTolerance(0.03, 0.03, 3) + .withTolerance(0.06, 0.06, 3) ), new ConveyorShootRoutine(0.55), @@ -45,11 +45,11 @@ public SixPieceCBAED(PathPlannerPath... paths) { new FollowPathAndIntake(paths[3]), new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot() - .withTolerance(0.033, 7)), + .withTolerance(0.06, 7)), new FollowPathAndIntake(paths[5]), new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot() - .withTolerance(0.033, 7)) + .withTolerance(0.06, 7)) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerry.java b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerry.java index 61fd7893..9296e004 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerry.java @@ -42,11 +42,11 @@ public ReroutableTopFerry(PathPlannerPath... paths) { new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) .andThen(new ShooterPodiumShot()), - SwerveDriveToPose.speakerRelative(45) + SwerveDriveToPose.speakerRelative(42.5) .withTolerance(0.03, 0.03, 3) ), - new ConveyorShootRoutine(), + new ConveyorShootRoutine(0.65), new ShooterSetRPM(Settings.Shooter.WING_FERRY), @@ -82,41 +82,19 @@ public ReroutableTopFerry(PathPlannerPath... paths) { new InstantCommand(), Intake.getInstance()::hasNote) ), - // else intake E + // else intake F new SequentialCommandGroup( - new FollowPathAndIntake(D_TO_E), - + new FollowPathAndIntake(E_TO_F), + new ConditionalCommand( - new SequentialCommandGroup( - // shoot E, intake F - SwerveDrive.getInstance().followPathCommand(E_FERRY_REROUTE), - new SwerveDriveStop(), - new ConveyorShootRoutine(), - new ShooterPodiumShot(), - - new SwerveDriveResetOdometry(() -> getPathStartPose(paths[4])), - new FollowPathAndIntake(paths[4]), - - new ConditionalCommand( - // if we have a note, we can shoot F - new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), - new InstantCommand(), - Intake.getInstance()::hasNote) - ), - new SequentialCommandGroup( - new FollowPathAndIntake(E_TO_F), - - new ConditionalCommand( - // if we have a note, we can shoot F - new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), - new InstantCommand(), - Intake.getInstance()::hasNote) - ), + // if we have a note, we can shoot F + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), + new InstantCommand(), Intake.getInstance()::hasNote) ), Intake.getInstance()::hasNote) ), - // else intake E + // don't shoot D, intake E new SequentialCommandGroup( new FollowPathAndIntake(D_TO_E), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerryM76.java b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerryM76.java new file mode 100644 index 00000000..dec91008 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerryM76.java @@ -0,0 +1,103 @@ +package com.stuypulse.robot.commands.auton.Ferry; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetOdometry; +import com.stuypulse.robot.commands.swerve.SwerveDriveStop; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +// top ferry until F, then drive along midline until robot intakes a note and stop +public class ReroutableTopFerryM76 extends SequentialCommandGroup { + + private Pose2d getPathStartPose(PathPlannerPath path) { + return path.getPreviewStartingHolonomicPose(); + } + + public ReroutableTopFerryM76(PathPlannerPath... paths) { + + PathPlannerPath MIDLINE_INTAKE_PATH = paths[4]; + PathPlannerPath D_TO_E = paths[6]; + PathPlannerPath E_FERRY_REROUTE = paths[8]; + PathPlannerPath E_TO_MIDLINE = paths[9]; + + Command INTAKE_E = new FollowPathAndIntake(paths[2]); + + addCommands ( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterPodiumShot()), + + SwerveDriveToPose.speakerRelative(42.5) + .withTolerance(0.03, 0.03, 3) + ), + + new ConveyorShootRoutine(0.65), + + new ShooterSetRPM(Settings.Shooter.WING_FERRY), + + // intake D + new FollowPathAndIntake(paths[0]), + + new ConditionalCommand( + // if we have a note, we can shoot D + new SequentialCommandGroup( + + // shoot D, intake E + SwerveDrive.getInstance().followPathCommand(paths[1]), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + INTAKE_E, + + new ConditionalCommand( + // if we have a note, we can shoot E + new SequentialCommandGroup( + + // shoot E, intake midline from shot spot + SwerveDrive.getInstance().followPathCommand(paths[3]), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + new ShooterPodiumShot(), + + new SwerveDriveResetOdometry(() -> getPathStartPose(MIDLINE_INTAKE_PATH)), + new FollowPathAndIntake(MIDLINE_INTAKE_PATH) + ), + // else midline intake + new FollowPathAndIntake(E_TO_MIDLINE), + Intake.getInstance()::hasNote) + ), + // else intake E + new SequentialCommandGroup( + new FollowPathAndIntake(D_TO_E), + + new ConditionalCommand( + new SequentialCommandGroup( + // shoot E, intake midline + SwerveDrive.getInstance().followPathCommand(E_FERRY_REROUTE), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + new ShooterPodiumShot(), + + new SwerveDriveResetOdometry(() -> getPathStartPose(MIDLINE_INTAKE_PATH)), + new FollowPathAndIntake(MIDLINE_INTAKE_PATH) + ), + new FollowPathAndIntake(E_TO_MIDLINE), + Intake.getInstance()::hasNote) + ), + Intake.getInstance()::hasNote) + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Ferry/TopFerry.java b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/TopFerry.java index 94edbebe..89be70c5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/Ferry/TopFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/TopFerry.java @@ -25,7 +25,7 @@ public TopFerry(PathPlannerPath... paths) { new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) .andThen(new ShooterPodiumShot()), - SwerveDriveToPose.speakerRelative(45) + SwerveDriveToPose.speakerRelative(42.5) .withTolerance(0.03, 0.03, 3) ), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Ferry/TopFerryM120.java b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/TopFerryM120.java new file mode 100644 index 00000000..b5cf698a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/TopFerryM120.java @@ -0,0 +1,65 @@ +package com.stuypulse.robot.commands.auton.Ferry; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; +import com.stuypulse.robot.commands.swerve.SwerveDriveStop; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +// top ferry until E pickup, then pause for 3 seconds, go back and shoot +public class TopFerryM120 extends SequentialCommandGroup { + public TopFerryM120(PathPlannerPath... paths) { + addCommands ( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterPodiumShot()), + + SwerveDriveToPose.speakerRelative(42.5) + .withTolerance(0.03, 0.03, 3) + ), + + new ConveyorShootRoutine(), + + new ShooterSetRPM(Settings.Shooter.WING_FERRY), + + // intake D + new FollowPathAndIntake(paths[0]), + + new ConditionalCommand( + // if we have a note, we can shoot D + new SequentialCommandGroup( + // shoot D, intake E + SwerveDrive.getInstance().followPathCommand(paths[1]), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + new FollowPathAndIntake(paths[2]) + ), + // no note, reroute + new FollowPathAndIntake(paths[4]), + Intake.getInstance()::hasNote + ), + + // wait 3 seconds + new ShooterPodiumShot(), + new WaitCommand(3.0), + + // shoot E + new FollowPathAlignAndShoot(paths[3], new SwerveDriveToShoot() + .withTolerance(0.06, 7)) + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHG.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHG.java new file mode 100644 index 00000000..9a8ed016 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHG.java @@ -0,0 +1,73 @@ +package com.stuypulse.robot.commands.auton.HGF; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.FastAlignShootSpeakerRelative; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShootFast; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class ReroutableFourPieceHG extends SequentialCommandGroup { + + // takes same paths as hgf + public ReroutableFourPieceHG(PathPlannerPath... paths) { + PathPlannerPath H_TO_G = paths[4]; + + addCommands( + new ParallelCommandGroup( + new WaitCommand(0.25) + .andThen(new ShooterPodiumShot()), + + SwerveDriveToPose.speakerRelative(-55) + .withTolerance(0.1, 0.1, 2) + ), + + new ShooterWaitForTarget(), + ConveyorShootRoutine.untilNoteShot(0.75), + + // intake H + new FollowPathAndIntake(paths[0]), + new ConditionalCommand( + // has H + new SequentialCommandGroup( + // shoot H, intake G + // new FollowPathAlignAndShootFast(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), + new FollowPathAlignAndShoot(paths[1], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), + new FollowPathAndIntake(paths[2]), + new ConditionalCommand( + // shoot G + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), + + new InstantCommand(), + + Intake.getInstance()::hasNote)), + // no H + new SequentialCommandGroup( + // intake G + new FollowPathAndIntake(H_TO_G), + new ConditionalCommand( + // shoot G, intake F, shoot F + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), + + new InstantCommand(), + + Intake.getInstance()::hasNote)), + + Intake.getInstance()::hasNote) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java index 2c2d09de..eecf7f70 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java @@ -48,11 +48,13 @@ public ReroutableFourPieceHGF(PathPlannerPath... paths) { // shoot G, intake F, shoot F new SequentialCommandGroup( // new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), - new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), new FollowPathAndIntake(paths[4]), new ConditionalCommand( - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), new DoNothingCommand(), Intake.getInstance()::hasNote)), @@ -61,7 +63,8 @@ public ReroutableFourPieceHGF(PathPlannerPath... paths) { new FollowPathAndIntake(G_TO_F), new ConditionalCommand( - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), new DoNothingCommand(), Intake.getInstance()::hasNote)), Intake.getInstance()::hasNote)), @@ -73,11 +76,13 @@ public ReroutableFourPieceHGF(PathPlannerPath... paths) { // shoot G, intake F, shoot F new SequentialCommandGroup( // new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), - new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), new FollowPathAndIntake(paths[4]), new ConditionalCommand( - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), new DoNothingCommand(), Intake.getInstance()::hasNote) ), @@ -87,7 +92,8 @@ public ReroutableFourPieceHGF(PathPlannerPath... paths) { new FollowPathAndIntake(G_TO_F), new ConditionalCommand( - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.03, 0.03, 3)), new DoNothingCommand(), Intake.getInstance()::hasNote)), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/ThreePieceGC.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ThreePieceGC.java new file mode 100644 index 00000000..587e772e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ThreePieceGC.java @@ -0,0 +1,43 @@ +package com.stuypulse.robot.commands.auton.HGF; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class ThreePieceGC extends SequentialCommandGroup { + + public ThreePieceGC(PathPlannerPath... paths) { + addCommands( + new ParallelCommandGroup( + new WaitCommand(0.25) + .andThen(new ShooterPodiumShot()), + + SwerveDriveToPose.speakerRelative(-55) + .withTolerance(0.05, 0.05, 2) + ), + + new ShooterWaitForTarget() + .withTimeout(0.25), + ConveyorShootRoutine.untilNoteShot(1.75), + + new FollowPathAndIntake(paths[0]), + new FollowPathAlignAndShoot(paths[1], SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.05, 0.05, 5)), + + new FollowPathAndIntake(paths[2]), + new SwerveDriveToShoot() + .withTolerance(0.03, 7), + new ConveyorShootRoutine(0.7) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 89e93896..2f92da19 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -27,8 +27,10 @@ /** This interface stores information about the field elements. */ public interface Field { - double WIDTH = Units.inchesToMeters(323.25); - double LENGTH = Units.inchesToMeters(651.25); + double WIDTH = Units.inchesToMeters(323.25); // 8.21 + double LENGTH = Units.inchesToMeters(651.25); // 16.54 + + double WING_TO_CENTERLINE = 2.45; double NOTE_LENGTH = Units.inchesToMeters(14.0); @@ -272,9 +274,13 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria /*** FERRYING ***/ - double FERRY_SHOT_MAX_X = 9.0; + // MIDLINE: 8.27 + + // BEFORE: 9 + // MIDLINE + 60% of WING_TO_CENTERLINE distance + double FERRY_SHOT_MAX_X = LENGTH / 2.0 + WING_TO_CENTERLINE * 0.60; double FERRY_SHOT_MIN_X = 6.0; - double FERRY_SHOT_MIN_FAR_X = 8.25; + double FERRY_SHOT_MIN_FAR_X = 8.5; /**** EMPTY FIELD POSES ****/ diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index ee1c29c8..70ab23a3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -225,25 +225,25 @@ public interface Drive { public interface FrontRight { String ID = "Front Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(40.4); + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(38.144531); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5); } public interface FrontLeft { String ID = "Front Left"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(-172.880859); + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(-173.408203); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * +0.5); } public interface BackLeft { String ID = "Back Left"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(22.939453); + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(24.609375); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * +0.5); } public interface BackRight { String ID = "Back Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(40.253906); + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(38.232422); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java index 5519ab04..f36898f1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java @@ -62,6 +62,8 @@ public static Amper getInstance() { public Amper() { targetHeight = new SmartNumber("Amper/Target Height", 0); // TODO: determine the default value minHeight = Settings.Amper.Lift.MIN_HEIGHT; + + SmartDashboard.putBoolean("Amper/Reset Minimum Lift Height", false); mechanism2d = new Mechanism2d(3, 3); mechanism2d.getRoot("Base Origin", 1, 1).append(new MechanismLigament2d( @@ -160,10 +162,18 @@ public final void resetConstraints() { @Override public void periodic() { - SmartDashboard.putNumber("Amper/Amp Distance", Units.metersToInches(Field.WIDTH - Odometry.getInstance().getPose().getY())); + SmartDashboard.putNumber("Amper/Amp Distance", + Units.metersToInches(Math.min( + Field.WIDTH - Odometry.getInstance().getPose().getY(), + Odometry.getInstance().getPose().getY()))); lift2d.setLength(Settings.Amper.Lift.VISUALIZATION_MIN_LENGTH + getLiftHeight()); + if (SmartDashboard.getBoolean("Amper/Reset Minimum Lift Height", false)) { + setMinHeight(Settings.Amper.Lift.MIN_HEIGHT); + setTargetHeight(minHeight); + } + if (targetHeight.get() > Settings.Amper.Lift.MAX_HEIGHT) targetHeight.set(Settings.Amper.Lift.MAX_HEIGHT);