Merge branch 'main' into pure-pursuit-point-calculation

src/main/java/frc/robot/Robot.java

@@ -51,9 +51,8 @@ public class Robot extends TimedRobot {
   private final LocalizationSubsystem localization = new LocalizationSubsystem(imu, vision, swerve);
   private final RobotManager robotManager =
       new RobotManager(arm, shooter, localization, vision, imu, intake, queuer, swerve);
-
-  private final RobotCommands robotCommands = new RobotCommands(robotManager);
   private final Trailblazer trailblazer = new Trailblazer(swerve, localization);
+  private final RobotCommands robotCommands = new RobotCommands(robotManager, trailblazer);
   private final Autos autos = new Autos(robotManager, trailblazer);
 
   private final LightsSubsystem lights = new LightsSubsystem(robotManager, hardware.candle);

src/main/java/frc/robot/autos/BaseAuto.java

@@ -16,7 +16,7 @@ public abstract class BaseAuto {
   protected BaseAuto(RobotManager robotManager, Trailblazer trailblazer) {
     this.robotManager = robotManager;
     this.trailblazer = trailblazer;
-    this.actions = new RobotCommands(robotManager);
+    this.actions = new RobotCommands(robotManager, trailblazer);
     this.autoCommands = new AutoCommands(actions, robotManager);
   }
 

src/main/java/frc/robot/autos/amp/Op345Auto.java

@@ -48,6 +48,12 @@ protected Command getRedAutoCommand() {
                 new AutoPoint(new Pose2d(8.52, 7.46, Rotation2d.fromDegrees(0.0))),
                 new AutoPoint(new Pose2d(12.58, 6.01, Rotation2d.fromDegrees(-6.63))))),
         Commands.sequence(autoCommands.speakerShotWithTimeout(), actions.intakeAssistCommand()),
+        trailblazer.followSegment(
+
+            new AutoSegment(
+                new AutoPoint(new Pose2d(8.52, 7.46, Rotation2d.fromDegrees(0.0))),
+                new AutoPoint(new Pose2d(12.58, 6.01, Rotation2d.fromDegrees(-6.63))))),
+        Commands.sequence(autoCommands.speakerShotWithTimeout(), actions.intakeAssistCommand()),
         trailblazer.followSegment(
             new AutoSegment(new AutoPoint(new Pose2d(10.71, 6.37, Rotation2d.fromDegrees(0.0))))));
   }

src/main/java/frc/robot/autos/trailblazer/Trailblazer.java

@@ -32,7 +32,8 @@ private static AutoConstraintOptions resolveConstraints(
   private final PathTracker pathTracker = new PurePursuitPathTracker();
   private final PathFollower pathFollower =
       new PidPathFollower(
-          new PIDController(4, 0, 0), new PIDController(4, 0, 0), new PIDController(8, 0, 0));
+          new PIDController(4, 0, 0), new PIDController(4, 0, 0), new PIDController(8.0, 0, 0));
+
   private int previousAutoPointIndex = -1;
   private ChassisSpeeds previousSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
   private double previousTimestamp = 0.0;

src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java

@@ -71,22 +71,21 @@ private static ChassisSpeeds constrainLinearAcceleration(
       double timeBetweenPreviousAndInputSpeeds,
       AutoConstraintOptions options) {
 
-    double deltaVx = inputSpeeds.vxMetersPerSecond - previousSpeeds.vxMetersPerSecond;
-    double deltaVy = inputSpeeds.vyMetersPerSecond - previousSpeeds.vyMetersPerSecond;
-    double unconstrainedLinearAcceleration =
-        Math.sqrt(deltaVx * deltaVx + deltaVy * deltaVy) / timeBetweenPreviousAndInputSpeeds;
-
-    double constrainedLinearAcceleration =
-        Math.min(unconstrainedLinearAcceleration, options.maxLinearAcceleration());
 
+    double currentLinearAcceleration =
+        Math.hypot(inputSpeeds.vxMetersPerSecond, inputSpeeds.vyMetersPerSecond);
+    double previousLinearAcceleration =
+        Math.hypot(previousSpeeds.vxMetersPerSecond, previousSpeeds.vyMetersPerSecond);
+    double unconstrainedLinearAcceleration =
+        (currentLinearAcceleration - previousLinearAcceleration)
+            / timeBetweenPreviousAndInputSpeeds;
+    double preserveTheta = Math.atan(inputSpeeds.vyMetersPerSecond / inputSpeeds.vxMetersPerSecond);
     if (unconstrainedLinearAcceleration > options.maxLinearAcceleration()) {
-      double constrainedVx =
-          previousSpeeds.vxMetersPerSecond
-              + (deltaVx / unconstrainedLinearAcceleration) * constrainedLinearAcceleration;
-      double constrainedVy =
-          previousSpeeds.vyMetersPerSecond
-              + (deltaVy / unconstrainedLinearAcceleration) * constrainedLinearAcceleration;
-
+      double finalAcceleration =
+          previousLinearAcceleration
+              + options.maxLinearAcceleration() * timeBetweenPreviousAndInputSpeeds;
+      double constrainedVx = finalAcceleration * Math.cos(preserveTheta);
+      double constrainedVy = finalAcceleration * Math.sin(preserveTheta);
       return new ChassisSpeeds(constrainedVx, constrainedVy, inputSpeeds.omegaRadiansPerSecond);
     }
     return inputSpeeds;

src/main/java/frc/robot/robot_manager/RobotCommands.java

@@ -1,16 +1,23 @@
 package frc.robot.robot_manager;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.autos.trailblazer.AutoPoint;
+import frc.robot.autos.trailblazer.AutoSegment;
+import frc.robot.autos.trailblazer.Trailblazer;
 import java.util.List;
 
 public class RobotCommands {
   private final RobotManager robot;
   private final Subsystem[] requirements;
+  private final Trailblazer trailblazer;
 
-  public RobotCommands(RobotManager robot) {
+  public RobotCommands(RobotManager robot, Trailblazer trailblazer) {
     this.robot = robot;
+    this.trailblazer = trailblazer;
     var requirementsList = List.of(robot.arm, robot.intake, robot.queuer, robot.shooter);
     requirements = requirementsList.toArray(Subsystem[]::new);
   }
@@ -118,4 +125,21 @@ public Command unjamCommand() {
     return Commands.runOnce(robot::unjamRequest, requirements)
         .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
+
+  public Command redAutoIntake() {
+    return Commands.sequence(
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoPoint(new Pose2d(15.14, 6.02, Rotation2d.fromDegrees(49.1))),
+                new AutoPoint(new Pose2d(15.629, 6.610, Rotation2d.fromDegrees(49.1))),
+                new AutoPoint(new Pose2d(16.079, 7.219, Rotation2d.fromDegrees(51.7))))),
+        Commands.print("Raise elevator, intake coral --> finished"),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoPoint(new Pose2d(15.14, 6.801, Rotation2d.fromDegrees(51.7))))));
+  }
+
+  public Command blueAutoIntake() {
+    return Commands.sequence(null);
+  }
 }