diff --git a/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto b/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto index 3536d00..22c1095 100644 --- a/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto +++ b/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto @@ -90,6 +90,18 @@ "data": { "name": "Wait and Shoot" } + }, + { + "type": "named", + "data": { + "name": "Intake Off" + } + }, + { + "type": "path", + "data": { + "pathName": "Dash" + } } ] } diff --git a/Robot/src/main/deploy/pathplanner/paths/Amp Big 2.path b/Robot/src/main/deploy/pathplanner/paths/Amp Big 2.path index 48396c7..f712683 100644 --- a/Robot/src/main/deploy/pathplanner/paths/Amp Big 2.path +++ b/Robot/src/main/deploy/pathplanner/paths/Amp Big 2.path @@ -75,7 +75,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Shoot No Vision" } } ] diff --git a/Robot/src/main/deploy/pathplanner/paths/Dash.path b/Robot/src/main/deploy/pathplanner/paths/Dash.path new file mode 100644 index 0000000..1b953b0 --- /dev/null +++ b/Robot/src/main/deploy/pathplanner/paths/Dash.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.239865262137932, + "y": 7.028734983395893 + }, + "prevControl": null, + "nextControl": { + "x": 4.2402544943978775, + "y": 7.123512450728605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.316160664728736, + "y": 7.415208515602217 + }, + "prevControl": { + "x": 7.208760783262307, + "y": 7.320906123293729 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.7, + "maxAcceleration": 3.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 640.0 + }, + "goalEndState": { + "velocity": 3.0, + "rotation": -172.82839318105817, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp", + "previewStartingState": { + "rotation": -163.10149254281322, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java index 4d75aaf..09f0343 100644 --- a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java +++ b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java @@ -112,10 +112,11 @@ public RobotContainer() { NamedCommands.registerCommand("Intake On", new IntakeSetCommand(intake, IntakeSubsystem.State.INTAKE)); NamedCommands.registerCommand("Intake Off", new IntakeSetCommand(intake, IntakeSubsystem.State.OFF)); NamedCommands.registerCommand("Intake Until Note", RobotCommands.intakeUntilNote(this)); - NamedCommands.registerCommand("Shoot", RobotCommands.aimAndShoot(this, false)); + NamedCommands.registerCommand("Shoot", RobotCommands.aimAndShoot(this, false, true)); NamedCommands.registerCommand("Shoot Quick", RobotCommands.shootQuick(this)); + NamedCommands.registerCommand("Shoot No Vision", RobotCommands.aimAndShoot(this, false, false)); NamedCommands.registerCommand("Fling", RobotCommands.flingNote(this)); - NamedCommands.registerCommand("Wait and Shoot", RobotCommands.aimAndShoot(this, true)); // Waits for indexer to have note + NamedCommands.registerCommand("Wait and Shoot", RobotCommands.aimAndShoot(this, true, true)); // Waits for indexer to have note NamedCommands.registerCommand("Eject Hard", RobotCommands.ejectHard(this)); // Create a chooser to select the autonomous diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java index ed6eff2..656117c 100644 --- a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java +++ b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java @@ -1,5 +1,8 @@ package com.swrobotics.robot.commands; +import java.util.ArrayList; +import java.util.List; + import com.swrobotics.robot.RobotContainer; import com.swrobotics.robot.config.NTData; import com.swrobotics.robot.subsystems.amp.AmpArm2Subsystem; @@ -20,25 +23,30 @@ public static Command justShoot(RobotContainer robot) { ); } - public static Command aimAndShoot(RobotContainer robot, boolean waitForNote) { + public static Command aimAndShoot(RobotContainer robot, boolean waitForNote, boolean useVision) { AimTowardsSpeakerCommand aim = new AimTowardsSpeakerCommand(robot.drive, robot.shooter); - Command shootSeq = Commands.sequence( - Commands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(false)), + + List commands = new ArrayList<>(); + commands.addAll(List.of( Commands.waitUntil(robot.shooter::isCalibrated), Commands.waitUntil(() -> (!waitForNote || robot.indexer.hasPiece()) && aim.isInTolerance(NTData.DRIVE_AIM_TOLERANCE.get()) && robot.shooter.isReadyToShoot()) .withTimeout(NTData.SHOOTER_AUTO_READY_TIMEOUT.get()), Commands.waitSeconds(NTData.SHOOTER_AUTO_AFTER_READY_DELAY.get()), - new IndexerFeedCommand(robot.indexer), - Commands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(true)) - ); + new IndexerFeedCommand(robot.indexer) + )); + + if (useVision) { + commands.add(0, Comm`ands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(false))); + commands.add(Commands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(true))); + } - return new ParallelDeadlineGroup(shootSeq, aim); + return new ParallelDeadlineGroup(Commands.sequence(commands.toArray(new Command[0])), aim); } public static Command shootQuick(RobotContainer robot) { - return aimAndShoot(robot, false); + return aimAndShoot(robot, false, true); // return Commands.sequence( // Commands.runOnce(() -> robot.shooter.forcePivotCalibration(70)), // Commands.waitUntil(() -> robot.shooter.isReadyToShoot()),