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 new file mode 100644 index 00000000..e46fca0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AmpShotSetup.path @@ -0,0 +1,58 @@ +{ + "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": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -90.0, + "rotateFast": true + } + ], + "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": -90.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 467a58cd..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; @@ -17,6 +18,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; @@ -60,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; @@ -73,6 +76,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 +125,7 @@ public class RobotContainer { private SendableChooser autoChooser; private PowerDistribution PDP; + Alliance currentAlliance; BooleanSupplier ZeroGyroSup; BooleanSupplier AimWhileMovingSup; BooleanSupplier ShootIfReadySup; @@ -216,8 +222,10 @@ public RobotContainer() { if(indexerExists) {indexInit();} if(intakeExists && shooterExists && indexerExists && angleShooterExists) {indexCommandInst();} Limelight.useDetectorLimelight(useDetectorLimelight); + configureDriveTrain(); configureBindings(); autoInit(); + ampShotInit(); // Configure the trigger bindings } private void climbSpotChooserInit() { @@ -273,7 +281,6 @@ private void indexCommandInst() { } private void autoInit() { - configureDriveTrain(); registerNamedCommands(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -382,41 +389,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); - SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup( - new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter), - 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() { @@ -518,7 +491,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, @@ -539,6 +512,16 @@ private void configureDriveTrain() { ); } + private Pose2d getAmpShotPose(Alliance currentAlliance) { + 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() { NamedCommands.registerCommand("awayFromPodium", new MoveMeters(driveTrain, 0.2, 1, 0, 0)); NamedCommands.registerCommand("stopDrivetrain", new InstantCommand(driveTrain::stop, driveTrain)); @@ -622,6 +605,53 @@ 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), + 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), + 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( @@ -655,6 +685,9 @@ 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;