mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-03 03:01:44 +00:00
[wpimath] Replace Speeds with Velocities (#8479)
I left "free speed" alone since that's the technical term for it. In general, velocity is a vector quantity, and speed is a magnitude (i.e., a strictly positive value). This PR also replaces the speed verbiage in MotorController with duty cycle. Fixes #8423.
This commit is contained in:
Tyler Veness
@@ -64,28 +64,28 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
private boolean m_reported;
|
||||
|
||||
/**
|
||||
* Wheel speeds for a differential drive.
|
||||
* Wheel velocities for a differential drive.
|
||||
*
|
||||
* <p>Uses normalized voltage [-1.0..1.0].
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public static class WheelSpeeds {
|
||||
/** Left wheel speed. */
|
||||
public static class WheelVelocities {
|
||||
/** Left wheel velocity. */
|
||||
public double left;
|
||||
|
||||
/** Right wheel speed. */
|
||||
/** Right wheel velocity. */
|
||||
public double right;
|
||||
|
||||
/** Constructs a WheelSpeeds with zeroes for left and right speeds. */
|
||||
public WheelSpeeds() {}
|
||||
/** Constructs a WheelVelocities with zeroes for left and right velocities. */
|
||||
public WheelVelocities() {}
|
||||
|
||||
/**
|
||||
* Constructs a WheelSpeeds.
|
||||
* Constructs a WheelVelocities.
|
||||
*
|
||||
* @param left The left speed [-1.0..1.0].
|
||||
* @param right The right speed [-1.0..1.0].
|
||||
* @param left The left velocity [-1.0..1.0].
|
||||
* @param right The right velocity [-1.0..1.0].
|
||||
*/
|
||||
public WheelSpeeds(double left, double right) {
|
||||
public WheelVelocities(double left, double right) {
|
||||
this.left = left;
|
||||
this.right = right;
|
||||
}
|
||||
@@ -103,7 +103,9 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
*/
|
||||
@SuppressWarnings({"removal", "this-escape"})
|
||||
public DifferentialDrive(MotorController leftMotor, MotorController rightMotor) {
|
||||
this((double output) -> leftMotor.set(output), (double output) -> rightMotor.set(output));
|
||||
this(
|
||||
(double output) -> leftMotor.setDutyCycle(output),
|
||||
(double output) -> rightMotor.setDutyCycle(output));
|
||||
SendableRegistry.addChild(this, leftMotor);
|
||||
SendableRegistry.addChild(this, rightMotor);
|
||||
}
|
||||
@@ -136,37 +138,37 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
|
||||
/**
|
||||
* Arcade drive method for differential drive platform. The calculated values will be squared to
|
||||
* decrease sensitivity at low speeds.
|
||||
* decrease sensitivity at low velocities.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
*/
|
||||
public void arcadeDrive(double xSpeed, double zRotation) {
|
||||
arcadeDrive(xSpeed, zRotation, true);
|
||||
public void arcadeDrive(double xVelocity, double zRotation) {
|
||||
arcadeDrive(xVelocity, zRotation, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive method for differential drive platform.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low speeds.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low velocities.
|
||||
*/
|
||||
public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
|
||||
public void arcadeDrive(double xVelocity, double zRotation, boolean squareInputs) {
|
||||
if (!m_reported) {
|
||||
HAL.reportUsage("RobotDrive", "DifferentialArcade");
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
|
||||
xVelocity = MathUtil.applyDeadband(xVelocity, m_deadband);
|
||||
zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
|
||||
|
||||
var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
|
||||
var velocities = arcadeDriveIK(xVelocity, zRotation, squareInputs);
|
||||
|
||||
m_leftOutput = speeds.left * m_maxOutput;
|
||||
m_rightOutput = speeds.right * m_maxOutput;
|
||||
m_leftOutput = velocities.left * m_maxOutput;
|
||||
m_rightOutput = velocities.right * m_maxOutput;
|
||||
|
||||
m_leftMotor.accept(m_leftOutput);
|
||||
m_rightMotor.accept(m_rightOutput);
|
||||
@@ -178,26 +180,26 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
* Curvature drive method for differential drive platform.
|
||||
*
|
||||
* <p>The rotation argument controls the curvature of the robot's path rather than its rate of
|
||||
* heading change. This makes the robot more controllable at high speeds.
|
||||
* heading change. This makes the robot more controllable at high velocities.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
|
||||
* @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
|
||||
* maneuvers. zRotation will control turning rate instead of curvature.
|
||||
*/
|
||||
public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
|
||||
public void curvatureDrive(double xVelocity, double zRotation, boolean allowTurnInPlace) {
|
||||
if (!m_reported) {
|
||||
HAL.reportUsage("RobotDrive", "DifferentialCurvature");
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
|
||||
xVelocity = MathUtil.applyDeadband(xVelocity, m_deadband);
|
||||
zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
|
||||
|
||||
var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
|
||||
var velocities = curvatureDriveIK(xVelocity, zRotation, allowTurnInPlace);
|
||||
|
||||
m_leftOutput = speeds.left * m_maxOutput;
|
||||
m_rightOutput = speeds.right * m_maxOutput;
|
||||
m_leftOutput = velocities.left * m_maxOutput;
|
||||
m_rightOutput = velocities.right * m_maxOutput;
|
||||
|
||||
m_leftMotor.accept(m_leftOutput);
|
||||
m_rightMotor.accept(m_rightOutput);
|
||||
@@ -207,37 +209,39 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
|
||||
/**
|
||||
* Tank drive method for differential drive platform. The calculated values will be squared to
|
||||
* decrease sensitivity at low speeds.
|
||||
* decrease sensitivity at low velocities.
|
||||
*
|
||||
* @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
|
||||
* @param leftVelocity The robot's left side velocity along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
* @param rightVelocity The robot's right side velocity along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
*/
|
||||
public void tankDrive(double leftSpeed, double rightSpeed) {
|
||||
tankDrive(leftSpeed, rightSpeed, true);
|
||||
public void tankDrive(double leftVelocity, double rightVelocity) {
|
||||
tankDrive(leftVelocity, rightVelocity, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Tank drive method for differential drive platform.
|
||||
*
|
||||
* @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
|
||||
* @param leftVelocity The robot left side's velocity along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low speeds.
|
||||
* @param rightVelocity The robot right side's velocity along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low velocities.
|
||||
*/
|
||||
public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
|
||||
public void tankDrive(double leftVelocity, double rightVelocity, boolean squareInputs) {
|
||||
if (!m_reported) {
|
||||
HAL.reportUsage("RobotDrive", "DifferentialTank");
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
|
||||
rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
|
||||
leftVelocity = MathUtil.applyDeadband(leftVelocity, m_deadband);
|
||||
rightVelocity = MathUtil.applyDeadband(rightVelocity, m_deadband);
|
||||
|
||||
var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
|
||||
var velocities = tankDriveIK(leftVelocity, rightVelocity, squareInputs);
|
||||
|
||||
m_leftOutput = speeds.left * m_maxOutput;
|
||||
m_rightOutput = speeds.right * m_maxOutput;
|
||||
m_leftOutput = velocities.left * m_maxOutput;
|
||||
m_rightOutput = velocities.right * m_maxOutput;
|
||||
|
||||
m_leftMotor.accept(m_leftOutput);
|
||||
m_rightMotor.accept(m_rightOutput);
|
||||
@@ -248,99 +252,102 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
/**
|
||||
* Arcade drive inverse kinematics for differential drive platform.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low speeds.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
* @param squareInputs If set, decreases the input sensitivity at low velocities.
|
||||
* @return Wheel velocities [-1.0..1.0].
|
||||
*/
|
||||
public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
|
||||
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
|
||||
public static WheelVelocities arcadeDriveIK(
|
||||
double xVelocity, double zRotation, boolean squareInputs) {
|
||||
xVelocity = Math.clamp(xVelocity, -1.0, 1.0);
|
||||
zRotation = Math.clamp(zRotation, -1.0, 1.0);
|
||||
|
||||
// Square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power.
|
||||
if (squareInputs) {
|
||||
xSpeed = MathUtil.copyDirectionPow(xSpeed, 2);
|
||||
xVelocity = MathUtil.copyDirectionPow(xVelocity, 2);
|
||||
zRotation = MathUtil.copyDirectionPow(zRotation, 2);
|
||||
}
|
||||
|
||||
double leftSpeed = xSpeed - zRotation;
|
||||
double rightSpeed = xSpeed + zRotation;
|
||||
double leftVelocity = xVelocity - zRotation;
|
||||
double rightVelocity = xVelocity + zRotation;
|
||||
|
||||
// Find the maximum possible value of (throttle + turn) along the vector
|
||||
// that the joystick is pointing, then desaturate the wheel speeds
|
||||
double greaterInput = Math.max(Math.abs(xSpeed), Math.abs(zRotation));
|
||||
double lesserInput = Math.min(Math.abs(xSpeed), Math.abs(zRotation));
|
||||
// that the joystick is pointing, then desaturate the wheel velocities
|
||||
double greaterInput = Math.max(Math.abs(xVelocity), Math.abs(zRotation));
|
||||
double lesserInput = Math.min(Math.abs(xVelocity), Math.abs(zRotation));
|
||||
if (greaterInput == 0.0) {
|
||||
return new WheelSpeeds(0.0, 0.0);
|
||||
return new WheelVelocities(0.0, 0.0);
|
||||
}
|
||||
double saturatedInput = (greaterInput + lesserInput) / greaterInput;
|
||||
leftSpeed /= saturatedInput;
|
||||
rightSpeed /= saturatedInput;
|
||||
leftVelocity /= saturatedInput;
|
||||
rightVelocity /= saturatedInput;
|
||||
|
||||
return new WheelSpeeds(leftSpeed, rightSpeed);
|
||||
return new WheelVelocities(leftVelocity, rightVelocity);
|
||||
}
|
||||
|
||||
/**
|
||||
* Curvature drive inverse kinematics for differential drive platform.
|
||||
*
|
||||
* <p>The rotation argument controls the curvature of the robot's path rather than its rate of
|
||||
* heading change. This makes the robot more controllable at high speeds.
|
||||
* heading change. This makes the robot more controllable at high velocities.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
|
||||
* @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
|
||||
* maneuvers. zRotation will control rotation rate around the Z axis instead of curvature.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
* @return Wheel velocities [-1.0..1.0].
|
||||
*/
|
||||
public static WheelSpeeds curvatureDriveIK(
|
||||
double xSpeed, double zRotation, boolean allowTurnInPlace) {
|
||||
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
|
||||
public static WheelVelocities curvatureDriveIK(
|
||||
double xVelocity, double zRotation, boolean allowTurnInPlace) {
|
||||
xVelocity = Math.clamp(xVelocity, -1.0, 1.0);
|
||||
zRotation = Math.clamp(zRotation, -1.0, 1.0);
|
||||
|
||||
double leftSpeed;
|
||||
double rightSpeed;
|
||||
double leftVelocity;
|
||||
double rightVelocity;
|
||||
|
||||
if (allowTurnInPlace) {
|
||||
leftSpeed = xSpeed - zRotation;
|
||||
rightSpeed = xSpeed + zRotation;
|
||||
leftVelocity = xVelocity - zRotation;
|
||||
rightVelocity = xVelocity + zRotation;
|
||||
} else {
|
||||
leftSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
|
||||
rightSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
|
||||
leftVelocity = xVelocity - Math.abs(xVelocity) * zRotation;
|
||||
rightVelocity = xVelocity + Math.abs(xVelocity) * zRotation;
|
||||
}
|
||||
|
||||
// Desaturate wheel speeds
|
||||
double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
|
||||
// Desaturate wheel velocities
|
||||
double maxMagnitude = Math.max(Math.abs(leftVelocity), Math.abs(rightVelocity));
|
||||
if (maxMagnitude > 1.0) {
|
||||
leftSpeed /= maxMagnitude;
|
||||
rightSpeed /= maxMagnitude;
|
||||
leftVelocity /= maxMagnitude;
|
||||
rightVelocity /= maxMagnitude;
|
||||
}
|
||||
|
||||
return new WheelSpeeds(leftSpeed, rightSpeed);
|
||||
return new WheelVelocities(leftVelocity, rightVelocity);
|
||||
}
|
||||
|
||||
/**
|
||||
* Tank drive inverse kinematics for differential drive platform.
|
||||
*
|
||||
* @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
|
||||
* @param leftVelocity The robot left side's velocity along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low speeds.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
* @param rightVelocity The robot right side's velocity along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low velocities.
|
||||
* @return Wheel velocities [-1.0..1.0].
|
||||
*/
|
||||
public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
|
||||
leftSpeed = Math.clamp(leftSpeed, -1.0, 1.0);
|
||||
rightSpeed = Math.clamp(rightSpeed, -1.0, 1.0);
|
||||
public static WheelVelocities tankDriveIK(
|
||||
double leftVelocity, double rightVelocity, boolean squareInputs) {
|
||||
leftVelocity = Math.clamp(leftVelocity, -1.0, 1.0);
|
||||
rightVelocity = Math.clamp(rightVelocity, -1.0, 1.0);
|
||||
|
||||
// Square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power.
|
||||
if (squareInputs) {
|
||||
leftSpeed = MathUtil.copyDirectionPow(leftSpeed, 2);
|
||||
rightSpeed = MathUtil.copyDirectionPow(rightSpeed, 2);
|
||||
leftVelocity = MathUtil.copyDirectionPow(leftVelocity, 2);
|
||||
rightVelocity = MathUtil.copyDirectionPow(rightVelocity, 2);
|
||||
}
|
||||
|
||||
return new WheelSpeeds(leftSpeed, rightSpeed);
|
||||
return new WheelVelocities(leftVelocity, rightVelocity);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -363,7 +370,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("DifferentialDrive");
|
||||
builder.setActuator(true);
|
||||
builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor);
|
||||
builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor);
|
||||
builder.addDoubleProperty("Left Motor Velocity", () -> m_leftOutput, m_leftMotor);
|
||||
builder.addDoubleProperty("Right Motor Velocity", () -> m_rightOutput, m_rightMotor);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -66,36 +66,36 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
private boolean m_reported;
|
||||
|
||||
/**
|
||||
* Wheel speeds for a mecanum drive.
|
||||
* Wheel velocities for a mecanum drive.
|
||||
*
|
||||
* <p>Uses normalized voltage [-1.0..1.0].
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public static class WheelSpeeds {
|
||||
/** Front-left wheel speed. */
|
||||
public static class WheelVelocities {
|
||||
/** Front-left wheel velocity. */
|
||||
public double frontLeft;
|
||||
|
||||
/** Front-right wheel speed. */
|
||||
/** Front-right wheel velocity. */
|
||||
public double frontRight;
|
||||
|
||||
/** Rear-left wheel speed. */
|
||||
/** Rear-left wheel velocity. */
|
||||
public double rearLeft;
|
||||
|
||||
/** Rear-right wheel speed. */
|
||||
/** Rear-right wheel velocity. */
|
||||
public double rearRight;
|
||||
|
||||
/** Constructs a WheelSpeeds with zeroes for all four speeds. */
|
||||
public WheelSpeeds() {}
|
||||
/** Constructs a WheelVelocities with zeroes for all four velocities. */
|
||||
public WheelVelocities() {}
|
||||
|
||||
/**
|
||||
* Constructs a WheelSpeeds.
|
||||
* Constructs a WheelVelocities.
|
||||
*
|
||||
* @param frontLeft The front left speed [-1.0..1.0].
|
||||
* @param frontRight The front right speed [-1.0..1.0].
|
||||
* @param rearLeft The rear left speed [-1.0..1.0].
|
||||
* @param rearRight The rear right speed [-1.0..1.0].
|
||||
* @param frontLeft The front left velocity [-1.0..1.0].
|
||||
* @param frontRight The front right velocity [-1.0..1.0].
|
||||
* @param rearLeft The rear left velocity [-1.0..1.0].
|
||||
* @param rearRight The rear right velocity [-1.0..1.0].
|
||||
*/
|
||||
public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
|
||||
public WheelVelocities(double frontLeft, double frontRight, double rearLeft, double rearRight) {
|
||||
this.frontLeft = frontLeft;
|
||||
this.frontRight = frontRight;
|
||||
this.rearLeft = rearLeft;
|
||||
@@ -120,10 +120,10 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
MotorController frontRightMotor,
|
||||
MotorController rearRightMotor) {
|
||||
this(
|
||||
(double output) -> frontLeftMotor.set(output),
|
||||
(double output) -> rearLeftMotor.set(output),
|
||||
(double output) -> frontRightMotor.set(output),
|
||||
(double output) -> rearRightMotor.set(output));
|
||||
(double output) -> frontLeftMotor.setDutyCycle(output),
|
||||
(double output) -> rearLeftMotor.setDutyCycle(output),
|
||||
(double output) -> frontRightMotor.setDutyCycle(output),
|
||||
(double output) -> rearRightMotor.setDutyCycle(output));
|
||||
SendableRegistry.addChild(this, frontLeftMotor);
|
||||
SendableRegistry.addChild(this, rearLeftMotor);
|
||||
SendableRegistry.addChild(this, frontRightMotor);
|
||||
@@ -167,46 +167,47 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
/**
|
||||
* Drive method for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
|
||||
* <p>Angles are measured counterclockwise from the positive X axis. The robot's velocity is
|
||||
* independent of its angle or rotation rate.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param yVelocity The robot's velocity along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
*/
|
||||
public void driveCartesian(double xSpeed, double ySpeed, double zRotation) {
|
||||
driveCartesian(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
|
||||
public void driveCartesian(double xVelocity, double yVelocity, double zRotation) {
|
||||
driveCartesian(xVelocity, yVelocity, zRotation, Rotation2d.kZero);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive method for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
|
||||
* <p>Angles are measured counterclockwise from the positive X axis. The robot's velocity is
|
||||
* independent of its angle or rotation rate.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param yVelocity The robot's velocity along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
|
||||
* controls.
|
||||
*/
|
||||
public void driveCartesian(double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
|
||||
public void driveCartesian(
|
||||
double xVelocity, double yVelocity, double zRotation, Rotation2d gyroAngle) {
|
||||
if (!m_reported) {
|
||||
HAL.reportUsage("RobotDrive", "MecanumCartesian");
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
|
||||
ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
|
||||
xVelocity = MathUtil.applyDeadband(xVelocity, m_deadband);
|
||||
yVelocity = MathUtil.applyDeadband(yVelocity, m_deadband);
|
||||
|
||||
var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
|
||||
var velocities = driveCartesianIK(xVelocity, yVelocity, zRotation, gyroAngle);
|
||||
|
||||
m_frontLeftOutput = speeds.frontLeft * m_maxOutput;
|
||||
m_rearLeftOutput = speeds.rearLeft * m_maxOutput;
|
||||
m_frontRightOutput = speeds.frontRight * m_maxOutput;
|
||||
m_rearRightOutput = speeds.rearRight * m_maxOutput;
|
||||
m_frontLeftOutput = velocities.frontLeft * m_maxOutput;
|
||||
m_rearLeftOutput = velocities.rearLeft * m_maxOutput;
|
||||
m_frontRightOutput = velocities.frontRight * m_maxOutput;
|
||||
m_rearRightOutput = velocities.rearRight * m_maxOutput;
|
||||
|
||||
m_frontLeftMotor.accept(m_frontLeftOutput);
|
||||
m_frontRightMotor.accept(m_frontRightOutput);
|
||||
@@ -219,10 +220,10 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
/**
|
||||
* Drive method for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured counterclockwise from straight ahead. The speed at which the robot
|
||||
* <p>Angles are measured counterclockwise from straight ahead. The velocity at which the robot
|
||||
* drives (translation) is independent of its angle or rotation rate.
|
||||
*
|
||||
* @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
|
||||
* @param magnitude The robot's velocity at a given angle [-1.0..1.0]. Forward is positive.
|
||||
* @param angle The gyro heading around the Z axis at which the robot drives.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
@@ -240,54 +241,55 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
/**
|
||||
* Cartesian inverse kinematics for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
|
||||
* <p>Angles are measured counterclockwise from the positive X axis. The robot's velocity is
|
||||
* independent of its angle or rotation rate.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param yVelocity The robot's velocity along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
* @return Wheel velocities [-1.0..1.0].
|
||||
*/
|
||||
public static WheelSpeeds driveCartesianIK(double xSpeed, double ySpeed, double zRotation) {
|
||||
return driveCartesianIK(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
|
||||
public static WheelVelocities driveCartesianIK(
|
||||
double xVelocity, double yVelocity, double zRotation) {
|
||||
return driveCartesianIK(xVelocity, yVelocity, zRotation, Rotation2d.kZero);
|
||||
}
|
||||
|
||||
/**
|
||||
* Cartesian inverse kinematics for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent of
|
||||
* its angle or rotation rate.
|
||||
* <p>Angles are measured clockwise from the positive X axis. The robot's velocity is independent
|
||||
* of its angle or rotation rate.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param xVelocity The robot's velocity along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param yVelocity The robot's velocity along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
|
||||
* controls.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
* @return Wheel velocities [-1.0..1.0].
|
||||
*/
|
||||
public static WheelSpeeds driveCartesianIK(
|
||||
double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
|
||||
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
|
||||
ySpeed = Math.clamp(ySpeed, -1.0, 1.0);
|
||||
public static WheelVelocities driveCartesianIK(
|
||||
double xVelocity, double yVelocity, double zRotation, Rotation2d gyroAngle) {
|
||||
xVelocity = Math.clamp(xVelocity, -1.0, 1.0);
|
||||
yVelocity = Math.clamp(yVelocity, -1.0, 1.0);
|
||||
|
||||
// Compensate for gyro angle.
|
||||
var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus());
|
||||
var input = new Translation2d(xVelocity, yVelocity).rotateBy(gyroAngle.unaryMinus());
|
||||
|
||||
double[] wheelSpeeds = new double[4];
|
||||
wheelSpeeds[MotorType.kFrontLeft.value] = input.getX() + input.getY() + zRotation;
|
||||
wheelSpeeds[MotorType.kFrontRight.value] = input.getX() - input.getY() - zRotation;
|
||||
wheelSpeeds[MotorType.kRearLeft.value] = input.getX() - input.getY() + zRotation;
|
||||
wheelSpeeds[MotorType.kRearRight.value] = input.getX() + input.getY() - zRotation;
|
||||
double[] wheelVelocities = new double[4];
|
||||
wheelVelocities[MotorType.kFrontLeft.value] = input.getX() + input.getY() + zRotation;
|
||||
wheelVelocities[MotorType.kFrontRight.value] = input.getX() - input.getY() - zRotation;
|
||||
wheelVelocities[MotorType.kRearLeft.value] = input.getX() - input.getY() + zRotation;
|
||||
wheelVelocities[MotorType.kRearRight.value] = input.getX() + input.getY() - zRotation;
|
||||
|
||||
normalize(wheelSpeeds);
|
||||
normalize(wheelVelocities);
|
||||
|
||||
return new WheelSpeeds(
|
||||
wheelSpeeds[MotorType.kFrontLeft.value],
|
||||
wheelSpeeds[MotorType.kFrontRight.value],
|
||||
wheelSpeeds[MotorType.kRearLeft.value],
|
||||
wheelSpeeds[MotorType.kRearRight.value]);
|
||||
return new WheelVelocities(
|
||||
wheelVelocities[MotorType.kFrontLeft.value],
|
||||
wheelVelocities[MotorType.kFrontRight.value],
|
||||
wheelVelocities[MotorType.kRearLeft.value],
|
||||
wheelVelocities[MotorType.kRearRight.value]);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -314,10 +316,12 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("MecanumDrive");
|
||||
builder.setActuator(true);
|
||||
builder.addDoubleProperty("Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor);
|
||||
builder.addDoubleProperty(
|
||||
"Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor);
|
||||
builder.addDoubleProperty("Rear Left Motor Speed", () -> m_rearLeftOutput, m_rearLeftMotor);
|
||||
builder.addDoubleProperty("Rear Right Motor Speed", () -> m_rearRightOutput, m_rearRightMotor);
|
||||
"Front Left Motor Velocity", () -> m_frontLeftOutput, m_frontLeftMotor);
|
||||
builder.addDoubleProperty(
|
||||
"Front Right Motor Velocity", () -> m_frontRightOutput, m_frontRightMotor);
|
||||
builder.addDoubleProperty("Rear Left Motor Velocity", () -> m_rearLeftOutput, m_rearLeftMotor);
|
||||
builder.addDoubleProperty(
|
||||
"Rear Right Motor Velocity", () -> m_rearRightOutput, m_rearRightMotor);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -96,21 +96,21 @@ public abstract class RobotDriveBase extends MotorSafety {
|
||||
public abstract String getDescription();
|
||||
|
||||
/**
|
||||
* Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
|
||||
* Normalize all wheel velocities if the magnitude of any wheel is greater than 1.0.
|
||||
*
|
||||
* @param wheelSpeeds List of wheel speeds to normalize.
|
||||
* @param wheelVelocities List of wheel velocities to normalize.
|
||||
*/
|
||||
protected static void normalize(double[] wheelSpeeds) {
|
||||
double maxMagnitude = Math.abs(wheelSpeeds[0]);
|
||||
for (int i = 1; i < wheelSpeeds.length; i++) {
|
||||
double temp = Math.abs(wheelSpeeds[i]);
|
||||
protected static void normalize(double[] wheelVelocities) {
|
||||
double maxMagnitude = Math.abs(wheelVelocities[0]);
|
||||
for (int i = 1; i < wheelVelocities.length; i++) {
|
||||
double temp = Math.abs(wheelVelocities[i]);
|
||||
if (maxMagnitude < temp) {
|
||||
maxMagnitude = temp;
|
||||
}
|
||||
}
|
||||
if (maxMagnitude > 1.0) {
|
||||
for (int i = 0; i < wheelSpeeds.length; i++) {
|
||||
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
|
||||
for (int i = 0; i < wheelVelocities.length; i++) {
|
||||
wheelVelocities[i] = wheelVelocities[i] / maxMagnitude;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user