From 23e2ad5a9bf06d2c66638b675c34886e5bf19197 Mon Sep 17 00:00:00 2001 From: Tylr-J42 <84476781+Tylr-J42@users.noreply.github.com> Date: Mon, 24 Mar 2025 02:13:02 -0400 Subject: [PATCH] improving auto align command implementation --- .../frc/robot/constants/AutoConstants.java | 11 +++--- .../java/frc/robot/subsystems/Drivetrain.java | 37 +++++++++++++------ 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 49184d4..369fd6e 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -11,14 +11,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.trajectory.TrapezoidProfile; public class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 5.5; + public static final double kMaxSpeedMetersPerSecond = 5; public static final double kMaxAccelerationMetersPerSecondSquared = 4; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 3.5; - public static final double kPYController = 5; - public static final double kPThetaController = 5.5; + public static final double kPXYController = 3.5; + public static final double kPThetaController = 5; // Constraint for the motion profiled robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( @@ -28,8 +27,8 @@ public class AutoConstants { public static RobotConfig kRobotConfig; public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController( - new PIDConstants(kPXController, 0, 0), - new PIDConstants(kPYController, 0, 0) + new PIDConstants(kPXYController, 0, 0), + new PIDConstants(kPThetaController, 0, 0) ); static { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 56a5ed8..f0dd5a9 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -19,7 +19,9 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -60,10 +62,12 @@ public class Drivetrain extends SubsystemBase { public Orchestra m_orchestra = new Orchestra(); private Timer musicTimer = new Timer(); - private PIDController pidHeading; + private ProfiledPIDController pidHeading; private PIDController pidTranslationX; private PIDController pidTranslationY; + private HolonomicDriveController driveController; + public Vision vision; public Pose2d orangePose2d; @@ -111,15 +115,16 @@ public class Drivetrain extends SubsystemBase { VecBuilder.fill(1, 1, Units.degreesToRadians(360)) ); - pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0); + pidHeading = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); pidHeading.setTolerance(Units.degreesToRadians(3)); pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180)); - pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0); + pidTranslationX = new PIDController(AutoConstants.kPXYController,0,0); pidTranslationX.setTolerance(Units.inchesToMeters(0.5)); - pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0); + pidTranslationY = new PIDController(AutoConstants.kPXYController,0,0); pidTranslationY.setTolerance(Units.inchesToMeters(0.5)); + driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading); AutoBuilder.configure( this::getPose, @@ -321,16 +326,24 @@ public class Drivetrain extends SubsystemBase { public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier headingSetpoint){ return startRun(() -> { - pidHeading.reset(); - pidTranslationX.reset(); - pidTranslationY.reset(); }, () -> { - drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.05, 0.05), - MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.05, 0.05), - pidHeading.calculate(Units.degreesToRadians(getHeading()), headingSetpoint.get().getRadians()), - true); - Logger.recordOutput("reeeee", pidHeading.getError()); + ChassisSpeeds controlEffort = driveController.calculate( + m_estimator.getEstimatedPosition(), + new Pose2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble(), + headingSetpoint.get()), + 0, + headingSetpoint.get() + ); + + double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond); + if (speed > AutoConstants.kMaxSpeedMetersPerSecond) { + double mul = AutoConstants.kMaxSpeedMetersPerSecond / speed; + controlEffort.vxMetersPerSecond *= mul; + controlEffort.vyMetersPerSecond *= mul; + } + + driveWithChassisSpeeds(controlEffort); Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d( new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),