Remove segment motion profile and format

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

@@ -28,58 +28,36 @@ protected Command getRedAutoCommand() {
         Commands.runOnce(
             () ->
                 robotManager.localization.resetPose(
-                  new Pose2d(9.0, 4.0, Rotation2d.fromDegrees(0)))),
+                    new Pose2d(9.0, 4.0, Rotation2d.fromDegrees(0)))),
         trailblazer.followSegment(
-          new AutoSegment(
-              new AutoConstraintOptions(false,5,500,8.0,5000),
-                new AutoPoint(
-                    new Pose2d(11.8, 4.0, Rotation2d.fromDegrees(0))))
-        ),
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 5000),
+                new AutoPoint(new Pose2d(11.8, 4.0, Rotation2d.fromDegrees(0))))),
         trailblazer.followSegment(
             new AutoSegment(
-              new AutoConstraintOptions(false,5,500,8.0,500),
-
-                new AutoPoint(
-                    new Pose2d(10.9,5.0, Rotation2d.fromDegrees(-10))),
-                new AutoPoint(
-                    new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))
-        ),
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(10.9, 5.0, Rotation2d.fromDegrees(-10))),
+                new AutoPoint(new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))),
         trailblazer.followSegment(
             new AutoSegment(
-              new AutoConstraintOptions(false,5,500,8.0,500),
-
-                new AutoPoint(
-                    new Pose2d(13.6,5.2, Rotation2d.fromDegrees(-120.559))))
-        ),
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(13.6, 5.2, Rotation2d.fromDegrees(-120.559))))),
         trailblazer.followSegment(
             new AutoSegment(
-              new AutoConstraintOptions(false,5,500,8.0,500),
-
-                new AutoPoint(
-                    new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))
-        ),
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))),
         trailblazer.followSegment(
             new AutoSegment(
-              new AutoConstraintOptions(false,5,500,8.0,500),
-
-                new AutoPoint(
-                    new Pose2d(13.6,5.2, Rotation2d.fromDegrees(-120.559))))
-        ),
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(13.6, 5.2, Rotation2d.fromDegrees(-120.559))))),
         trailblazer.followSegment(
-          new AutoSegment(
-            new AutoConstraintOptions(false,5,500,8.0,500),
-
-              new AutoPoint(
-                  new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))
-      ),
-      trailblazer.followSegment(
             new AutoSegment(
-              new AutoConstraintOptions(false,5,500,8.0,500),
-
-            new AutoPoint(
-                new Pose2d(12.2, 5.5, Rotation2d.fromDegrees(-60.0))),
-                new AutoPoint(new Pose2d(12.46, 5.16, Rotation2d.fromDegrees(-59.927))))
-    )
-                    );
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(12.2, 5.5, Rotation2d.fromDegrees(-60.0))),
+                new AutoPoint(new Pose2d(12.46, 5.16, Rotation2d.fromDegrees(-59.927))))));
   }
 }

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

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.autos.trailblazer.constraints.AutoConstraintCalculator;
 import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions;
-import frc.robot.autos.trailblazer.constraints.SegmentMotionProfile;
 import frc.robot.autos.trailblazer.followers.PathFollower;
 import frc.robot.autos.trailblazer.followers.PidPathFollower;
 import frc.robot.autos.trailblazer.trackers.PathTracker;
@@ -23,14 +22,9 @@ public class Trailblazer {
    * while following that point.
    */
   private static AutoConstraintOptions resolveConstraints(
-      AutoPoint point,
-      AutoConstraintOptions segmentConstraints,
-      SegmentMotionProfile motionProfile,
-      Pose2d robotPose) {
+      AutoPoint point, AutoConstraintOptions segmentConstraints) {
     var constraints = point.constraints.orElse(segmentConstraints);
     return constraints;
-    // return AutoConstraintCalculator.generateLinearVelocityConstraint(
-    //     robotPose,motionProfile, constraints);
   }
 
   private final SwerveSubsystem swerve;
@@ -42,10 +36,6 @@ private static AutoConstraintOptions resolveConstraints(
   private int previousAutoPointIndex = -1;
   private ChassisSpeeds previousSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
   private double previousTimestamp = 0.0;
-  private int lastMotionProfileIndex = -1;
-  private SegmentMotionProfile motionProfile = new SegmentMotionProfile(new Pose2d(), 0, 0, 0);
-  private double currentEndVelocity = 0.0;
-  private double currentMaxAcceleration = 0.0;
 
   public Trailblazer(SwerveSubsystem swerve, LocalizationSubsystem localization) {
     this.swerve = swerve;
@@ -62,7 +52,6 @@ public Command followSegment(AutoSegment segment, boolean shouldEnd) {
                 () -> {
                   pathTracker.resetAndSetPoints(segment.points);
                   previousAutoPointIndex = -1;
-                  lastMotionProfileIndex = -1;
                   DogLog.log(
                       "Trailblazer/CurrentSegment/InitialPoints",
                       segment.points.stream()
@@ -77,15 +66,6 @@ public Command followSegment(AutoSegment segment, boolean shouldEnd) {
                       var currentAutoPointIndex = pathTracker.getCurrentPointIndex();
                       var currentAutoPoint = segment.points.get(currentAutoPointIndex);
 
-                      if (lastMotionProfileIndex != currentAutoPointIndex) {
-                        var currentAutoPointConstraints =
-                            currentAutoPoint.constraints.orElse(segment.defaultConstraints);
-                        currentEndVelocity = currentAutoPointConstraints.maxLinearVelocity();
-                        currentMaxAcceleration =
-                            currentAutoPointConstraints.maxLinearAcceleration();
-                        lastMotionProfileIndex = currentAutoPointIndex;
-                      }
-
                       var constrainedVelocityGoal =
                           getSwerveSetpoint(currentAutoPoint, segment.defaultConstraints);
                       swerve.setFieldRelativeAutoSpeeds(constrainedVelocityGoal);
@@ -135,10 +115,8 @@ private ChassisSpeeds getSwerveSetpoint(
     var originalVelocityGoal = pathFollower.calculateSpeeds(robotPose, originalTargetPose);
     var currentVelocity =
         Math.hypot(originalVelocityGoal.vxMetersPerSecond, originalVelocityGoal.vyMetersPerSecond);
-    motionProfile =
-        new SegmentMotionProfile(
-            point.poseSupplier.get(), currentEndVelocity, currentVelocity, currentMaxAcceleration);
-    var usedConstraints = resolveConstraints(point, segmentConstraints, motionProfile, robotPose);
+
+    var usedConstraints = resolveConstraints(point, segmentConstraints);
     DogLog.log(
         "Trailblazer/Constraints/VelocityCalculation/CalculatedVelocity",
         usedConstraints.maxLinearVelocity());

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

@@ -1,39 +1,9 @@
 package frc.robot.autos.trailblazer.constraints;
 
-import dev.doglog.DogLog;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 
 public class AutoConstraintCalculator {
 
-  public static AutoConstraintOptions generateLinearVelocityConstraint(
-      Pose2d robotPose, SegmentMotionProfile motionProfile, AutoConstraintOptions constraints) {
-    var endPoint = motionProfile.endPose();
-    var endVelocity = motionProfile.endVelocity();
-    var currentVelocity = motionProfile.currentVelocity();
-    var maxAcceleration = motionProfile.maxLinearAcceleration();
-    DogLog.log("Trailblazer/Constraints/VelocityCalculation/EndVelocityGoal", endVelocity);
-    DogLog.log("Trailblazer/Constraints/VelocityCalculation/CurrentVelocity", currentVelocity);
-    DogLog.log("Trailblazer/Constraints/VelocityCalculation/MaxAccelaration", maxAcceleration);
-
-    var distanceToEnd = robotPose.getTranslation().getDistance(endPoint.getTranslation());
-    var currentAcceleration =
-        ((endVelocity * endVelocity) - (currentVelocity * currentVelocity)) / (2 * distanceToEnd);
-    if (Math.abs(currentAcceleration) > maxAcceleration) {
-      var goalAcceleration = Math.copySign(maxAcceleration, currentAcceleration);
-      var newVelocity =
-          Math.sqrt((endVelocity * endVelocity) - (goalAcceleration * (2 * distanceToEnd)));
-
-      return new AutoConstraintOptions(
-          constraints.collisionAvoidance(),
-          newVelocity,
-          constraints.maxAngularVelocity(),
-          constraints.maxLinearAcceleration(),
-          constraints.maxAngularAcceleration());
-    }
-    return constraints;
-  }
-
   public static ChassisSpeeds constrainVelocityGoal(
       ChassisSpeeds inputSpeeds,
       ChassisSpeeds previousSpeeds,

src/main/java/frc/robot/autos/trailblazer/trackers/pure_pursuit/PurePursuitPathTracker.java

@@ -1,7 +1,5 @@
 package frc.robot.autos.trailblazer.trackers.pure_pursuit;
 
-import java.util.List;
-
 import dev.doglog.DogLog;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -10,6 +8,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.autos.trailblazer.AutoPoint;
 import frc.robot.autos.trailblazer.trackers.PathTracker;
+import java.util.List;
 
 // TODO: Implement https://github.com/team581/2024-offseason-bot/issues/95
 public class PurePursuitPathTracker implements PathTracker {