Skip to content

Commit

Permalink
did more for setpoints
Browse files Browse the repository at this point in the history
  • Loading branch information
@Superbro525Alt
Superbro525Alt committed Jan 24, 2025
1 parent d84b442 commit 8192b81
Show file tree
Hide file tree
Showing 9 changed files with 143 additions and 70 deletions.
12 changes: 6 additions & 6 deletions src/main/java/org/curtinfrc/frc2025/BuildConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {}
Expand Down
93 changes: 79 additions & 14 deletions src/main/java/org/curtinfrc/frc2025/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<Integer> _tagIdsBlue;
Expand All @@ -104,20 +105,84 @@ 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 / 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) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/curtinfrc/frc2025/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}
4 changes: 4 additions & 0 deletions src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,4 +26,8 @@ public Command goToSetpoint(Setpoints point) {
public boolean isStable() {
return io.isStable();
}

public void stop() {
io.stop();
}
}
4 changes: 2 additions & 2 deletions src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down
2 changes: 2 additions & 0 deletions src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,4 +32,6 @@ public default void reset() {}
public default boolean isStable() {
return false;
}

public default void stop() {}
}
46 changes: 23 additions & 23 deletions src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMPC.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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(
Expand All @@ -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;
}
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
}
}

Expand Down Expand Up @@ -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;
}
}

Expand All @@ -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;
Expand Down Expand Up @@ -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);
}
}
Loading

0 comments on commit 8192b81

Please sign in to comment.