diff --git a/src/main/deploy/pathplanner/paths/Shoot2 To Climb.path b/src/main/deploy/pathplanner/paths/Shoot2 To Climb.path index e4dfa40..1f195db 100644 --- a/src/main/deploy/pathplanner/paths/Shoot2 To Climb.path +++ b/src/main/deploy/pathplanner/paths/Shoot2 To Climb.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.5481222222222222, - "y": 3.752822222222222 + "x": 0.7419, + "y": 2.664422222222222 }, "prevControl": { - "x": 2.0147248545881693, - "y": 2.880077770320452 + "x": 1.2085026323659471, + "y": 1.7916777703204518 }, "nextControl": null, "isLocked": false, @@ -42,10 +42,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": -0.939190945735607 + "rotation": 180.0 }, "reversed": false, - "folder": "Neutral Zone Climb", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 59.058463533677475 diff --git a/src/main/deploy/pathplanner/paths/ShootToClimb.path b/src/main/deploy/pathplanner/paths/ShootToClimb.path index 9222673..a86a838 100644 --- a/src/main/deploy/pathplanner/paths/ShootToClimb.path +++ b/src/main/deploy/pathplanner/paths/ShootToClimb.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.5380444444444445, - "y": 3.7125111111111115 + "x": 1.3667222222222224, + "y": 4.871455555555555 }, "prevControl": { - "x": 1.920864779920239, - "y": 3.7581512560925034 + "x": 1.7495425576980168, + "y": 4.917095700536947 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1e16f7..36619f4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -288,8 +288,8 @@ public RobotContainer() { autoChooser_ = new LoggedDashboardChooser<>("Auto Choices"); autoChooser_.onChange(auto -> { - System.err.println("Auto \"" + auto.getName() + "\" selected!"); - // This should be used to set up robot position setting, initialization, etc. + System.out.println("Auto \"" + auto.getName() + "\" selected!"); + // Anything you may want to do when the auto is selected. }); // Test Bindings @@ -326,6 +326,9 @@ private void configureBindings() { // When the shooter isnt shooting, get it ready to shoot. shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); + + //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. + gamepad_.a().whileTrue(intake_.ejectSequence()); } private void configureDriveBindings() { diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 167fbcf..db9befe 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -62,6 +62,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.drive.Drive; +import frc.robot.util.MapleSimUtil; public class DriveCommands { private static final double kStoppedVelocity = 0.15 ; @@ -593,10 +594,15 @@ public static Command initialFollowPathCommand(Drive drive, String pathName, boo return Commands.none(); } + var startingPose = initPosePath.getStartingHolonomicPose().orElseThrow(); + return Commands.sequence( setPoseCommand( drive, - initPosePath.getStartingHolonomicPose().orElseThrow(), false), + startingPose, + false + ), + Commands.runOnce(() -> MapleSimUtil.placeRobotOnField(startingPose)), AutoBuilder.followPath(path.get())); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f2d74d3..8f639b1 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -52,6 +52,7 @@ public final class IntakeConstants { public static final AngularVelocity rollerMaxVelocity= DegreesPerSecond.of(360); //Temporary speed to be changed as needed public static final Voltage rollerCollectVoltage= Volts.of(6); //Temporary voltage + public static final Voltage rollerEjectVoltage= Volts.of(-6); //Temporary voltage public static final Angle pivotTolerance = Degrees.of(5); //Tolerance to compare current angle to target diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index d7f18df..ae9b67a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -69,6 +69,10 @@ private void stopIntaking() { io.stopRoller(); } + private void eject(){ + io.setRollerVoltage(IntakeConstants.rollerEjectVoltage); + } + /** * Stows the intake and hopper. */ @@ -118,6 +122,14 @@ public Command stowCmd() { .withTimeout(2)).withName("Stow Intake"); } + /** + * Command that stops the rollers, mainly for in automodes + * @return + */ + public Command stopIntake(){ + return runOnce(() -> stopIntaking()).withName("Stop Rollers"); + } + /** * Command that deploys the intake and ends when it gets there. * @return @@ -150,6 +162,16 @@ public Command intakeSequence() { ).finallyDo(interrupted -> waiting()); } + public Command runEjectCmd() { + return startEnd(this::eject, this::stopIntaking); + } + + public Command ejectSequence() { + return runEjectCmd().beforeStarting( + deployCmd().unless(this::isIntakeDeployed) + ).finallyDo(interrupted -> waiting()); + } + //////////////////////////// /// Sys ID Routine creation/ /// //////////////////////// diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 4645c33..286406f 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -137,6 +137,10 @@ public static Pose2d getPosition() { return drivebaseSimulation.getSimulatedDriveTrainPose(); } + public static void placeRobotOnField(Pose2d pose) { + drivebaseSimulation.setSimulationWorldPose(pose); + } + public static ChassisSpeeds getFieldChassisSpeeds() { return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); }