From f254d0c547f30f46a01adc6bed1eb8254092d1ad Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Mon, 18 Nov 2024 23:31:13 -0600 Subject: [PATCH 1/6] added code to our amp shot command that pathfinds to a specific position on the field in front of the amp before starting the amp shot. --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 467a58cd..17f998f4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,6 +73,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -120,6 +122,7 @@ public class RobotContainer { private SendableChooser autoChooser; private PowerDistribution PDP; + Alliance currentAlliance; BooleanSupplier ZeroGyroSup; BooleanSupplier AimWhileMovingSup; BooleanSupplier ShootIfReadySup; @@ -403,8 +406,14 @@ public boolean runsWhenDisabled() { new InstantCommand(()->intake.setNoteHeld(false)) ); SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed); + /** + * the following code producs a command that will first pathfind to a pose right in front of the amplifier, then drive backwards into the amp, then run our shooters amp shot + * sequence as it was at the 2024 State competition. + */ SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup( new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter), + new AutoBuilder().pathfindToPose(getAmpShotPose(), DEFAUL_PATH_CONSTRAINTS), + new MoveMeters(driveTrain, 2, -2, 0, 0), new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), new InstantCommand(driveTrain::pointWheelsInward, driveTrain), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem), @@ -539,6 +548,13 @@ private void configureDriveTrain() { ); } + private Pose2d getAmpShotPose() { + if(currentAlliance == Alliance.Blue) { + return new Pose2d(1.83, 6.88, new Rotation2d(Math.toRadians(-90))); + } else { + return new Pose2d(14.75, 6.88, new Rotation2d(Math.toRadians(-90))); + } + } private void registerNamedCommands() { NamedCommands.registerCommand("awayFromPodium", new MoveMeters(driveTrain, 0.2, 1, 0, 0)); NamedCommands.registerCommand("stopDrivetrain", new InstantCommand(driveTrain::stop, driveTrain)); @@ -638,6 +654,7 @@ public void teleopInit() { } public void teleopPeriodic() { SmartDashboard.putData(driveTrain.getCurrentCommand()); + currentAlliance = DriverStation.getAlliance().get(); driveTrain.calculateSpeakerAngle(); if(useDetectorLimelight) { SmartDashboard.putNumber("Is Note Seen?", limelight.getNeuralDetectorValues().ta); From c2da3fb403cb03adf3651936d348ab2e9d50c159 Mon Sep 17 00:00:00 2001 From: Liam Sykes <2491nomythic@gmail.com> Date: Wed, 20 Nov 2024 18:30:05 -0600 Subject: [PATCH 2/6] patched odometry and shooting issues --- src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index 274e4e7d..4ee9e641 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -172,7 +172,7 @@ public double calculateSpeakerAngle() { offsetSpeakerX = Field.SHOOTER_RED_SPEAKER_X - targetOffset.getX(); offsetSpeakerY = Field.RED_SPEAKER_Y - targetOffset.getY(); } else { - offsetSpeakerX = Field.SHOOTER_BLUE_SPEAKER_X - targetOffset.getX(); + offsetSpeakerX = Field.SHOOTER_BLUE_SPEAKER_X + targetOffset.getX(); offsetSpeakerY = Field.BLUE_SPEAKER_Y - targetOffset.getY(); } double offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index f576cb16..3ee5c78b 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -324,7 +324,7 @@ public void updateOdometryWithVision() { } if(!doRejectUpdate) { Logger.recordOutput("Vision/MergesPose", estimate.pose); - odometer.addVisionMeasurement(new Pose2d(estimate.pose.getX(), estimate.pose.getY(), getGyroscopeRotation()), estimate.timestampSeconds); + odometer.addVisionMeasurement(estimate.pose, estimate.timestampSeconds); } RobotState.getInstance().LimelightsUpdated = true; } else { From 1f7e5859ddcbb067bca6391199802d800b5e5ea8 Mon Sep 17 00:00:00 2001 From: Liam Sykes <2491nomythic@gmail.com> Date: Wed, 20 Nov 2024 18:30:24 -0600 Subject: [PATCH 3/6] reordered initializations so that pathfindtoPose could be used in the amp shot --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17f998f4..21a32793 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -219,6 +219,7 @@ public RobotContainer() { if(indexerExists) {indexInit();} if(intakeExists && shooterExists && indexerExists && angleShooterExists) {indexCommandInst();} Limelight.useDetectorLimelight(useDetectorLimelight); + configureDriveTrain(); configureBindings(); autoInit(); // Configure the trigger bindings @@ -276,7 +277,6 @@ private void indexCommandInst() { } private void autoInit() { - configureDriveTrain(); registerNamedCommands(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -549,10 +549,13 @@ private void configureDriveTrain() { } private Pose2d getAmpShotPose() { - if(currentAlliance == Alliance.Blue) { - return new Pose2d(1.83, 6.88, new Rotation2d(Math.toRadians(-90))); - } else { - return new Pose2d(14.75, 6.88, new Rotation2d(Math.toRadians(-90))); + if(currentAlliance == null) { return new Pose2d(5,5, new Rotation2d(Math.toRadians(-90)));} + else { + if(currentAlliance == Alliance.Blue) { + return new Pose2d(1.83, 6.88, new Rotation2d(Math.toRadians(-90))); + } else { + return new Pose2d(14.75, 6.88, new Rotation2d(Math.toRadians(-90))); + } } } private void registerNamedCommands() { @@ -654,7 +657,6 @@ public void teleopInit() { } public void teleopPeriodic() { SmartDashboard.putData(driveTrain.getCurrentCommand()); - currentAlliance = DriverStation.getAlliance().get(); driveTrain.calculateSpeakerAngle(); if(useDetectorLimelight) { SmartDashboard.putNumber("Is Note Seen?", limelight.getNeuralDetectorValues().ta); @@ -672,6 +674,8 @@ public void logPower(){ } } public void robotPeriodic() { + currentAlliance = DriverStation.getAlliance().get(); + // logPower(); } public void disabledPeriodic() { From baf023c92c000868577fd012529683e628b1549d Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 26 Nov 2024 08:38:37 -0600 Subject: [PATCH 4/6] changed the moving amp shot to use pathFindThenFollowPath instead of pathFindToPose, since we can't choose our robots heading with pathFindToPose --- .../pathplanner/paths/AmpShotSetup.path | 52 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 5 +- 2 files changed, 55 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/AmpShotSetup.path diff --git a/src/main/deploy/pathplanner/paths/AmpShotSetup.path b/src/main/deploy/pathplanner/paths/AmpShotSetup.path new file mode 100644 index 00000000..2eec54ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AmpShotSetup.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.8, + "y": 7.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8, + "y": 7.7 + }, + "prevControl": { + "x": 1.8, + "y": 7.6000000000000005 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 2.3, + "maxAngularVelocity": 650.0, + "maxAngularAcceleration": 350.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21a32793..1fda313d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; @@ -412,8 +413,8 @@ public boolean runsWhenDisabled() { */ SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup( new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter), - new AutoBuilder().pathfindToPose(getAmpShotPose(), DEFAUL_PATH_CONSTRAINTS), - new MoveMeters(driveTrain, 2, -2, 0, 0), + new AutoBuilder().pathfindThenFollowPath(PathPlannerPath.fromPathFile("AmpShotSetup"), DEFAUL_PATH_CONSTRAINTS), + new MoveMeters(driveTrain, 2, -0.5, 0, 0), new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), new InstantCommand(driveTrain::pointWheelsInward, driveTrain), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem), From ccdc920fa3730312aaeb75110b4e74c2757c9627 Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 26 Nov 2024 18:50:16 -0600 Subject: [PATCH 5/6] decent commit --- .../pathplanner/autos/testAmpShotAuto.auto | 25 ++++++ .../pathplanner/paths/AmpShotSetup.path | 10 ++- src/main/java/frc/robot/RobotContainer.java | 89 ++++++++++--------- 3 files changed, 80 insertions(+), 44 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/testAmpShotAuto.auto diff --git a/src/main/deploy/pathplanner/autos/testAmpShotAuto.auto b/src/main/deploy/pathplanner/autos/testAmpShotAuto.auto new file mode 100644 index 00000000..d0453627 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/testAmpShotAuto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.786947863972064, + "y": 6.987301913535124 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "rotate path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AmpShotSetup.path b/src/main/deploy/pathplanner/paths/AmpShotSetup.path index 2eec54ab..e46fca0a 100644 --- a/src/main/deploy/pathplanner/paths/AmpShotSetup.path +++ b/src/main/deploy/pathplanner/paths/AmpShotSetup.path @@ -28,7 +28,13 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -90.0, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -45,7 +51,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -60.0, + "rotation": -90.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fda313d..8b170121 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -223,6 +223,7 @@ public RobotContainer() { configureDriveTrain(); configureBindings(); autoInit(); + ampShotInit(); // Configure the trigger bindings } private void climbSpotChooserInit() { @@ -386,47 +387,7 @@ public boolean runsWhenDisabled() { if(intakeExists&&indexerExists) { new Trigger(intake::isNoteSeen).and(()->!intake.isNoteHeld()).and(()->DriverStation.isTeleop()).and(()->!AimWhileMovingSup.getAsBoolean()).onTrue(new IndexerNoteAlign(indexer, intake).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).withTimeout(5)); } - if(indexerExists&&shooterExists&&angleShooterExists) { - double indexerAmpSpeed; - double shooterAmpSpeed; - if(Preferences.getBoolean("CompBot", true)) { - shooterAmpSpeed = ShooterConstants.COMP_AMP_RPS; - indexerAmpSpeed = IndexerConstants.COMP_INDEXER_AMP_SPEED; - } else { - shooterAmpSpeed = ShooterConstants.PRAC_AMP_RPS; - indexerAmpSpeed = IndexerConstants.PRAC_INDEXER_AMP_SPEED; - } - SequentialCommandGroup scoreAmp = new SequentialCommandGroup( - // new InstantCommand(()->shooter.shootSameRPS(ShooterConstants.AMP_RPS), shooter), - new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter), - new MoveMeters(driveTrain, 0.06, 0.3, 0, 0), - // new WaitCommand(2), - new WaitUntil(()->(shooter.validShot() && driveTrain.getChassisSpeeds().vxMetersPerSecond == 0)), - new InstantCommand(()->indexer.magicRPS(indexerAmpSpeed), indexer),//45 worked but a bit too high - new WaitCommand(0.5), - new InstantCommand(()->intake.setNoteHeld(false)) - ); - SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed); - /** - * the following code producs a command that will first pathfind to a pose right in front of the amplifier, then drive backwards into the amp, then run our shooters amp shot - * sequence as it was at the 2024 State competition. - */ - SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup( - new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter), - new AutoBuilder().pathfindThenFollowPath(PathPlannerPath.fromPathFile("AmpShotSetup"), DEFAUL_PATH_CONSTRAINTS), - new MoveMeters(driveTrain, 2, -0.5, 0, 0), - new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), - new InstantCommand(driveTrain::pointWheelsInward, driveTrain), - new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem), - new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<0.3)), - new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), - new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), - new WaitCommand(0.5), - new InstantCommand(()->intake.setNoteHeld(false)) - ); - new Trigger(AmpAngleSup).whileTrue(orbitAmpShot); - SmartDashboard.putData("amp shot", scoreAmp); - } + SmartDashboard.putData("move 1 meter", new MoveMeters(driveTrain, 1, 0.2, 0.2, 0.2)); InstantCommand setOffsets = new InstantCommand(driveTrain::setEncoderOffsets) { public boolean runsWhenDisabled() { @@ -528,7 +489,7 @@ private void configureDriveTrain() { driveTrain::getPose, // Pose2d supplier driveTrain::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto driveTrain::getChassisSpeeds, - driveTrain::driveWhileAimed, + driveTrain::drive, new HolonomicPathFollowerConfig( new PIDConstants( k_XY_P, @@ -642,6 +603,50 @@ public void execute() { } NamedCommands.registerCommand("wait x seconds", new WaitCommand(Preferences.getDouble("wait # of seconds", 0))); } + public void ampShotInit() { + if(indexerExists&&shooterExists&&angleShooterExists) { + double indexerAmpSpeed; + double shooterAmpSpeed; + if(Preferences.getBoolean("CompBot", true)) { + shooterAmpSpeed = ShooterConstants.COMP_AMP_RPS; + indexerAmpSpeed = IndexerConstants.COMP_INDEXER_AMP_SPEED; + } else { + shooterAmpSpeed = ShooterConstants.PRAC_AMP_RPS; + indexerAmpSpeed = IndexerConstants.PRAC_INDEXER_AMP_SPEED; + } + SequentialCommandGroup scoreAmp = new SequentialCommandGroup( + // new InstantCommand(()->shooter.shootSameRPS(ShooterConstants.AMP_RPS), shooter), + new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter), + new MoveMeters(driveTrain, 0.06, 0.3, 0, 0), + // new WaitCommand(2), + new WaitUntil(()->(shooter.validShot() && driveTrain.getChassisSpeeds().vxMetersPerSecond == 0)), + new InstantCommand(()->indexer.magicRPS(indexerAmpSpeed), indexer),//45 worked but a bit too high + new WaitCommand(0.5), + new InstantCommand(()->intake.setNoteHeld(false)) + ); + SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed); + /** + * the following code producs a command that will first pathfind to a pose right in front of the amplifier, then drive backwards into the amp, then run our shooters amp shot + * sequence as it was at the 2024 State competition. + */ + SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup( + new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter), + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem), + // new AutoBuilder().pathfindThenFollowPath(PathPlannerPath.fromPathFile("AmpShotSetup"), DEFAUL_PATH_CONSTRAINTS), + new AutoBuilder().pathfindToPose(getAmpShotPose(), DEFAUL_PATH_CONSTRAINTS), + new MoveMeters(driveTrain, 0.9, -0.5, 0, 0), + new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), + new InstantCommand(driveTrain::pointWheelsInward, driveTrain), + new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<0.3)), + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), + new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), + new WaitCommand(0.5), + new InstantCommand(()->intake.setNoteHeld(false)) + ); + new Trigger(AmpAngleSup).whileTrue(orbitAmpShot); + SmartDashboard.putData("amp shot", scoreAmp); + } + } public void teleopInit() { if(climberExists) { SequentialCommandGroup resetClimbers = new SequentialCommandGroup( From 1ffb5078b9d3c745549e5f58d122df87ddd1fc96 Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 26 Nov 2024 19:41:04 -0600 Subject: [PATCH 6/6] changed the variable name of DEFAULT_PATH_CONSTRAINTS, and redid our system for choosing a position for the amp lineup based on alliance --- src/main/java/frc/robot/RobotContainer.java | 12 ++++++++--- .../frc/robot/commands/goToPose/GoToAmp.java | 4 ++-- .../commands/goToPose/GoToClimbSpot.java | 20 +++++++++---------- .../java/frc/robot/settings/Constants.java | 2 +- 4 files changed, 22 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b170121..24c219af 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import static frc.robot.settings.Constants.ShooterConstants.PRAC_AMP_RPS; import static frc.robot.settings.Constants.ShooterConstants.LONG_SHOOTING_RPS; +import java.util.Map; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -61,6 +62,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; @@ -510,7 +512,7 @@ private void configureDriveTrain() { ); } - private Pose2d getAmpShotPose() { + private Pose2d getAmpShotPose(Alliance currentAlliance) { if(currentAlliance == null) { return new Pose2d(5,5, new Rotation2d(Math.toRadians(-90)));} else { if(currentAlliance == Alliance.Blue) { @@ -633,7 +635,10 @@ public void ampShotInit() { new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem), // new AutoBuilder().pathfindThenFollowPath(PathPlannerPath.fromPathFile("AmpShotSetup"), DEFAUL_PATH_CONSTRAINTS), - new AutoBuilder().pathfindToPose(getAmpShotPose(), DEFAUL_PATH_CONSTRAINTS), + Commands.select(Map.of( + Alliance.Red, AutoBuilder.pathfindToPose(getAmpShotPose(Alliance.Red), DEFAULT_PATH_CONSTRAINTS), + Alliance.Blue, AutoBuilder.pathfindToPose(getAmpShotPose(Alliance.Blue), DEFAULT_PATH_CONSTRAINTS) + ), ()->currentAlliance==null ? Alliance.Red : currentAlliance), new MoveMeters(driveTrain, 0.9, -0.5, 0, 0), new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), new InstantCommand(driveTrain::pointWheelsInward, driveTrain), @@ -681,7 +686,8 @@ public void logPower(){ } public void robotPeriodic() { currentAlliance = DriverStation.getAlliance().get(); - + SmartDashboard.putBoolean("RobotPeriodicRan", true); + SmartDashboard.putString("AlliancePeriodic", currentAlliance == null? "null" : currentAlliance == Alliance.Red? "Red": "Blue" ); // logPower(); } public void disabledPeriodic() { diff --git a/src/main/java/frc/robot/commands/goToPose/GoToAmp.java b/src/main/java/frc/robot/commands/goToPose/GoToAmp.java index 25ff8f26..0ba94741 100644 --- a/src/main/java/frc/robot/commands/goToPose/GoToAmp.java +++ b/src/main/java/frc/robot/commands/goToPose/GoToAmp.java @@ -4,7 +4,7 @@ package frc.robot.commands.goToPose; -import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS; +import static frc.robot.settings.Constants.DriveConstants.DEFAULT_PATH_CONSTRAINTS; import java.util.function.BooleanSupplier; @@ -30,7 +30,7 @@ public GoToAmp(DrivetrainSubsystem drivetrain) { public void initialize() { // PathPlannerPath ampPath = PathPlannerPath.fromPathFile("goToAmp"); PathPlannerPath ampPath = PathPlannerPath.fromPathFile("ScoreAmp"); - actualCommand = AutoBuilder.pathfindThenFollowPath(ampPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(ampPath, DEFAULT_PATH_CONSTRAINTS); actualCommand.initialize(); } diff --git a/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java b/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java index 06373c62..e121ef8d 100644 --- a/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java +++ b/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java @@ -4,7 +4,7 @@ package frc.robot.commands.goToPose; -import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS; +import static frc.robot.settings.Constants.DriveConstants.DEFAULT_PATH_CONSTRAINTS; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; @@ -40,38 +40,38 @@ public void initialize() { switch (climbSpotChooser.getSelected()) { case "L-Chain Left": System.out.println("L-Chain Source!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideLPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideLPath, DEFAULT_PATH_CONSTRAINTS); break; case "L-Chain Middle": System.out.println("L-Chain Middle!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidLPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidLPath, DEFAULT_PATH_CONSTRAINTS); break; case "L-Chain Right": System.out.println("L-Chain Amp!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideLPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideLPath, DEFAULT_PATH_CONSTRAINTS); case "Mid-Chain Left": System.out.println("Mid-Chain Source!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideMPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideMPath, DEFAULT_PATH_CONSTRAINTS); break; case "Mid-Chain Middle": System.out.println("Mid-Chain Middle!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidMPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidMPath, DEFAULT_PATH_CONSTRAINTS); break; case "Mid-Chain Right": System.out.println("Mid-Chain Amp!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideMPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideMPath, DEFAULT_PATH_CONSTRAINTS); break; case "R-Chain Left": System.out.println("R-Chain Source!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideRPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideRPath, DEFAULT_PATH_CONSTRAINTS); break; case "R-Chain Middle": System.out.println("R-Chain Middle!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidRPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidRPath, DEFAULT_PATH_CONSTRAINTS); break; case "R-Chain Right": System.out.println("R-Chain Amp!"); - actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideRPath, DEFAUL_PATH_CONSTRAINTS); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideRPath, DEFAULT_PATH_CONSTRAINTS); break; default: System.out.println("We broke!(Or went to the default command)"); diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 1de9d097..a378e4e5 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -209,7 +209,7 @@ public double getValue() { public static final double k_BALANCE_TOLORANCE_DEGREES = 10.0; public static final double k_BALANCE_TOLORANCE_DEG_PER_SEC = 1; - public static final PathConstraints DEFAUL_PATH_CONSTRAINTS = new PathConstraints(2, 1.5, Math.toRadians(360), Math.toRadians(360)); + public static final PathConstraints DEFAULT_PATH_CONSTRAINTS = new PathConstraints(2, 1.5, Math.toRadians(360), Math.toRadians(360)); public static final double k_PICKUP_NOTE_ta_P = 1; public static final double k_PICKUP_NOTE_ta_I = 0;