Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
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
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpilibj/src/main/java/org/wpilib/counter/Tachometer.java
View File

@@ -13,10 +13,10 @@ import org.wpilib.util.sendable.SendableRegistry;
/**
* Tachometer.
*
* <p>The Tachometer class measures the time between digital pulses to determine the rotation speed
* of a mechanism. Examples of devices that could be used with the tachometer class are a hall
* effect sensor, break beam sensor, or optical sensor detecting tape on a shooter wheel. Unlike
* encoders, this class only needs a single digital input.
* <p>The Tachometer class measures the time between digital pulses to determine the rotation
* velocity of a mechanism. Examples of devices that could be used with the tachometer class are a
* hall effect sensor, break beam sensor, or optical sensor detecting tape on a shooter wheel.
* Unlike encoders, this class only needs a single digital input.
*/
public class Tachometer implements Sendable, AutoCloseable {
private final int m_handle;

181
wpilibj/src/main/java/org/wpilib/drive/DifferentialDrive.java
View File

@@ -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);
}
}

138
wpilibj/src/main/java/org/wpilib/drive/MecanumDrive.java
View File

@@ -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);
}
}

16
wpilibj/src/main/java/org/wpilib/drive/RobotDriveBase.java
View File

@@ -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;
}
}
}

8
wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java
View File

@@ -153,14 +153,14 @@ public class ExpansionHubMotor implements AutoCloseable {
}
/**
* Sets the percentage power to run the motor at, between -1 and 1.
* Sets the duty cycle.
*
* @param power The power to drive the motor at
* @param dutyCycle The duty cycle between -1 and 1 (sign indicates direction).
*/
public void setPercentagePower(double power) {
public void setDutyCycle(double dutyCycle) {
setEnabled(true);
m_modePublisher.set(kPercentageMode);
m_setpointPublisher.set(power);
m_setpointPublisher.set(dutyCycle);
}
/**

24
wpilibj/src/main/java/org/wpilib/hardware/led/LEDPattern.java
View File

@@ -114,7 +114,7 @@ public interface LEDPattern {
*
* <p>This method is intentionally designed to use separate objects for reading and writing data.
* By splitting them up, we can easily modify the behavior of some base pattern to make it {@link
* #scrollAtRelativeSpeed(Frequency) scroll}, {@link #blink(Time, Time) blink}, or {@link
* #scrollAtRelativeVelocity(Frequency) scroll}, {@link #blink(Time, Time) blink}, or {@link
* #breathe(Time) breathe} by intercepting the data writes to transform their behavior to whatever
* we like.
*
@@ -182,13 +182,13 @@ public interface LEDPattern {
/**
* Creates a pattern that displays this one in reverse. Scrolling patterns will scroll in the
* opposite direction (but at the same speed). It will treat the end of an LED strip as the start,
* and the start of the strip as the end. This can be useful for making ping-pong patterns that
* travel from one end of an LED strip to the other, then reverse direction and move back to the
* start. This can also be useful when working with LED strips connected in a serpentine pattern
* (where the start of one strip is connected to the end of the previous one); however, consider
* using a {@link AddressableLEDBufferView#reversed() reversed view} of the overall buffer for
* that segment rather than reversing patterns.
* opposite direction (but at the same velocity). It will treat the end of an LED strip as the
* start, and the start of the strip as the end. This can be useful for making ping-pong patterns
* that travel from one end of an LED strip to the other, then reverse direction and move back to
* the start. This can also be useful when working with LED strips connected in a serpentine
* pattern (where the start of one strip is connected to the end of the previous one); however,
* consider using a {@link AddressableLEDBufferView#reversed() reversed view} of the overall
* buffer for that segment rather than reversing patterns.
*
* @return the reverse pattern
* @see AddressableLEDBufferView#reversed()
@@ -219,14 +219,14 @@ public interface LEDPattern {
*
* <pre>
* LEDPattern rainbow = LEDPattern.rainbow(255, 255);
* LEDPattern scrollingRainbow = rainbow.scrollAtRelativeSpeed(Percent.per(Second).of(25));
* LEDPattern scrollingRainbow = rainbow.scrollAtRelativeVelocity(Percent.per(Second).of(25));
* </pre>
*
* @param velocity how fast the pattern should move, in terms of how long it takes to do a full
* scroll along the length of LEDs and return back to the starting position
* @return the scrolling pattern
*/
default LEDPattern scrollAtRelativeSpeed(Frequency velocity) {
default LEDPattern scrollAtRelativeVelocity(Frequency velocity) {
final double periodMicros = velocity.asPeriod().in(Microseconds);
return mapIndex(
@@ -254,7 +254,7 @@ public interface LEDPattern {
*
* LEDPattern rainbow = LEDPattern.rainbow();
* LEDPattern scrollingRainbow =
* rainbow.scrollAtAbsoluteSpeed(InchesPerSecond.of(4), LED_SPACING);
* rainbow.scrollAtAbsoluteVelocity(InchesPerSecond.of(4), LED_SPACING);
* </pre>
*
* <p>Note that this pattern will scroll <i>faster</i> if applied to a less dense LED strip (such
@@ -265,7 +265,7 @@ public interface LEDPattern {
* @param ledSpacing the distance between adjacent LEDs on the physical LED strip
* @return the scrolling pattern
*/
default LEDPattern scrollAtAbsoluteSpeed(LinearVelocity velocity, Distance ledSpacing) {
default LEDPattern scrollAtAbsoluteVelocity(LinearVelocity velocity, Distance ledSpacing) {
// eg velocity = 10 m/s, spacing = 0.01m
// meters per micro = 1e-5 m/us
// micros per LED = 1e-2 m / (1e-5 m/us) = 1e-3 us

52
wpilibj/src/main/java/org/wpilib/hardware/motor/MotorController.java
View File

@@ -12,63 +12,65 @@ import org.wpilib.units.measure.Voltage;
/** Interface for motor controlling devices. */
public interface MotorController {
/**
* Common interface for setting the speed of a motor controller.
* Sets the duty cycle of the motor controller.
*
* @param speed The speed to set. Value should be between -1.0 and 1.0.
* @param dutyCycle The duty cycle between -1 and 1 (sign indicates direction).
*/
void set(double speed);
void setDutyCycle(double dutyCycle);
/**
* Sets the voltage output of the MotorController. Compensates for the current bus voltage to
* ensure that the desired voltage is output even if the battery voltage is below 12V - highly
* useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
* calculation).
* Sets the voltage output of the motor controller.
*
* <p>Compensates for the current bus voltage to ensure that the desired voltage is output even if
* the battery voltage is below 12V - highly useful when the voltage outputs are "meaningful"
* (e.g. they come from a feedforward calculation).
*
* <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
* properly - unlike the ordinary set function, it is not "set it and forget it."
*
* @param outputVolts The voltage to output, in Volts.
* @param voltage The voltage.
*/
default void setVoltage(double outputVolts) {
set(outputVolts / RobotController.getBatteryVoltage());
default void setVoltage(double voltage) {
setDutyCycle(voltage / RobotController.getBatteryVoltage());
}
/**
* Sets the voltage output of the MotorController. Compensates for the current bus voltage to
* ensure that the desired voltage is output even if the battery voltage is below 12V - highly
* useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
* calculation).
* Sets the voltage output of the motor controller.
*
* <p>Compensates for the current bus voltage to ensure that the desired voltage is output even if
* the battery voltage is below 12V - highly useful when the voltage outputs are "meaningful"
* (e.g. they come from a feedforward calculation).
*
* <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
* properly - unlike the ordinary set function, it is not "set it and forget it."
*
* @param outputVoltage The voltage to output.
* @param voltage The voltage.
*/
default void setVoltage(Voltage outputVoltage) {
setVoltage(outputVoltage.in(Volts));
default void setVoltage(Voltage voltage) {
setVoltage(voltage.in(Volts));
}
/**
* Common interface for getting the current set speed of a motor controller.
* Gets the duty cycle of the motor controller.
*
* @return The current set speed. Value is between -1.0 and 1.0.
* @return The duty cycle between -1 and 1 (sign indicates direction).
*/
double get();
double getDutyCycle();
/**
* Common interface for inverting direction of a motor controller.
* Sets the inversion state of the motor controller.
*
* @param isInverted The state of inversion true is inverted.
* @param isInverted The inversion state.
*/
void setInverted(boolean isInverted);
/**
* Common interface for returning if a motor controller is in the inverted state or not.
* Gets the inversion state of the motor controller.
*
* @return isInverted The state of the inversion true is inverted.
* @return The inversion state.
*/
boolean getInverted();
/** Disable the motor controller. */
/** Disables the motor controller. */
void disable();
}

71
wpilibj/src/main/java/org/wpilib/hardware/motor/PWMMotorController.java
View File

@@ -25,7 +25,7 @@ public abstract class PWMMotorController extends MotorSafety
protected PWM m_pwm;
private SimDevice m_simDevice;
private SimDouble m_simSpeed;
private SimDouble m_simDutyCycle;
private boolean m_eliminateDeadband;
private int m_minPwm;
@@ -48,7 +48,7 @@ public abstract class PWMMotorController extends MotorSafety
m_simDevice = SimDevice.create("PWMMotorController", channel);
if (m_simDevice != null) {
m_simSpeed = m_simDevice.createDouble("Speed", Direction.kOutput, 0.0);
m_simDutyCycle = m_simDevice.createDouble("DutyCycle", Direction.kOutput, 0.0);
m_pwm.setSimDevice(m_simDevice);
}
}
@@ -62,7 +62,7 @@ public abstract class PWMMotorController extends MotorSafety
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
m_simSpeed = null;
m_simDutyCycle = null;
}
}
@@ -91,39 +91,41 @@ public abstract class PWMMotorController extends MotorSafety
}
/**
* Takes a speed from -1 to 1, and outputs it in the microsecond format.
* Takes a duty cycle from -1 to 1 (the sign indicates direction), and outputs it in the
* microsecond format.
*
* @param speed the speed to output
* @param dutyCycle The duty cycle between -1 and 1 (sign indicates direction).
*/
protected final void setSpeed(double speed) {
if (Double.isFinite(speed)) {
speed = Math.clamp(speed, -1.0, 1.0);
protected final void setDutyCycleInternal(double dutyCycle) {
if (Double.isFinite(dutyCycle)) {
dutyCycle = Math.clamp(dutyCycle, -1.0, 1.0);
} else {
speed = 0.0;
dutyCycle = 0.0;
}
if (m_simSpeed != null) {
m_simSpeed.set(speed);
if (m_simDutyCycle != null) {
m_simDutyCycle.set(dutyCycle);
}
int rawValue;
if (speed == 0.0) {
if (dutyCycle == 0.0) {
rawValue = m_centerPwm;
} else if (speed > 0.0) {
rawValue = (int) Math.round(speed * getPositiveScaleFactor()) + getMinPositivePwm();
} else if (dutyCycle > 0.0) {
rawValue = (int) Math.round(dutyCycle * getPositiveScaleFactor()) + getMinPositivePwm();
} else {
rawValue = (int) Math.round(speed * getNegativeScaleFactor()) + getMaxNegativePwm();
rawValue = (int) Math.round(dutyCycle * getNegativeScaleFactor()) + getMaxNegativePwm();
}
m_pwm.setPulseTimeMicroseconds(rawValue);
}
/**
* Gets the speed from -1 to 1, from the currently set pulse time.
* Gets the duty cycle from -1 to 1 (the sign indicates direction), from the currently set pulse
* time.
*
* @return motor controller speed
* @return motor controller duty cycle
*/
protected final double getSpeed() {
protected final double getDutyCycleInternal() {
int rawValue = m_pwm.getPulseTimeMicroseconds();
if (rawValue == 0) {
@@ -159,36 +161,23 @@ public abstract class PWMMotorController extends MotorSafety
m_minPwm = minPwm;
}
/**
* Set the PWM value.
*
* <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
* FPGA.
*
* @param speed The speed value between -1.0 and 1.0 to set.
*/
@Override
public void set(double speed) {
public void setDutyCycle(double dutyCycle) {
if (m_isInverted) {
speed = -speed;
dutyCycle = -dutyCycle;
}
setSpeed(speed);
setDutyCycleInternal(dutyCycle);
for (var follower : m_followers) {
follower.set(speed);
follower.setDutyCycle(dutyCycle);
}
feed();
}
/**
* Get the recently set value of the PWM. This value is affected by the inversion property.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
@Override
public double get() {
return getSpeed() * (m_isInverted ? -1.0 : 1.0);
public double getDutyCycle() {
return getDutyCycleInternal() * (m_isInverted ? -1.0 : 1.0);
}
/**
@@ -197,7 +186,7 @@ public abstract class PWMMotorController extends MotorSafety
* @return The voltage of the motor controller, nominally between -12 V and 12 V.
*/
public double getVoltage() {
return get() * RobotController.getBatteryVoltage();
return getDutyCycle() * RobotController.getBatteryVoltage();
}
@Override
@@ -214,8 +203,8 @@ public abstract class PWMMotorController extends MotorSafety
public void disable() {
m_pwm.setDisabled();
if (m_simSpeed != null) {
m_simSpeed.set(0.0);
if (m_simDutyCycle != null) {
m_simDutyCycle.set(0.0);
}
for (var follower : m_followers) {
@@ -275,6 +264,6 @@ public abstract class PWMMotorController extends MotorSafety
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Motor Controller");
builder.setActuator(true);
builder.addDoubleProperty("Value", this::get, this::set);
builder.addDoubleProperty("Value", this::getDutyCycle, this::setDutyCycle);
}
}

2
wpilibj/src/main/java/org/wpilib/hardware/rotation/Encoder.java
View File

@@ -351,7 +351,7 @@ public class Encoder implements CounterBase, Sendable, AutoCloseable {
builder.setSmartDashboardType("Encoder");
}
builder.addDoubleProperty("Speed", this::getRate, null);
builder.addDoubleProperty("Velocity", this::getRate, null);
builder.addDoubleProperty("Distance", this::getDistance, null);
builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null);
}

12
wpilibj/src/main/java/org/wpilib/simulation/PWMMotorControllerSim.java
View File

@@ -9,7 +9,7 @@ import org.wpilib.hardware.motor.PWMMotorController;
/** Class to control a simulated PWM motor controller. */
public class PWMMotorControllerSim {
private final SimDouble m_simSpeed;
private final SimDouble m_simDutyCycle;
/**
* Constructor.
@@ -27,15 +27,15 @@ public class PWMMotorControllerSim {
*/
public PWMMotorControllerSim(int channel) {
SimDeviceSim simDevice = new SimDeviceSim("PWMMotorController", channel);
m_simSpeed = simDevice.getDouble("Speed");
m_simDutyCycle = simDevice.getDouble("DutyCycle");
}
/**
* Gets the motor speed set.
* Gets the motor duty cycle.
*
* @return Speed
* @return Duty cycle
*/
public double getSpeed() {
return m_simSpeed.get();
public double getDutyCycle() {
return m_simDutyCycle.get();
}
}