added feature to the amp shot that will line up the robot in front of the amp automatically

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
+}

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
+}

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<Command> 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() {

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();
   }
 

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)");

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;

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);

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 {