mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)
This commit is contained in:
Gold856
@@ -1,168 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
|
||||
/**
|
||||
* This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain
|
||||
* (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve
|
||||
* compared to skid-steer style drivetrains because it is possible to individually control
|
||||
* field-relative x, y, and angular velocity.
|
||||
*
|
||||
* <p>The holonomic drive controller takes in one PID controller for each direction, field-relative
|
||||
* x and y, and one profiled PID controller for the angular direction. Because the heading dynamics
|
||||
* are decoupled from translations, users can specify a custom heading that the drivetrain should
|
||||
* point toward. This heading reference is profiled for smoothness.
|
||||
*/
|
||||
public class HolonomicDriveController {
|
||||
private Pose2d m_poseError = Pose2d.kZero;
|
||||
private Rotation2d m_rotationError = Rotation2d.kZero;
|
||||
private Pose2d m_poseTolerance = Pose2d.kZero;
|
||||
private boolean m_enabled = true;
|
||||
|
||||
private final PIDController m_xController;
|
||||
private final PIDController m_yController;
|
||||
private final ProfiledPIDController m_thetaController;
|
||||
|
||||
private boolean m_firstRun = true;
|
||||
|
||||
/**
|
||||
* Constructs a holonomic drive controller.
|
||||
*
|
||||
* @param xController A PID Controller to respond to error in the field-relative x direction.
|
||||
* @param yController A PID Controller to respond to error in the field-relative y direction.
|
||||
* @param thetaController A profiled PID controller to respond to error in angle.
|
||||
*/
|
||||
public HolonomicDriveController(
|
||||
PIDController xController, PIDController yController, ProfiledPIDController thetaController) {
|
||||
m_xController = xController;
|
||||
m_yController = yController;
|
||||
m_thetaController = thetaController;
|
||||
m_thetaController.enableContinuousInput(0, Units.degreesToRadians(360.0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*
|
||||
* @return True if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
public boolean atReference() {
|
||||
final var eTranslate = m_poseError.getTranslation();
|
||||
final var eRotate = m_rotationError;
|
||||
final var tolTranslate = m_poseTolerance.getTranslation();
|
||||
final var tolRotate = m_poseTolerance.getRotation();
|
||||
return Math.abs(eTranslate.getX()) < tolTranslate.getX()
|
||||
&& Math.abs(eTranslate.getY()) < tolTranslate.getY()
|
||||
&& Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerance for use with atReference().
|
||||
*
|
||||
* @param tolerance The pose error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(Pose2d tolerance) {
|
||||
m_poseTolerance = tolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose, as measured by odometry or pose estimator.
|
||||
* @param trajectoryPose The desired trajectory pose, as sampled for the current timestep.
|
||||
* @param desiredLinearVelocity The desired linear velocity in m/s.
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose,
|
||||
Pose2d trajectoryPose,
|
||||
double desiredLinearVelocity,
|
||||
Rotation2d desiredHeading) {
|
||||
// If this is the first run, then we need to reset the theta controller to the current pose's
|
||||
// heading.
|
||||
if (m_firstRun) {
|
||||
m_thetaController.reset(currentPose.getRotation().getRadians());
|
||||
m_firstRun = false;
|
||||
}
|
||||
|
||||
// Calculate feedforward velocities (field-relative).
|
||||
double xFF = desiredLinearVelocity * trajectoryPose.getRotation().getCos();
|
||||
double yFF = desiredLinearVelocity * trajectoryPose.getRotation().getSin();
|
||||
double thetaFF =
|
||||
m_thetaController.calculate(
|
||||
currentPose.getRotation().getRadians(), desiredHeading.getRadians());
|
||||
|
||||
m_poseError = trajectoryPose.relativeTo(currentPose);
|
||||
m_rotationError = desiredHeading.minus(currentPose.getRotation());
|
||||
|
||||
if (!m_enabled) {
|
||||
return new ChassisSpeeds(xFF, yFF, thetaFF).toRobotRelative(currentPose.getRotation());
|
||||
}
|
||||
|
||||
// Calculate feedback velocities (based on position error).
|
||||
double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
|
||||
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
|
||||
|
||||
// Return next output.
|
||||
return new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF)
|
||||
.toRobotRelative(currentPose.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose, as measured by odometry or pose estimator.
|
||||
* @param desiredState The desired trajectory pose, as sampled for the current timestep.
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
|
||||
return calculate(currentPose, desiredState.pose, desiredState.velocity, desiredHeading);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting problems. When calculate() is called on
|
||||
* a disabled controller, only feedforward values are returned.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
public void setEnabled(boolean enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the x controller.
|
||||
*
|
||||
* @return X PIDController
|
||||
*/
|
||||
public PIDController getXController() {
|
||||
return m_xController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the y controller.
|
||||
*
|
||||
* @return Y PIDController
|
||||
*/
|
||||
public PIDController getYController() {
|
||||
return m_yController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading controller.
|
||||
*
|
||||
* @return heading ProfiledPIDController
|
||||
*/
|
||||
public ProfiledPIDController getThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
}
|
||||
@@ -1,53 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.kinematics;
|
||||
|
||||
/**
|
||||
* Represents the motor voltages for a mecanum drive drivetrain.
|
||||
*
|
||||
* @deprecated Use {@link
|
||||
* edu.wpi.first.wpilibj2.command.MecanumControllerCommand.MecanumVoltagesConsumer}
|
||||
*/
|
||||
@Deprecated(since = "2025", forRemoval = true)
|
||||
public class MecanumDriveMotorVoltages {
|
||||
/** Voltage of the front left motor. */
|
||||
public double frontLeft;
|
||||
|
||||
/** Voltage of the front right motor. */
|
||||
public double frontRight;
|
||||
|
||||
/** Voltage of the rear left motor. */
|
||||
public double rearLeft;
|
||||
|
||||
/** Voltage of the rear right motor. */
|
||||
public double rearRight;
|
||||
|
||||
/** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
|
||||
public MecanumDriveMotorVoltages() {}
|
||||
|
||||
/**
|
||||
* Constructs a MecanumDriveMotorVoltages.
|
||||
*
|
||||
* @param frontLeft Voltage of the front left motor.
|
||||
* @param frontRight Voltage of the front right motor.
|
||||
* @param rearLeft Voltage of the rear left motor.
|
||||
* @param rearRight Voltage of the rear right motor.
|
||||
*/
|
||||
public MecanumDriveMotorVoltages(
|
||||
double frontLeft, double frontRight, double rearLeft, double rearRight) {
|
||||
this.frontLeft = frontLeft;
|
||||
this.frontRight = frontRight;
|
||||
this.rearLeft = rearLeft;
|
||||
this.rearRight = rearRight;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format(
|
||||
"MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
|
||||
+ "Rear Left: %.2f V, Rear Right: %.2f V)",
|
||||
frontLeft, frontRight, rearLeft, rearRight);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user