changed the variable name of DEFAULT_PATH_CONSTRAINTS, and redid our …

…system for choosing a position for the amp lineup based on alliance
2491-NoMythic · Nov 27, 2024 · 1ffb507 · 1ffb507
1 parent ccdc920
commit 1ffb507
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 16 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;