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;