Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

improving auto align command implementation

Browse Source
This commit is contained in:
Tylr-J42 2025-03-24 02:13:02 -04:00
parent d693faf5c9
commit 23e2ad5a9b
2 changed files with 30 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -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 {

37
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -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<Rotation2d> 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()),