Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51: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

44
robotpyExamples/MecanumBot/drivetrain.py
View File

@@ -12,8 +12,8 @@ import wpimath
class Drivetrain:
"""Represents a mecanum drive style drivetrain."""
kMaxSpeed = 3.0 # 3 meters per second
kMaxAngularSpeed = math.pi # 1/2 rotation per second
kMaxVelocity = 3.0 # 3 meters per second
kMaxAngularVelocity = math.pi # 1/2 rotation per second
def __init__(self) -> None:
self.frontLeftMotor = wpilib.PWMSparkMax(1)
@@ -60,9 +60,9 @@ class Drivetrain:
self.frontRightMotor.setInverted(True)
self.backRightMotor.setInverted(True)
def getCurrentState(self) -> wpimath.MecanumDriveWheelSpeeds:
def getCurrentState(self) -> wpimath.MecanumDriveWheelVelocities:
"""Returns the current state of the drivetrain."""
return wpimath.MecanumDriveWheelSpeeds(
return wpimath.MecanumDriveWheelVelocities(
self.frontLeftEncoder.getRate(),
self.frontRightEncoder.getRate(),
self.backLeftEncoder.getRate(),
@@ -78,24 +78,24 @@ class Drivetrain:
positions.rearRight = self.backRightEncoder.getDistance()
return positions
def setSpeeds(self, speeds: wpimath.MecanumDriveWheelSpeeds) -> None:
"""Sets the desired speeds for each wheel."""
frontLeftFeedforward = self.feedforward.calculate(speeds.frontLeft)
frontRightFeedforward = self.feedforward.calculate(speeds.frontRight)
backLeftFeedforward = self.feedforward.calculate(speeds.rearLeft)
backRightFeedforward = self.feedforward.calculate(speeds.rearRight)
def setVelocities(self, velocities: wpimath.MecanumDriveWheelVelocities) -> None:
"""Sets the desired velocities for each wheel."""
frontLeftFeedforward = self.feedforward.calculate(velocities.frontLeft)
frontRightFeedforward = self.feedforward.calculate(velocities.frontRight)
backLeftFeedforward = self.feedforward.calculate(velocities.rearLeft)
backRightFeedforward = self.feedforward.calculate(velocities.rearRight)
frontLeftOutput = self.frontLeftPIDController.calculate(
self.frontLeftEncoder.getRate(), speeds.frontLeft
self.frontLeftEncoder.getRate(), velocities.frontLeft
)
frontRightOutput = self.frontRightPIDController.calculate(
self.frontRightEncoder.getRate(), speeds.frontRight
self.frontRightEncoder.getRate(), velocities.frontRight
)
backLeftOutput = self.frontLeftPIDController.calculate(
self.backLeftEncoder.getRate(), speeds.rearLeft
self.backLeftEncoder.getRate(), velocities.rearLeft
)
backRightOutput = self.frontRightPIDController.calculate(
self.backRightEncoder.getRate(), speeds.rearRight
self.backRightEncoder.getRate(), velocities.rearRight
)
self.frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward)
@@ -105,21 +105,21 @@ class Drivetrain:
def drive(
self,
xSpeed: float,
ySpeed: float,
xVelocity: float,
yVelocity: float,
rot: float,
fieldRelative: bool,
periodSeconds: float,
) -> None:
"""Method to drive the robot using joystick info."""
chassisSpeeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot)
chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
if fieldRelative:
chassisSpeeds = chassisSpeeds.toRobotRelative(self.imu.getRotation2d())
chassisVelocities = chassisVelocities.toRobotRelative(self.imu.getRotation2d())
self.setSpeeds(
self.kinematics.toWheelSpeeds(
chassisSpeeds.discretize(periodSeconds)
).desaturate(self.kMaxSpeed)
self.setVelocities(
self.kinematics.toWheelVelocities(
chassisVelocities.discretize(periodSeconds)
).desaturate(self.kMaxVelocity)
)
def updateOdometry(self) -> None: