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; import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants { 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 kMaxAccelerationMetersPerSecondSquared = 4;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 3.5; public static final double kPXYController = 3.5;
public static final double kPYController = 5; public static final double kPThetaController = 5;
public static final double kPThetaController = 5.5;
// Constraint for the motion profiled robot angle controller // Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
@ -28,8 +27,8 @@ public class AutoConstants {
public static RobotConfig kRobotConfig; public static RobotConfig kRobotConfig;
public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController( public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
new PIDConstants(kPXController, 0, 0), new PIDConstants(kPXYController, 0, 0),
new PIDConstants(kPYController, 0, 0) new PIDConstants(kPThetaController, 0, 0)
); );
static { 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.MathUtil;
import edu.wpi.first.math.VecBuilder; 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.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
@ -60,10 +62,12 @@ public class Drivetrain extends SubsystemBase {
public Orchestra m_orchestra = new Orchestra(); public Orchestra m_orchestra = new Orchestra();
private Timer musicTimer = new Timer(); private Timer musicTimer = new Timer();
private PIDController pidHeading; private ProfiledPIDController pidHeading;
private PIDController pidTranslationX; private PIDController pidTranslationX;
private PIDController pidTranslationY; private PIDController pidTranslationY;
private HolonomicDriveController driveController;
public Vision vision; public Vision vision;
public Pose2d orangePose2d; public Pose2d orangePose2d;
@ -111,15 +115,16 @@ public class Drivetrain extends SubsystemBase {
VecBuilder.fill(1, 1, Units.degreesToRadians(360)) 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.setTolerance(Units.degreesToRadians(3));
pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180)); 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)); 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)); pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
AutoBuilder.configure( AutoBuilder.configure(
this::getPose, this::getPose,
@ -321,16 +326,24 @@ public class Drivetrain extends SubsystemBase {
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){ public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
return startRun(() -> { return startRun(() -> {
pidHeading.reset();
pidTranslationX.reset();
pidTranslationY.reset();
}, },
() -> { () -> {
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.05, 0.05), ChassisSpeeds controlEffort = driveController.calculate(
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.05, 0.05), m_estimator.getEstimatedPosition(),
pidHeading.calculate(Units.degreesToRadians(getHeading()), headingSetpoint.get().getRadians()), new Pose2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble(),
true); headingSetpoint.get()),
Logger.recordOutput("reeeee", pidHeading.getError()); 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( Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()), new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),