From 25e807712c5fe5dceecfbd1006ba607460fb2a95 Mon Sep 17 00:00:00 2001 From: JosephTLockwood Date: Mon, 17 Nov 2025 02:06:56 -0500 Subject: [PATCH 1/2] Update Drive to point to use profiled PID --- .../java/frc/robot/commands/DriveToPoint.java | 113 +++++++++++++++--- 1 file changed, 95 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveToPoint.java b/src/main/java/frc/robot/commands/DriveToPoint.java index b91f918..22f69df 100644 --- a/src/main/java/frc/robot/commands/DriveToPoint.java +++ b/src/main/java/frc/robot/commands/DriveToPoint.java @@ -4,10 +4,14 @@ package frc.robot.commands; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -17,57 +21,130 @@ public class DriveToPoint extends Command { private final CommandSwerveDrivetrain m_drivetrain; private final Pose2d m_targetPose; + private final LinearPath path; + + private double elapsedTime; + private LinearPath.State initialState; + private double lastUpdateTime; + private ChassisSpeeds lastSetpointSpeeds; + private LinearPath.State setpoint; private PIDController xController = new PIDController(10, 0, 0); private PIDController yController = new PIDController(10, 0, 0); private PIDController thetaController = new PIDController(7, 0, 0); - private SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric(); - /** Creates a new DriveToPoint. */ + private SwerveRequest.ApplyFieldSpeeds driveRequest = new SwerveRequest.ApplyFieldSpeeds(); + + private final WheelForceCalculator forceCalculator; + + // Maximum velocities and accelerations for path following + private static final double kMaxV = 4.3; // m/s + private static final double kMaxA = 4; // m/s^2 + private static final double kMaxOmega = Math.PI; // rad/s + private static final double kMaxAlpha = 2.0 * Math.PI; // rad/s^2 + + /** + * Creates a new DriveToPoint command. + * + * @param drivetrain The swerve drivetrain subsystem + * @param targetPose Target pose in field coordinates. + */ public DriveToPoint(CommandSwerveDrivetrain drivetrain, Pose2d targetPose) { m_drivetrain = drivetrain; m_targetPose = targetPose; + // Use 6 if you aren't sure about MOI + forceCalculator = new WheelForceCalculator(m_drivetrain.getModuleLocations(), 40, 6); + + // Create linear path with velocity/acceleration constraints + this.path = + new LinearPath( + new TrapezoidProfile.Constraints(kMaxV, kMaxA), + new TrapezoidProfile.Constraints(kMaxOmega, kMaxAlpha)); + // We do this so robot will rotate fastest way around thetaController.enableContinuousInput(-Math.PI, Math.PI); - // Use addRequirements() here to declare subsystem dependencies. addRequirements(drivetrain); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + Pose2d currentPose = m_drivetrain.getState().Pose; + ChassisSpeeds currentSpeeds = m_drivetrain.getState().Speeds; + + // Save the INITIAL state - this never changes + initialState = + new LinearPath.State( + currentPose, + ChassisSpeeds.fromFieldRelativeSpeeds(currentSpeeds, currentPose.getRotation())); + + elapsedTime = 0.0; + lastUpdateTime = Utils.getCurrentTimeSeconds(); + + setpoint = path.calculate(elapsedTime, initialState, m_targetPose); + + lastSetpointSpeeds = setpoint.speeds; + } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Pose2d currentPose = m_drivetrain.getPose(); + double currentTime = Utils.getCurrentTimeSeconds(); + double dt = currentTime - lastUpdateTime; + lastUpdateTime = currentTime; + + elapsedTime += dt; + + // Update setpoint to get speed robot should be running at based on current time + setpoint = path.calculate(elapsedTime, initialState, m_targetPose); - double xVelocity = xController.calculate(currentPose.getX(), m_targetPose.getX()); - double yVelocity = yController.calculate(currentPose.getY(), m_targetPose.getY()); - double thetaVelocity = + Pose2d currentPose = m_drivetrain.getState().Pose; + + // Calculate feedback corrections + double xFeedback = xController.calculate(currentPose.getX(), setpoint.pose.getX()); + double yFeedback = yController.calculate(currentPose.getY(), setpoint.pose.getY()); + double thetaFeedback = thetaController.calculate( - currentPose.getRotation().getRadians(), m_targetPose.getRotation().getRadians()); + currentPose.getRotation().getRadians(), setpoint.pose.getRotation().getRadians()); + + ChassisSpeeds feedbackSpeeds = new ChassisSpeeds(xFeedback, yFeedback, thetaFeedback); + // Calculate feedforward forces based on PLANNED trajectory change + Feedforwards feedforwards = forceCalculator.calculate(dt, lastSetpointSpeeds, setpoint.speeds); + + // Combine feedforward + feedback + ChassisSpeeds correctedSpeeds = + new ChassisSpeeds( + setpoint.speeds.vxMetersPerSecond + feedbackSpeeds.vxMetersPerSecond, + setpoint.speeds.vyMetersPerSecond + feedbackSpeeds.vyMetersPerSecond, + setpoint.speeds.omegaRadiansPerSecond + feedbackSpeeds.omegaRadiansPerSecond); + + // Apply feedforward forces (from planned trajectory) + corrected speeds (planned + feedback) m_drivetrain.setControl( driveRequest - .withVelocityX(xVelocity) - .withVelocityY(yVelocity) - .withRotationalRate(thetaVelocity)); + .withSpeeds(correctedSpeeds) + .withWheelForceFeedforwardsX(feedforwards.x_newtons) + .withWheelForceFeedforwardsY(feedforwards.y_newtons)); + + // Save setpoint speeds for next feedforward calculation + lastSetpointSpeeds = setpoint.speeds; + + // Telemetry + SmartDashboard.putString("DriveToPoint/TargetPose", m_targetPose.toString()); + SmartDashboard.putString("DriveToPoint/CurrentPose", currentPose.toString()); + SmartDashboard.putNumber( + "DriveToPoint/DistanceToTarget", + currentPose.getTranslation().getDistance(m_targetPose.getTranslation())); } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { m_drivetrain.setControl(new SwerveRequest.Idle()); } - // Returns true when the command should end. @Override public boolean isFinished() { - // Only recommend if you need it to end - // return xController.atSetpoint() && yController.atSetpoint() && thetaController.atSetpoint(); - return false; + return path.isFinished(elapsedTime); } } From 56cb5151586870a7ab8b2688dfa4beed08f16a1d Mon Sep 17 00:00:00 2001 From: JosephTLockwood Date: Wed, 19 Nov 2025 02:49:36 -0500 Subject: [PATCH 2/2] Set drive request type to Velocity for DriveToPoint MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude --- src/main/java/frc/robot/commands/DriveToPoint.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveToPoint.java b/src/main/java/frc/robot/commands/DriveToPoint.java index 22f69df..7127f40 100644 --- a/src/main/java/frc/robot/commands/DriveToPoint.java +++ b/src/main/java/frc/robot/commands/DriveToPoint.java @@ -5,6 +5,7 @@ package frc.robot.commands; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.controller.PIDController; @@ -33,7 +34,8 @@ public class DriveToPoint extends Command { private PIDController yController = new PIDController(10, 0, 0); private PIDController thetaController = new PIDController(7, 0, 0); - private SwerveRequest.ApplyFieldSpeeds driveRequest = new SwerveRequest.ApplyFieldSpeeds(); + private SwerveRequest.ApplyFieldSpeeds driveRequest = + new SwerveRequest.ApplyFieldSpeeds().withDriveRequestType(DriveRequestType.Velocity); private final WheelForceCalculator forceCalculator; @@ -78,7 +80,7 @@ public void initialize() { initialState = new LinearPath.State( currentPose, - ChassisSpeeds.fromFieldRelativeSpeeds(currentSpeeds, currentPose.getRotation())); + ChassisSpeeds.fromRobotRelativeSpeeds(currentSpeeds, currentPose.getRotation())); elapsedTime = 0.0; lastUpdateTime = Utils.getCurrentTimeSeconds();