diff --git a/src/main/java/org/curtinfrc/frc2025/BuildConstants.java b/src/main/java/org/curtinfrc/frc2025/BuildConstants.java index 68a3772a..36ea2473 100644 --- a/src/main/java/org/curtinfrc/frc2025/BuildConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/BuildConstants.java @@ -5,12 +5,12 @@ 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 = 44; - public static final String GIT_SHA = "7a9d09bac5cba210546af5956c74dd217ac9767e"; - public static final String GIT_DATE = "2025-01-16 17:13:41 AWST"; - public static final String GIT_BRANCH = "paul/predictive-control"; - public static final String BUILD_DATE = "2025-01-23 17:47:15 AWST"; - public static final long BUILD_UNIX_TIME = 1737625635094L; + public static final int GIT_REVISION = 46; + public static final String GIT_SHA = "d84b4423208cba8a340d370cdf9ca54fcb9dcd31"; + public static final String GIT_DATE = "2025-01-23 17:58:03 AWST"; + public static final String GIT_BRANCH = "paul/mpc"; + public static final String BUILD_DATE = "2025-01-24 10:43:50 AWST"; + public static final long BUILD_UNIX_TIME = 1737686630605L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index f939641d..a201ef3e 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/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 @@ -75,9 +76,9 @@ public static enum Mode { public static 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 _tagIdsBlue; @@ -104,20 +105,84 @@ public Pose3d toPose(Pose3d currentPose) { private Pose3d resolvePose(List tagIds, Pose3d currentPose) { if (tagIds.isEmpty()) return new Pose3d(); - ArrayList poses = new ArrayList<>(); + double sideOffset = Math.max(ROBOT_X / 1000.0, ROBOT_Y / 1000.0) / 2.0; + + final class Closest { + Pose3d pose = null; + double distance = Double.MAX_VALUE; + } + + Closest closest = new Closest(); + 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 = /*normalizeAngle(*/rotation.getAngle()/*)*/; + + double cosHeading = Math.cos(baseAngle); + double sinHeading = Math.sin(baseAngle); + + double adjustedXLeft, adjustedYLeft, adjustedXRight, adjustedYRight; + + adjustedXLeft = mappedPose.getX() + sideOffset * sinHeading; + adjustedYLeft = mappedPose.getY() - sideOffset * cosHeading; + + adjustedXRight = mappedPose.getX() - sideOffset * sinHeading; + adjustedYRight = mappedPose.getY() + sideOffset * cosHeading; + + Pose3d leftPose = + new Pose3d(adjustedXLeft, adjustedYLeft, mappedPose.getZ(), rotation); + + Pose3d rightPose = + new Pose3d(adjustedXRight, adjustedYRight, mappedPose.getZ(), rotation); + + double leftDistance = distanceBetween(leftPose, currentPose); + if (leftDistance < closest.distance) { + closest.pose = leftPose; + closest.distance = leftDistance; + } + + double rightDistance = distanceBetween(rightPose, currentPose); + if (rightDistance < closest.distance) { + closest.pose = rightPose; + closest.distance = rightDistance; + } + + String tagPrefix = String.format("resolvePose.tagId_%d", tagId); + Logger.recordOutput(tagPrefix + "/baseAngle", String.format("%.2f", baseAngle)); + Logger.recordOutput( + tagPrefix + "/leftPose", + String.format( + "(%.2f, %.2f, %.2f)", adjustedXLeft, adjustedYLeft, mappedPose.getZ())); + Logger.recordOutput( + tagPrefix + "/rightPose", + String.format( + "(%.2f, %.2f, %.2f)", adjustedXRight, adjustedYRight, mappedPose.getZ())); + Logger.recordOutput( + tagPrefix + "/currentPose", + String.format( + "(%.2f, %.2f, %.2f)", + currentPose.getX(), currentPose.getY(), currentPose.getZ())); + Logger.recordOutput( + tagPrefix + "/tagPose", + String.format( + "(%.2f, %.2f, %.2f)", + tagPose.getX(), tagPose.getY(), tagPose.getZ())); + }); } - 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 double normalizeAngle(double angle) { + while (angle > Math.PI) angle -= 2 * Math.PI; + while (angle < -Math.PI) angle += 2 * Math.PI; + return angle; } private double distanceBetween(Pose3d pose1, Pose3d pose2) { diff --git a/src/main/java/org/curtinfrc/frc2025/Superstructure.java b/src/main/java/org/curtinfrc/frc2025/Superstructure.java index 3596f048..8bd34162 100644 --- a/src/main/java/org/curtinfrc/frc2025/Superstructure.java +++ b/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()); } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java index 28c0e056..2abdd7ef 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java @@ -26,4 +26,8 @@ public Command goToSetpoint(Setpoints point) { public boolean isStable() { return io.isStable(); } + + public void stop() { + io.stop(); + } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java index 1708cffc..769a31a6 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java @@ -4,7 +4,7 @@ public class ElevatorConstants { // 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 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) @@ -14,7 +14,7 @@ public class ElevatorConstants { // 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 maxAccel = 38197; // Max acceleration in RPM per second public static double allowedErr = 0.002; // Allowable position error for stability // Model Predictive Control Gains diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java index e30e25da..9fd22a50 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java @@ -32,4 +32,6 @@ public default void reset() {} public default boolean isStable() { return false; } + + public default void stop() {} } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMPC.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMPC.java index 6b6e71fc..c9d99c3a 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMPC.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMPC.java @@ -1,11 +1,10 @@ package org.curtinfrc.frc2025.subsystems.elevator; -import edu.wpi.first.math.system.plant.DCMotor; import org.curtinfrc.frc2025.Constants.Setpoints; import org.curtinfrc.frc2025.util.Util; public class ElevatorIONeoMPC extends ElevatorIONeo { - private final double controlLoopPeriod = 0.02; + private final double controlLoopPeriod = 0.02; private Setpoints set = Setpoints.COLLECT; private double computedVoltage = 0.0; @@ -15,17 +14,16 @@ public class ElevatorIONeoMPC extends ElevatorIONeo { @Override public void updateInputs(ElevatorIOInputs inputs) { - inputs.distanceSensorReading = 0.0; - inputs.encoderReading = elevatorEncoder.getPosition(); - inputs.point = set; - inputs.pointRot = - ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); + inputs.distanceSensorReading = 0.0; + inputs.encoderReading = elevatorEncoder.getPosition(); + inputs.point = set; + inputs.pointRot = ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); inputs.motorVoltage = computedVoltage; - inputs.motorCurrent = elevatorMotor.getOutputCurrent(); - inputs.motorTemp = elevatorMotor.getMotorTemperature(); - inputs.motorVelocity = elevatorEncoder.getVelocity() / 60.0; + inputs.motorCurrent = elevatorMotor.getOutputCurrent(); + inputs.motorTemp = elevatorMotor.getMotorTemperature(); + inputs.motorVelocity = elevatorEncoder.getVelocity() / 60.0; inputs.positionError = Math.abs( @@ -36,10 +34,8 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.predictionHorizon = predictionHorizon; - inputs.predictedPosition = - elevatorEncoder.getPosition(); - inputs.predictedVelocity = - elevatorEncoder.getVelocity() / 60.0; + inputs.predictedPosition = elevatorEncoder.getPosition(); + inputs.predictedVelocity = elevatorEncoder.getVelocity() / 60.0; inputs.velocityError = velocityError; } @@ -50,7 +46,7 @@ public void goToSetpoint(Setpoints point) { double targetPosition = ElevatorIONeoMaxMotion.convertSetpoint(point.elevatorSetpoint()); double currentPosition = elevatorEncoder.getPosition(); - double velocity = elevatorEncoder.getVelocity() / 60.0; + double velocity = elevatorEncoder.getVelocity() / 60.0; adjustPredictionHorizon(currentPosition, targetPosition); generateReferenceTrajectory(targetPosition); @@ -65,7 +61,7 @@ public void goToSetpoint(Setpoints point) { // reduceVoltageNearSetpoint(computedVoltage, Math.abs(targetPosition - currentPosition)); if (Math.abs(targetPosition - currentPosition) < ElevatorConstants.positionTolerance) { - computedVoltage = 0; + computedVoltage = 0; } elevatorMotor.setVoltage(computedVoltage); @@ -89,8 +85,7 @@ private void generateReferenceTrajectory(double targetPosition) { for (int i = 0; i < predictionHorizon; i++) { double factor = (double) i / (predictionHorizon - 1); - referenceTrajectory[i] = - currentPosition + distance * Math.pow(factor, 2); + referenceTrajectory[i] = currentPosition + distance * Math.pow(factor, 2); } } @@ -120,7 +115,7 @@ private double computeMPCControl(double currentPosition, double velocity) { if (Math.abs(positionError) < ElevatorConstants.positionTolerance) { stepVoltage *= Math.abs(positionError) / ElevatorConstants.positionTolerance; if (Math.abs(stepVoltage) < 0.1) { - stepVoltage = 0; + stepVoltage = 0; } } @@ -136,7 +131,7 @@ private double computeMPCControl(double currentPosition, double velocity) { if (Math.abs(predictedPosition[i] - referenceTrajectory[i]) < ElevatorConstants.positionTolerance) { - predictedVelocity[i] = 0; + predictedVelocity[i] = 0; } totalVoltage = stepVoltage; @@ -164,21 +159,26 @@ private boolean isWithinDeadband(double positionError, double velocity) { private double clampVoltageForDeadband(double voltage, double positionError, double velocity) { if (isWithinDeadband(positionError, velocity)) { - return 0.0; + return 0.0; } return voltage; } private double reduceVoltageNearSetpoint(double voltage, double positionError) { if (positionError < ElevatorConstants.positionTolerance) { - return 0.0; + return 0.0; } double scalingFactor = Math.max(0.1, 1.0 - Math.abs(positionError) / (2 * ElevatorConstants.positionTolerance)); - return voltage * scalingFactor; + return voltage * scalingFactor; } private double clampVoltage(double voltage) { return Math.max(ElevatorConstants.kMinOutput, Math.min(ElevatorConstants.kMaxOutput, voltage)); } + + @Override + public void stop() { + elevatorMotor.setVoltage(0); + } } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSimMPC.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSimMPC.java index a4be9703..0aae2b9b 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSimMPC.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSimMPC.java @@ -9,7 +9,7 @@ public class ElevatorIOSimMPC implements ElevatorIO { private final DCMotor motor = DCMotor.getNEO(ElevatorConstants.motorCount); private final DCMotorSim elevatorSim; - private final double controlLoopPeriod = 0.02; + private final double controlLoopPeriod = 0.02; private Setpoints set = Setpoints.COLLECT; private double computedVoltage = 0.0; @@ -25,16 +25,15 @@ public ElevatorIOSimMPC() { public void updateInputs(ElevatorIOInputs inputs) { elevatorSim.update(controlLoopPeriod); - inputs.distanceSensorReading = 0.0; - inputs.encoderReading = elevatorSim.getAngularPositionRotations(); - inputs.point = set; - inputs.pointRot = - ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); + inputs.distanceSensorReading = 0.0; + inputs.encoderReading = elevatorSim.getAngularPositionRotations(); + inputs.point = set; + inputs.pointRot = ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); - inputs.motorVoltage = computedVoltage; + inputs.motorVoltage = elevatorSim.getInputVoltage(); - inputs.motorCurrent = elevatorSim.getCurrentDrawAmps(); - inputs.motorTemp = 25.0; + inputs.motorCurrent = elevatorSim.getCurrentDrawAmps(); + inputs.motorTemp = 25.0; inputs.motorVelocity = elevatorSim.getAngularVelocityRPM() / 60.0; // Convert RPM to RPS inputs.positionError = @@ -46,10 +45,8 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.predictionHorizon = predictionHorizon; - inputs.predictedPosition = - elevatorSim.getAngularPositionRotations(); - inputs.predictedVelocity = - elevatorSim.getAngularVelocityRPM() / 60.0; + inputs.predictedPosition = elevatorSim.getAngularPositionRotations(); + inputs.predictedVelocity = elevatorSim.getAngularVelocityRPM() / 60.0; inputs.velocityError = velocityError; } @@ -75,7 +72,7 @@ public void goToSetpoint(Setpoints point) { // reduceVoltageNearSetpoint(computedVoltage, Math.abs(targetPosition - currentPosition)); if (Math.abs(targetPosition - currentPosition) < ElevatorConstants.positionTolerance) { - computedVoltage = 0; + computedVoltage = 0; } elevatorSim.setInputVoltage(computedVoltage); @@ -99,8 +96,7 @@ private void generateReferenceTrajectory(double targetPosition) { for (int i = 0; i < predictionHorizon; i++) { double factor = (double) i / (predictionHorizon - 1); - referenceTrajectory[i] = - currentPosition + distance * Math.pow(factor, 2); + referenceTrajectory[i] = currentPosition + distance * Math.pow(factor, 2); } } @@ -130,7 +126,7 @@ private double computeMPCControl(double currentPosition, double velocity) { if (Math.abs(positionError) < ElevatorConstants.positionTolerance) { stepVoltage *= Math.abs(positionError) / ElevatorConstants.positionTolerance; if (Math.abs(stepVoltage) < 0.1) { - stepVoltage = 0; + stepVoltage = 0; } } @@ -146,7 +142,7 @@ private double computeMPCControl(double currentPosition, double velocity) { if (Math.abs(predictedPosition[i] - referenceTrajectory[i]) < ElevatorConstants.positionTolerance) { - predictedVelocity[i] = 0; + predictedVelocity[i] = 0; } totalVoltage = stepVoltage; @@ -174,21 +170,26 @@ private boolean isWithinDeadband(double positionError, double velocity) { private double clampVoltageForDeadband(double voltage, double positionError, double velocity) { if (isWithinDeadband(positionError, velocity)) { - return 0.0; + return 0.0; } return voltage; } private double reduceVoltageNearSetpoint(double voltage, double positionError) { if (positionError < ElevatorConstants.positionTolerance) { - return 0.0; + return 0.0; } double scalingFactor = Math.max(0.1, 1.0 - Math.abs(positionError) / (2 * ElevatorConstants.positionTolerance)); - return voltage * scalingFactor; + return voltage * scalingFactor; } private double clampVoltage(double voltage) { return Math.max(ElevatorConstants.kMinOutput, Math.min(ElevatorConstants.kMaxOutput, voltage)); } + + @Override + public void stop() { + elevatorSim.setInputVoltage(0); + } } diff --git a/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java b/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java index 04f32076..b8b7b8f0 100644 --- a/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java +++ b/src/main/java/org/curtinfrc/frc2025/util/PoseUtil.java @@ -6,12 +6,13 @@ import org.curtinfrc.frc2025.Constants; public final class PoseUtil { - public static Pose3d mapPose(Pose2d pose) { - double angle = pose.getRotation().getRadians(); + public static Pose3d mapPose(Pose3d pose) { + double angle = pose.getRotation().getAngle(); return new Pose3d( pose.getX() + Math.cos(angle) * Constants.ROBOT_X / 2000.0, pose.getY() + Math.sin(angle) * Constants.ROBOT_Y / 2000.0, 0.0, - new Rotation3d(0.0, 0.0, angle)); + pose.getRotation() + ); } }