CANMappings + Drivetrain Logging and SOTM

Author: sophiep29

src/main/java/frc/robot/commands/DrivetrainCommand.java

@@ -5,11 +5,16 @@
 import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.config.TunerConstants;
 import frc.robot.helpers.ApplyModuleStates;
+import frc.robot.helpers.ShotCalculator;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import java.util.function.DoubleSupplier;
 
@@ -18,14 +23,16 @@ public class DrivetrainCommand extends Command {
   public static enum Position {
     TELEOP,
     STILL_SHOT,
-    SOTM
+    SOTM,
+    AUTO_ALIGN_HUB
   }
 
   private CommandSwerveDrivetrain subsystem;
   private DrivetrainCommand.Position pose;
   private DoubleSupplier leftY;
   private DoubleSupplier leftX;
   private DoubleSupplier rightX;
+  public static String drivetrainState = "Stopped";
 
   private final double MaxSpeed =
       TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -48,6 +55,9 @@ public static enum Position {
   };
   private final ApplyModuleStates applyRequest = new ApplyModuleStates();
   private final SwerveRequest.SwerveDriveBrake brakeRequest = new SwerveRequest.SwerveDriveBrake();
+  private final ProfiledPIDController autoAlignPidController;
+  private Rotation2d m_targetAngle = Rotation2d.kZero;
+  private static final double FUEL_EXIT_VELOCITY = 10.0; // This needs to be tuned
 
   public DrivetrainCommand(
       CommandSwerveDrivetrain subsystem,
@@ -60,10 +70,38 @@ public DrivetrainCommand(
     this.leftX = leftX;
     this.leftY = leftY;
     this.rightX = rightX;
+    double alignP = 85;
+    double alignI = 0;
+    double alignD = 0;
+    this.autoAlignPidController =
+        new ProfiledPIDController(
+            alignP,
+            alignI,
+            alignD,
+            new TrapezoidProfile.Constraints(MaxAngularRate, MaxAngularRate / 0.2));
+    this.autoAlignPidController.enableContinuousInput(
+        -Math.PI, Math.PI); // Swerve angles are continuous (-180 to 180 deg)
+    // Don't compute hub-facing target in the constructor (DriverStation alliance may be
+    // unavailable during initialization). Initialize to current heading; execute() will
+    // recompute the actual hub-facing target each loop.
+    this.m_targetAngle = subsystem.getState().Pose.getRotation();
 
     addRequirements(subsystem);
   }
 
+  public double getAutoAlignRotationalOutput() {
+    Rotation2d currentAngle = subsystem.getState().Pose.getRotation();
+    double current = currentAngle.getRadians();
+    double target = m_targetAngle.getRadians();
+    double output = autoAlignPidController.calculate(current, target);
+    if (output > MaxAngularRate) {
+      output = MaxAngularRate;
+    } else if (output < -MaxAngularRate) {
+      output = -MaxAngularRate;
+    }
+    return output;
+  }
+
   @Override
   public void execute() {
     switch (pose) {
@@ -78,20 +116,71 @@ public void execute() {
                 .withRotationalRate(
                     -rightX.getAsDouble()
                         * MaxAngularRate)); // Drive counterclockwise with negative X
+        drivetrainState = "Teleop Drive";
         break;
 
       // (Needs check) Mode for when shots are from a still position
       case STILL_SHOT:
         // make X with wheels, set wheels to brake mode
         applyRequest.ModuleStates = states;
         subsystem.setControl(applyRequest);
-        System.out.println("Drivetrain: Still Shot Configuration");
+        drivetrainState = "Still Shot";
         break;
 
       // (Incomplete) Mode for moving shots
       case SOTM:
-        // shoot on the move, reference Mechanical Advantage build log
-        System.out.println("Drivetrain: SOTM Drive");
+        drivetrainState = "Shoot on the Move Drive";
+        double vx = -leftY.getAsDouble() * MaxSpeed;
+        double vy = -leftX.getAsDouble() * MaxSpeed;
+        Translation2d robotPosition = subsystem.getState().Pose.getTranslation();
+        ChassisSpeeds speeds = subsystem.getState().Speeds;
+        Translation2d robotVelocity =
+            new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+        Translation2d goal = ShotCalculator.calculateHubPosition();
+        double distance = robotPosition.getDistance(goal);
+        double flightTime = distance / FUEL_EXIT_VELOCITY;
+        Translation2d toGoal = goal.minus(robotPosition);
+        Translation2d toGoalDir =
+            toGoal.getNorm() > 1e-6 ? toGoal.div(toGoal.getNorm()) : new Translation2d();
+        Translation2d lateralVelocity =
+            robotVelocity.minus(toGoalDir.times(robotVelocity.dot(toGoalDir)));
+        Translation2d virtualGoal = goal.minus(lateralVelocity.times(flightTime));
+        Rotation2d targetAngle = virtualGoal.minus(robotPosition).getAngle();
+        double currentHeading = subsystem.getState().Pose.getRotation().getRadians();
+        double targetHeading = targetAngle.getRadians();
+        double error = targetHeading - currentHeading;
+        error = Math.atan2(Math.sin(error), Math.cos(error));
+        double kP = 50; // tune
+        double omega = error * kP;
+        omega = Math.max(-MaxAngularRate, Math.min(MaxAngularRate, omega));
+        subsystem.setControl(drive.withVelocityX(vx).withVelocityY(vy).withRotationalRate(omega));
+
+        break;
+
+      case AUTO_ALIGN_HUB:
+        // Recompute the desired heading toward the hub each loop
+        m_targetAngle =
+            ShotCalculator.getRotationTowardsHub(
+                ShotCalculator.calculateHubPosition(), subsystem.getState().Pose.getTranslation());
+        double currentRad = subsystem.getState().Pose.getRotation().getRadians();
+        double targetRad = m_targetAngle.getRadians();
+        // Normalize error1 to [-pi, pi]
+        double error1 =
+            Math.atan2(Math.sin(targetRad - currentRad), Math.cos(targetRad - currentRad));
+        double absError = Math.abs(error1);
+        double angleTolerance = Math.toRadians(0.5); // stop within 1 degree
+
+        double rotOutput = getAutoAlignRotationalOutput();
+
+        if (absError < angleTolerance) {
+          // Aligned: stop rotating (and hold position)
+          subsystem.setControl(drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0));
+        } else {
+          // Not aligned: apply rotational output
+          subsystem.setControl(
+              drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput));
+        }
+        drivetrainState = "Auto Align";
         break;
 
       default:
@@ -104,6 +193,22 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return false;
+    if (pose != Position.AUTO_ALIGN_HUB) {
+      return false;
+    }
+
+    Translation2d hubPos = ShotCalculator.calculateHubPosition();
+
+    Translation2d currentTrans = subsystem.getState().Pose.getTranslation();
+    if (currentTrans == null) {
+      return false;
+    }
+
+    Rotation2d target = ShotCalculator.getRotationTowardsHub(hubPos, currentTrans);
+    double current = subsystem.getState().Pose.getRotation().getRadians();
+    double targetRad = target.getRadians();
+    double err = Math.atan2(Math.sin(targetRad - current), Math.cos(targetRad - current));
+    double angleTolerance = Math.toRadians(1.0); // 1 degree
+    return Math.abs(err) < angleTolerance;
   }
 }

src/main/java/frc/robot/config/CANMappings.java

@@ -8,8 +8,6 @@ public class CANMappings {
 
   public static final int INTAKE_MOTOR_ID = 3;
 
-  public static final int CLIMB_MOTOR_ID = 22; // unset
-
   public static final int FLYWHEEL_RIGHT_MOTOR_ID = 19;
   public static final int FLYWHEEL_LEFT_MOTOR_ID = 2;