Feature/coral station autoalign

src/main/java/frc/robot/RobotContainer.java

@@ -65,8 +65,8 @@
 import frc.robot.subsystems.drive.commands.DriveCommandFactory;
 import frc.robot.subsystems.drive.commands.DriveWeightCommand;
 import frc.robot.subsystems.drive.weights.AntiTipWeight;
+import frc.robot.subsystems.drive.weights.CStationAlignWeight;
 import frc.robot.subsystems.drive.weights.DynamicAlignWeight;
-import frc.robot.subsystems.drive.weights.FollowPathDirect;
 import frc.robot.subsystems.drive.weights.FollowPathNearest;
 import frc.robot.subsystems.drive.weights.JoystickDriveWeight;
 import frc.robot.subsystems.drive.weights.LocalTagAlignWeight;
@@ -149,9 +149,10 @@ public class RobotContainer {
   private boolean premoveLift = false;
 
   private FollowPathNearest followPathReef;
-  private FollowPathDirect followPathCoral;
+  // private FollowPathDirect followPathCoral;
   private LocalTagAlignWeight localAlign;
   private DynamicAlignWeight dynamicAlign;
+  private CStationAlignWeight cStationAlignWeight;
 
   private final JoystickDriveWeight joystickDriveWeight;
 
@@ -337,18 +338,29 @@ public RobotContainer() {
                     () -> coralPreset),
             driveSubsystem);
 
-    followPathCoral =
-        new FollowPathDirect(
+    // followPathCoral =
+    //     new FollowPathDirect(
+    //         () ->
+    //             AllianceManager.chooseFromAlliance(
+    //                 FieldConstants.coralStationPosBlue, FieldConstants.coralStationPosRed),
+    //         (x) ->
+    //             DistanceManager.addRotatedDim(
+    //                 x, ((-RobotDimensions.robotLength - 0.08) / 2), x.getRotation()),
+    //         gyro,
+    //         () -> RobotOdometry.instance.getPose("Main"),
+    //         AutoAlignConfig.coralStationPathConstraints,
+    //         driveSubsystem);
+
+    cStationAlignWeight =
+        new CStationAlignWeight(
             () ->
                 AllianceManager.chooseFromAlliance(
                     FieldConstants.coralStationPosBlue, FieldConstants.coralStationPosRed),
-            (x) ->
-                DistanceManager.addRotatedDim(
-                    x, ((-RobotDimensions.robotLength - 0.08) / 2), x.getRotation()),
-            gyro,
+            (x) -> x,
             () -> RobotOdometry.instance.getPose("Main"),
-            AutoAlignConfig.coralStationPathConstraints,
-            driveSubsystem);
+            driveSubsystem,
+            driveCommandFactory,
+            gyro);
 
     DriveWeightCommand.addPersistentWeight(joystickDriveWeight);
 
@@ -427,6 +439,7 @@ public void periodic() {
                         .getTranslation()
                         .getDistance(getTarget().getTranslation()));
                 Logger.recordOutput("target", getTarget());
+                Logger.recordOutput("StationAlignDone", cStationAlignWeight.isAutoalignComplete());
               }
             });
 
@@ -509,8 +522,11 @@ private void configureBindings() {
     DriveWeightCommand.createWeightTrigger(
         dynamicAlign,
         () -> driveController.a().getAsBoolean() && !dynamicAlign.globalAlignComplete());
-    followPathCoral.generateTrigger(
-        () -> driveHID.getLeftBumperButton() && !followPathCoral.isAutoalignComplete());
+    // followPathCoral.generateTrigger(
+    //     () -> driveHID.getLeftBumperButton() && !followPathCoral.isAutoalignComplete());
+    DriveWeightCommand.createWeightTrigger(
+        cStationAlignWeight,
+        () -> driveHID.getLeftBumperButton() && !cStationAlignWeight.isAutoalignComplete());
 
     new Trigger(
             () ->

src/main/java/frc/robot/constants/FieldConstants.java

@@ -67,12 +67,12 @@ public class FieldConstants {
   // Based off of the 2025-reefscape.json
   public static final Pose2d[] coralStationPosBlue =
       new Pose2d[] {
-        new Pose2d(0.851154, 0.65532, Rotation2d.fromDegrees(54)), // 12
-        new Pose2d(0.851154, 7.3964799999999995, Rotation2d.fromDegrees(306)) // 13
+        new Pose2d(1.526, 0.914, Rotation2d.fromDegrees(50)), // 12
+        new Pose2d(1.645, 7.196, Rotation2d.fromDegrees(-50.000)) // 13
       };
   public static final Pose2d[] coralStationPosRed =
       new Pose2d[] {
-        new Pose2d(16.697198, 0.65532, Rotation2d.fromDegrees(126)), // 1
-        new Pose2d(16.697198, 7.3964799999999995, Rotation2d.fromDegrees(234)) // 2
+        new Pose2d(16.54 - 1.526, .914, Rotation2d.fromDegrees(50)), // 1
+        new Pose2d(16.54 - 1.645, 7.196, Rotation2d.fromDegrees(234)) // 2
       };
 }

src/main/java/frc/robot/constants/RobotConstants.java

@@ -123,6 +123,9 @@ public static class AutoAlignConfig {
     public static final Constraints localAlignPpidConstraints =
         new Constraints(DriveConstants.maxSpeed, 3);
     public static final double profiledDistThreshold = 0.4;
+    // cStation align
+    public static final Constraints cStationPpidConstraints =
+        new Constraints(DriveConstants.maxSpeed, 2);
   }
 
   public static class DriveConstants {
@@ -169,6 +172,11 @@ public static class DriveConstants {
 
     public static final double maxAntiTipCorrectionSpeed = 1.5;
     public static final double minTipDegrees = 3;
+
+    // public static final double cStationDriveMaxSpeed = 4.5;
+    public static final double cStationSlowAlignDistance = 0.05;
+    public static final TrapezoidProfile.Constraints cStationDriveConstraints =
+        new TrapezoidProfile.Constraints(maxSpeed, accelLimit);
   }
 
   public static class CameraConstants {

src/main/java/frc/robot/constants/RobotPIDConstants.java

@@ -98,7 +98,9 @@ public static final MAXMotionConfig constructMaxMotionPos(PIDConstants constant)
 
   public static final PIDConstants linearDrivePID = new PIDConstants(0.25, 0, 0);
 
-  public static final PIDConstants linearDrivePIDProfiled = new PIDConstants(0.1, 0, 0);
+  public static final PIDConstants cStationDrivePID = new PIDConstants(0.2, 0, 0.4);
+  public static final PIDConstants cStationDrivePPID = new PIDConstants(0.75, 0, 0.3);
+  public static final PIDConstants cStationAnglePid = new PIDConstants(0.8, 0.001, 0.001);
 
   public static final PIDConstants rotateToAnglePIDRadians = new PIDConstants(0.5, 0.001, 0.0001);
   public static final PIDConstants angleFollowPath = new PIDConstants(1, 0, 0);

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -162,6 +162,7 @@ public ChassisSpeeds getChassisSpeeds() {
     return DriveConstants.kinematics.toChassisSpeeds(getActualSwerveStates());
   }
 
+  @AutoLogOutput(key = "Drive/MeasuredVelocity")
   public double chassisSpeedsMagnitude() {
     return ChassisSpeedHelper.magnitude(getChassisSpeeds());
   }

src/main/java/frc/robot/subsystems/drive/weights/CStationAlignWeight.java

@@ -0,0 +1,79 @@
+package frc.robot.subsystems.drive.weights;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.sensors.gyro.Gyro;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.drive.commands.DriveCommandFactory;
+import frc.robot.util.helpers.AutoAlignHelper;
+import frc.robot.util.misc.DistanceManager;
+import java.util.function.Function;
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+public class CStationAlignWeight implements DriveWeight {
+  private Supplier<Pose2d[]> positions;
+  private Function<Pose2d, Pose2d> poseFunction;
+  private Supplier<Pose2d> robotPose;
+  private Pose2d targetPose;
+  private AutoAlignHelper autoAlignHelper;
+  private DriveSubsystem driveSubsystem;
+  private DriveCommandFactory driveCommandFactory;
+  private Gyro gyro;
+
+  public CStationAlignWeight(
+      Supplier<Pose2d[]> positions,
+      Function<Pose2d, Pose2d> poseFunction,
+      Supplier<Pose2d> robotPose,
+      DriveSubsystem driveSubsystem,
+      DriveCommandFactory driveCommandFactory,
+      Gyro gyro) {
+    this.positions = positions;
+    this.robotPose = robotPose;
+    this.targetPose = new Pose2d();
+    this.autoAlignHelper = new AutoAlignHelper();
+    this.driveSubsystem = driveSubsystem;
+    this.driveCommandFactory = driveCommandFactory;
+    this.gyro = gyro;
+  }
+
+  @Override
+  public ChassisSpeeds getSpeeds() {
+    targetPose = findNearest(positions.get());
+    ChassisSpeeds speeds = autoAlignHelper.getCStationAlignSpeedsLine(
+      robotPose.get(), targetPose, gyro, driveSubsystem);
+    Logger.recordOutput("cStationAlign/speeds", speeds);
+    return speeds;
+  }
+
+  private Pose2d findNearest(Pose2d[] pos) {
+    if (poseFunction != null) {
+      return DistanceManager.getNearestPosition(robotPose.get(), pos, poseFunction);
+    }
+    return DistanceManager.getNearestPosition(robotPose.get(), pos);
+  }
+
+  @Override
+  public void onStart() {
+    autoAlignHelper.resetCStationMotionProfile(robotPose.get(), driveSubsystem);
+  }
+
+  public boolean isAutoalignComplete() {
+    boolean complete =
+        (robotPose.get().getTranslation().getDistance(targetPose.getTranslation()) < 0.15
+            && Math.abs(robotPose.get().getRotation().minus(targetPose.getRotation()).getDegrees())
+                < 6);
+    ChassisSpeeds chassisSpeeds = driveSubsystem.getChassisSpeeds();
+    complete &= Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) < 0.01;
+    return complete;
+  }
+
+  public Command getAutoCommand() {
+    return driveCommandFactory
+        .runVelocityCommand(() -> getSpeeds(), () -> true)
+        .finallyDo(
+            () -> driveCommandFactory.runVelocityCommand(() -> new ChassisSpeeds(), () -> true));
+  }
+}

src/main/java/frc/robot/util/helpers/AutoAlignHelper.java

@@ -18,6 +18,7 @@
 public class AutoAlignHelper {
   PIDController linearDrivePID = RobotPIDConstants.constructPID(RobotPIDConstants.linearDrivePID);
   SlewRateLimiter accel = new SlewRateLimiter(3);
+  SlewRateLimiter cStationAccel = new SlewRateLimiter(3);
   PIDController rotatePID =
       RobotPIDConstants.constructPID(RobotPIDConstants.rotateToAnglePIDRadians);
 
@@ -36,6 +37,18 @@ public class AutoAlignHelper {
           AutoAlignConfig.localAlignPpidConstraints,
           "LocalAlignPPID");
 
+  private PIDController cStationDrivePid =
+      RobotPIDConstants.constructPID(RobotPIDConstants.cStationDrivePID);
+  // private PIDController cStationYLinearDrivePid =
+  //     RobotPIDConstants.constructPID(RobotPIDConstants.cStationTagAlignY);
+  private PIDController cStationRotationPid =
+      RobotPIDConstants.constructPID(RobotPIDConstants.cStationAnglePid);
+  private ProfiledPIDController cStationDriveProfiledPid =
+      RobotPIDConstants.constructProfiledPIDController(
+          RobotPIDConstants.cStationDrivePPID,
+          AutoAlignConfig.cStationPpidConstraints,
+          "cStationAlignPPID");
+
   public ChassisSpeeds getPoseSpeedsLine(Pose2d robotPose, Pose2d targetPose, Gyro gyro) {
     Pose2d robot = robotPose;
     Pose2d target = targetPose;
@@ -129,7 +142,7 @@ public ChassisSpeeds getLocalAlignSpeedsLine(
 
   public void resetLocalMotionProfile(Translation2d vector, DriveSubsystem driveSubsystem) {
     ChassisSpeeds speeds = driveSubsystem.getChassisSpeeds();
-    System.out.println(speeds);
+    // System.out.println(speeds); // goofy stuff
     Translation2d velocity = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
     double parallelVel =
         -driveSubsystem.chassisSpeedsMagnitude()
@@ -158,4 +171,54 @@ public ChassisSpeeds getLocalAlignSpeedsLine(
     // convert to field-centric
     return convertToFieldRelative(new ChassisSpeeds(x, y, rot), robotRotation);
   } */
+
+  public ChassisSpeeds getCStationAlignSpeedsLine(
+      Pose2d robotPose, Pose2d targetPose, Gyro gyro, DriveSubsystem driveSubsystem) {
+    cStationRotationPid.enableContinuousInput(-Math.PI, Math.PI);
+
+    Pose2d robot = robotPose;
+    Pose2d target = targetPose;
+    Logger.recordOutput("cStationAlign/targetPos", target);
+    // take measurements
+    double dist = robot.getTranslation().getDistance(target.getTranslation());
+    Rotation2d angleToTarget = target.getTranslation().minus(robot.getTranslation()).getAngle();
+    // calculate output
+
+    Logger.recordOutput("cStationaligndist", dist);
+    double linear =
+        dist < DriveConstants.cStationSlowAlignDistance
+            ? cStationDrivePid.calculate(dist, 0)
+            : cStationDriveProfiledPid.calculate(dist, 0);
+    Logger.recordOutput(
+        "cStationAlign/profiledcStationAlign", dist > DriveConstants.cStationSlowAlignDistance);
+    double rotational =
+        cStationRotationPid.calculate(
+            robot.getRotation().minus(target.getRotation()).getRadians(), 0);
+    // convert to percentage
+    linear = MathUtil.clamp(linear, -1, 1);
+    linear = MathUtil.applyDeadband(linear, 0.01);
+    linear *= DriveConstants.maxSpeed; // TODO scale max speed for acceleration?
+
+    rotational = MathUtil.clamp(rotational, -1, 1);
+    rotational = MathUtil.applyDeadband(rotational, 0.01);
+    rotational *= DriveConstants.maxOmega;
+    // limit rate
+    // linear = cStationAccel.calculate(linear);
+    // find component vectors
+    double vx = -linear * angleToTarget.getCos();
+    double vy = -linear * angleToTarget.getSin();
+    // convert chassis speeds
+    ChassisSpeeds robotRelative = new ChassisSpeeds(vx, vy, rotational);
+    return convertToFieldRelative(robotRelative, gyro, robot);
+  }
+
+  public void resetCStationMotionProfile(Pose2d robot, DriveSubsystem driveSubsystem) {
+    ChassisSpeeds speeds = driveSubsystem.getChassisSpeeds();
+    // System.out.println(speeds); // goofy stuff
+    Translation2d velocity = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+    double parallelVel =
+        -driveSubsystem.chassisSpeedsMagnitude()
+            * (velocity.getAngle().minus(robot.getRotation())).getCos();
+    cStationDriveProfiledPid.reset(robot.getRotation().getRadians(), parallelVel);
+  }
 }