Paul/mpc

src/main/java/org/curtinfrc/frc2025/BuildConstants.java

@@ -0,0 +1,17 @@
+package org.curtinfrc.frc2025;
+
+/** Automatically generated file containing build version information. */
+public final class BuildConstants {
+  public static final String MAVEN_GROUP = "";
+  public static final String MAVEN_NAME = "2025-Reefscape";
+  public static final String VERSION = "unspecified";
+  public static final int GIT_REVISION = 45;
+  public static final String GIT_SHA = "9fc4b9afa35ec7f3eaf252c12769966fc21bd42f";
+  public static final String GIT_DATE = "2025-01-24 12:21:39 AWST";
+  public static final String GIT_BRANCH = "paul/mpc";
+  public static final String BUILD_DATE = "2025-01-24 12:22:08 AWST";
+  public static final long BUILD_UNIX_TIME = 1737692528741L;
+  public static final int DIRTY = 0;
+
+  private BuildConstants() {}
+}

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -16,13 +16,14 @@
 import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.aprilTagLayout;
 
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
-import java.util.ArrayList;
 import java.util.List;
 import org.curtinfrc.frc2025.util.PoseUtil;
+import org.littletonrobotics.junction.Logger;
 
 /**
  * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
@@ -83,9 +84,9 @@ public static void main(String... args) {
   public enum Setpoints {
     /* in mm */
     COLLECT(950, List.of(13, 12), List.of(1, 2)),
-    L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)),
-    L2(810, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)),
-    L3(1210, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6));
+    L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 7, 10, 6, 11)),
+    L2(810, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 7, 10, 6, 11)),
+    L3(1210, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 7, 10, 6, 11));
 
     private final int _elevatorSetpoint;
     private final List<Integer> _tagIdsBlue;
@@ -112,20 +113,74 @@ public Pose3d toPose(Pose3d currentPose) {
     private Pose3d resolvePose(List<Integer> tagIds, Pose3d currentPose) {
       if (tagIds.isEmpty()) return new Pose3d();
 
-      ArrayList<Pose3d> poses = new ArrayList<>();
+      double sideOffset = Math.max(ROBOT_X, ROBOT_Y) / 2000.0;
+
+      class ClosestPose {
+        Pose3d pose = null;
+        double distance = Double.MAX_VALUE;
+      }
+
+      ClosestPose closest = new ClosestPose();
+
       for (int tagId : tagIds) {
         aprilTagLayout
             .getTagPose(tagId)
-            .ifPresent(pose -> poses.add(PoseUtil.mapPose(pose.toPose2d())));
+            .ifPresent(
+                tagPose -> {
+                  Pose3d mappedPose = PoseUtil.mapPose(tagPose);
+                  Rotation3d rotation = mappedPose.getRotation();
+                  double baseAngle = rotation.getAngle(),
+                      cos = Math.cos(baseAngle),
+                      sin = Math.sin(baseAngle);
+                  double xOffset = sideOffset * sin, yOffset = sideOffset * cos;
+
+                  for (Pose3d pose :
+                      new Pose3d[] {
+                        new Pose3d(
+                            mappedPose.getX() + xOffset,
+                            mappedPose.getY() - yOffset,
+                            mappedPose.getZ(),
+                            rotation),
+                        new Pose3d(
+                            mappedPose.getX() - xOffset,
+                            mappedPose.getY() + yOffset,
+                            mappedPose.getZ(),
+                            rotation)
+                      }) {
+                    double distance = distanceBetween(pose, currentPose);
+                    if (distance < closest.distance) {
+                      closest.pose = pose;
+                      closest.distance = distance;
+                    }
+                  }
+
+                  String tagPrefix = String.format("resolvePose.tagId_%d", tagId);
+                  Logger.recordOutput(tagPrefix + "/baseAngle", String.format("%.2f", baseAngle));
+                  Logger.recordOutput(
+                      tagPrefix + "/leftPose",
+                      formatPose(
+                          new Pose3d(
+                              mappedPose.getX() + xOffset,
+                              mappedPose.getY() - yOffset,
+                              mappedPose.getZ(),
+                              rotation)));
+                  Logger.recordOutput(
+                      tagPrefix + "/rightPose",
+                      formatPose(
+                          new Pose3d(
+                              mappedPose.getX() - xOffset,
+                              mappedPose.getY() + yOffset,
+                              mappedPose.getZ(),
+                              rotation)));
+                  Logger.recordOutput(tagPrefix + "/currentPose", formatPose(currentPose));
+                  Logger.recordOutput(tagPrefix + "/tagPose", formatPose(tagPose));
+                });
       }
-      if (poses.isEmpty()) return new Pose3d();
-
-      return poses.stream()
-          .min(
-              (pose1, pose2) ->
-                  Double.compare(
-                      distanceBetween(pose1, currentPose), distanceBetween(pose2, currentPose)))
-          .orElse(new Pose3d());
+      return closest.pose != null ? closest.pose : new Pose3d();
+    }
+
+    private String formatPose(Pose3d pose) {
+      return String.format("(%.2f, %.2f, %.2f)", pose.getX(), pose.getY(), pose.getZ());
     }
 
     private double distanceBetween(Pose3d pose1, Pose3d pose2) {

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -38,8 +38,8 @@
 import org.curtinfrc.frc2025.subsystems.drive.ModuleIOTalonFX;
 import org.curtinfrc.frc2025.subsystems.elevator.Elevator;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIO;
-import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIONeoMaxMotionLaserCAN;
-import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIOSim;
+import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIONeoMPC;
+import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIOSimMPC;
 import org.curtinfrc.frc2025.subsystems.vision.Vision;
 import org.curtinfrc.frc2025.subsystems.vision.VisionIO;
 import org.curtinfrc.frc2025.subsystems.vision.VisionIOLimelight;
@@ -142,8 +142,7 @@ public Robot() {
                   new VisionIOLimelightGamepiece(camera0Name),
                   new VisionIOLimelight(camera1Name, drive::getRotation),
                   new VisionIOQuestNav());
-          // elevator = new Elevator(new ElevatorIONeoMaxMotionLaserCAN());
-          elevator = new Elevator(new ElevatorIO() {});
+          elevator = new Elevator(new ElevatorIONeoMPC());
         }
 
         case DEVBOT -> {
@@ -161,7 +160,7 @@ public Robot() {
                   new VisionIOLimelightGamepiece(camera0Name),
                   new VisionIOLimelight(camera1Name, drive::getRotation),
                   new VisionIOQuestNav());
-          elevator = new Elevator(new ElevatorIONeoMaxMotionLaserCAN());
+          elevator = new Elevator(new ElevatorIONeoMPC());
         }
 
         case SIMBOT -> {
@@ -180,7 +179,7 @@ public Robot() {
                   new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose),
                   new VisionIO() {});
 
-          elevator = new Elevator(new ElevatorIOSim());
+          elevator = new Elevator(new ElevatorIOSimMPC());
         }
       }
     } else {
@@ -270,6 +269,8 @@ public Robot() {
 
     // controller.x().whileTrue(drive.autoAlign(drive.findClosestTag(aprilTagLayout.getTags())));
 
+    // controller.x().whileTrue(drive.autoAlign(drive.findClosestTag(aprilTagLayout.getTags())));
+
     // Reset gyro to 0° when B button is pressed
     controller
         .y()

src/main/java/org/curtinfrc/frc2025/Superstructure.java

@@ -22,7 +22,7 @@ public Command align(Setpoints setpoint) {
         () ->
             Commands.parallel(
                 m_drivebase.autoAlign(setpoint.toPose(new Pose3d(m_drivebase.getPose()))),
-                m_elevator.goToSetpoint(setpoint)),
+                m_elevator.goToSetpoint(setpoint).finallyDo(() -> m_elevator.stop())),
         Set.of());
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java

@@ -167,11 +167,11 @@ public void periodic() {
       }
 
       // Update gyro angle
-      if (gyroInputs.connected) {
+      if (gyroInputs.connected && i >= 0 && i < gyroInputs.odometryYawPositions.length) {
         // Use the real gyro angle
         rawGyroRotation = gyroInputs.odometryYawPositions[i];
       } else {
-        // Use the angle delta from the kinematics and module deltas
+        // Fallback to calculating the angle delta
         Twist2d twist = kinematics.toTwist2d(moduleDeltas);
         rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
       }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java

@@ -22,4 +22,12 @@ public void periodic() {
   public Command goToSetpoint(Setpoints point) {
     return run(() -> io.goToSetpoint(point)).until(() -> io.isStable());
   }
+
+  public boolean isStable() {
+    return io.isStable();
+  }
+
+  public void stop() {
+    io.stop();
+  }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java

@@ -1,20 +1,28 @@
 package org.curtinfrc.frc2025.subsystems.elevator;
 
 public class ElevatorConstants {
-  public static int motorCount = 2;
-  public static int distanceSensorPort = 99;
-  public static int motorPort = 99;
-  public static double tolerance = 0.01;
+  // reduction for elevator neo is 10.7 : 1
+  // TODO: GET VALUES FOR SOME OF THESE
+  public static int motorCount = 1; // Number of motors in the elevator system
+  public static int distanceSensorPort = 0; // Port for distance sensor (if used)
+  public static int motorPort = 1; // Motor controller port
+  public static double tolerance = 0.01; // Velocity tolerance (rotations per second)
+  public static double positionTolerance = 0.002; // Position tolerance (rotations)
+  public static double friction = 0.05; // Estimated system friction (Nm) TODO
+  public static double stableVelocityThreshold =
+      0.005; // Threshold for velocity to be considered stable
 
-  public static double maxVel = 5676;
-  public static double maxAccel = 0;
-  public static double allowedErr = 0;
+  // Motion profile constraints
+  public static double maxVel = 5676 / 10.7; // Max velocity in RPM
+  public static double maxAccel = 38197; // Max acceleration in RPM per second
+  public static double allowedErr = 0.002; // Allowable position error for stability
 
-  // TODO: TUNE PID
-  public static double kP = 1;
-  public static double kI = 0;
-  public static double kD = 0;
-  public static double kMinOutput = 0;
-  public static double kMaxOutput = 0;
-  public static double kV = 473;
+  // Model Predictive Control Gains
+  public static double kP = 6; // Proportional gain TODO
+  public static double kI = 0.01; // Integral gain TODO
+  public static double kD = 0.1; // Derivative gain TODO
+  public static double kA = 0.3;
+  public static double kMinOutput = -12.0; // Minimum voltage output
+  public static double kMaxOutput = 12.0; // Maximum voltage output
+  public static double kV = 473; // Voltage constant (i for it from the datasheet)
 }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java

@@ -10,21 +10,28 @@ public static class ElevatorIOInputs {
     public double distanceSensorReading;
     public double encoderReading;
     public Setpoints point;
+    public double pointRot;
     public double motorVoltage;
     public double motorCurrent;
     public double motorTemp;
     public double motorVelocity;
+    public double positionError;
+    public double velocityError;
+    public boolean stable;
+    public int predictionHorizon;
+    public double predictedPosition;
+    public double predictedVelocity;
   }
 
   public default void goToSetpoint(Setpoints point) {}
 
-  public default void applyVoltage(double volts) {}
-
   public default void updateInputs(ElevatorIOInputs inputs) {}
 
   public default void reset() {}
 
   public default boolean isStable() {
     return false;
   }
+
+  public default void stop() {}
 }