diff --git a/src/main/deploy/pathplanner/autos/1 and Preload.auto b/src/main/deploy/pathplanner/autos/1 and Preload.auto new file mode 100644 index 0000000..0f730a5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 and Preload.auto @@ -0,0 +1,51 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.225000000000017, + "y": 7.915 + }, + "rotation": 7.642727713197618e-27 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Stack" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "IntakeAltStack1" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Stack Alt.auto b/src/main/deploy/pathplanner/autos/2 Stack Alt.auto index 2685dc8..ccd3238 100644 --- a/src/main/deploy/pathplanner/autos/2 Stack Alt.auto +++ b/src/main/deploy/pathplanner/autos/2 Stack Alt.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.225, + "x": 1.225000000000017, "y": 7.915 }, - "rotation": 2.774373461529055e-26 + "rotation": 7.642727713197618e-27 }, "command": { "type": "sequential", @@ -21,6 +21,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "IntakeSetpoint" + } + }, { "type": "race", "data": { diff --git a/src/main/java/frc/team5115/RobotContainer.java b/src/main/java/frc/team5115/RobotContainer.java index 4e1b3fe..402db53 100644 --- a/src/main/java/frc/team5115/RobotContainer.java +++ b/src/main/java/frc/team5115/RobotContainer.java @@ -164,7 +164,7 @@ private void configureButtonBindings() { joyManip.x().onTrue(DriveCommands.prepareDispense(dispenser, arm)); - joyManip.rightBumper().onTrue(DriveCommands.stackSequence(dispenser, arm)); + joyManip.rightBumper().onTrue(DriveCommands.stackSet(dispenser, arm)); joyManip.rightTrigger().onTrue(dispenser.dispense()).onFalse(dispenser.stop()); joyManip.leftTrigger().onTrue(dispenser.intake()).onFalse(dispenser.stop()); @@ -194,6 +194,7 @@ public static void registerCommands( NamedCommands.registerCommand("Dispense", DriveCommands.quickScore(dispenser, arm, drivetrain)); NamedCommands.registerCommand("Intake", DriveCommands.intakeUntilCanister(dispenser, arm)); + NamedCommands.registerCommand("IntakeSetpoint", DriveCommands.armIntake(arm)); NamedCommands.registerCommand("Stack", DriveCommands.stackSequence(dispenser, arm)); } diff --git a/src/main/java/frc/team5115/commands/DriveCommands.java b/src/main/java/frc/team5115/commands/DriveCommands.java index 67fa9b5..d842c96 100644 --- a/src/main/java/frc/team5115/commands/DriveCommands.java +++ b/src/main/java/frc/team5115/commands/DriveCommands.java @@ -44,6 +44,14 @@ public static Command quickScore(Dispenser dispenser, Arm arm, Drivetrain drivet .withInterruptBehavior(InterruptionBehavior.kCancelSelf); } + public static Command armIntake(Arm arm) { + return Commands.sequence( + arm.intake(), + arm.waitForSetpoint(2.0) + ) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf); + } + public static Command stackSequence(Dispenser dispenser, Arm arm) { return Commands.sequence( arm.prepareStack(), @@ -56,6 +64,15 @@ public static Command stackSequence(Dispenser dispenser, Arm arm) { .withInterruptBehavior(InterruptionBehavior.kCancelSelf); } + public static Command stackSet(Dispenser dispenser, Arm arm) { + return Commands.sequence( + arm.prepareStack(), + arm.waitForSetpoint(3.0) + ) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf); + } + + public static Command intakeUntilCanister(Dispenser dispenser, Arm arm) { return Commands.sequence( dispenser.intake(), arm.intake(), dispenser.waitTillCanister(), dispenser.stop());