Skip to content

Commit

Permalink
Fix Autoalign Jitter
Browse files Browse the repository at this point in the history
  • Loading branch information
@KPatel008
KPatel008 committed Mar 4, 2025
1 parent e4c97af commit 134d402
Showing 1 changed file with 48 additions and 59 deletions.
107 changes: 48 additions & 59 deletions src/main/java/frc/robot/subsystems/drivetrain/ReefBranchAlign.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,14 +31,17 @@ public class ReefBranchAlign extends Command {
private SwerveRequest.ApplyRobotSpeeds m_drive;
private Transform2d m_shift;
private DoubleSupplier m_joystickInput;
private Pose2d m_lastPose;


public ReefBranchAlign(CommandSwerveDrivetrain drivetrain, Transform2d shift, DoubleSupplier joystickXInput) {
m_shift = shift;
m_lastPose = new Pose2d();

m_joystickInput = joystickXInput;

m_translationController =
new ProfiledPIDController(4.0, 0.0, 0.1, new TrapezoidProfile.Constraints(2, 2));
new ProfiledPIDController(4.0, 0.0, 0.1, new TrapezoidProfile.Constraints(7, 7));
m_thetaController =
new ProfiledPIDController(
5.0, 0.0, 0.1, new TrapezoidProfile.Constraints(1 * Math.PI, 1 * Math.PI));
Expand All @@ -54,25 +57,16 @@ public ReefBranchAlign(CommandSwerveDrivetrain drivetrain, Transform2d shift, Do
@Override
public void initialize() {
m_goalPose = m_drivetrain.findClosestNode().transformBy(m_shift);

Pose2d initialPose = m_drivetrain.getState().Pose;

m_translationController.setTolerance(0.1);
m_thetaController.setTolerance(Units.degreesToRadians(3.0));
m_thetaController.enableContinuousInput(-Math.PI, Math.PI);
ChassisSpeeds fieldVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(m_drivetrain.getState().Speeds, initialPose.getRotation());

m_translationController.reset(
initialPose.getTranslation().getDistance(m_goalPose.getTranslation()),
Math.min(
0.0,
-new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond)
.rotateBy(
m_goalPose
.getTranslation()
.minus(initialPose.getTranslation())
.getAngle()
.unaryMinus())
.getX()));
Math.abs(initialPose.getTranslation().getY() - m_goalPose.getTranslation().getY()),
Math.min(0.0, m_drivetrain.getState().Speeds.vyMetersPerSecond));
m_thetaController.reset(
initialPose.getRotation().getRadians(),
fieldVelocity.omegaRadiansPerSecond);
Expand All @@ -82,60 +76,55 @@ public void initialize() {
@Override
public void execute() {
Pose2d currentPose = m_drivetrain.getState().Pose;
double distanceToGoalPose = currentPose.getTranslation().getY() - m_goalPose.getTranslation().getY();

double distanceToGoalPose =
currentPose.getTranslation().getDistance(m_goalPose.getTranslation());
// double ffScaler = MathUtil.clamp((distanceToGoalPose - 0.2) / (0.8 - 0.2), 0.0, 1.0);

double ffScaler = MathUtil.clamp((distanceToGoalPose - 0.2) / (0.8 - 0.2), 0.0, 1.0);
// m_translationController.reset(
// Math.abs(m_lastSetpointTranslation.getY() - m_goalPose.getY()),
// m_translationController.getSetpoint().velocity);//ROBO RELATIVE

m_translationController.reset(
m_lastSetpointTranslation.getDistance(m_goalPose.getTranslation()),
m_translationController.getSetpoint().velocity);//ROBO RELATIVE
double driveVelocity =
// m_translationController.getSetpoint().velocity * ffScaler
m_translationController.calculate(distanceToGoalPose, 0.0);

double driveVelocityScalar =
m_translationController.getSetpoint().velocity * ffScaler
+ m_translationController.calculate(distanceToGoalPose, 0.0);

if (distanceToGoalPose < m_translationController.getPositionTolerance())
driveVelocityScalar = 0.0;
// if (distanceToGoalPose < m_translationController.getPositionTolerance())
// driveVelocityScalar = 0.0;

m_lastSetpointTranslation =
new Pose2d(
m_goalPose.getTranslation(),
currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
.transformBy(
new Transform2d(
new Translation2d(m_translationController.getSetpoint().position, 0.0),
new Rotation2d()))
.getTranslation();

double thetaVelocity =
m_thetaController.getSetpoint().velocity * ffScaler
+ m_thetaController.calculate(
currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians());
double thetaErrorAbsolute =
Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians());
if (thetaErrorAbsolute < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0;

Translation2d driveVelocity =
new Pose2d(
new Translation2d(),
currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
.transformBy(
new Transform2d(new Translation2d(driveVelocityScalar, 0.0), new Rotation2d()))
.getTranslation();

final ChassisSpeeds CS = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(driveVelocity.getX(), driveVelocity.getY(), thetaVelocity), currentPose.getRotation());

m_drivetrain.setControl(m_drive.withSpeeds(new ChassisSpeeds(m_joystickInput.getAsDouble(), CS.vyMetersPerSecond, CS.omegaRadiansPerSecond)));
// m_lastSetpointTranslation =
// new Pose2d(
// m_goalPose.getTranslation(),
// currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
// .transformBy(
// new Transform2d(
// new Translation2d(m_translationController.getSetpoint().position, 0.0),
// new Rotation2d()))
// .getTranslation();

double thetaVelocity = m_thetaController.calculate(currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians());
// m_thetaController.getSetpoint().velocity * ffScaler


// double thetaErrorAbsolute =
// Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians());
// if (thetaErrorAbsolute < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0;

// Translation2d driveVelocity =
// new Pose2d(
// new Translation2d(),
// currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle())
// .transformBy(
// new Transform2d(new Translation2d(driveVelocityScalar, 0.0), new Rotation2d()))
// .getTranslation();

final ChassisSpeeds CS = new ChassisSpeeds(m_joystickInput.getAsDouble(), driveVelocity, thetaVelocity);

m_drivetrain.setControl(m_drive.withSpeeds(CS));

Logger.recordOutput("Drivetrain/DriveToPose/ChassisSpeeds", CS);

Logger.recordOutput("Drivetrain/DriveToPose/DistanceToGoalPose", distanceToGoalPose);
Logger.recordOutput(
"Drivetrain/DriveToPose/Setpoint",
new Pose2d(
m_lastSetpointTranslation, new Rotation2d(m_thetaController.getSetpoint().position)));
Logger.recordOutput("Drivetrain/DriveToPose/Goal", m_goalPose);

m_lastPose = currentPose;
}
}

0 comments on commit 134d402

Please sign in to comment.