Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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

24
robotpyExamples/SwerveBot/drivetrain.py
View File

@@ -9,8 +9,8 @@ import wpilib
import swervemodule
import wpimath
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
class Drivetrain:
@@ -53,29 +53,29 @@ 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.
:param xSpeed: Speed of the robot in the x direction (forward).
:param ySpeed: Speed of the robot in the y direction (sideways).
:param xVelocity: Velocity of the robot in the x direction (forward).
:param yVelocity: Velocity of the robot in the y direction (sideways).
:param rot: Angular rate of the robot.
:param fieldRelative: Whether the provided x and y speeds are relative to the field.
:param fieldRelative: Whether the provided x and y velocities are relative to the field.
:param periodSeconds: Time
"""
robot_speeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot)
robot_velocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
if fieldRelative:
robot_speeds = robot_speeds.toRobotRelative(self.imu.getRotation2d())
robot_velocities = robot_velocities.toRobotRelative(self.imu.getRotation2d())
swerveModuleStates = self.kinematics.toSwerveModuleStates(
wpimath.ChassisSpeeds.discretize(robot_speeds, periodSeconds)
wpimath.ChassisVelocities.discretize(robot_velocities, periodSeconds)
)
wpimath.SwerveDrive4Kinematics.desaturateWheelSpeeds(
swerveModuleStates, kMaxSpeed
wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
swerveModuleStates, kMaxVelocity
)
self.frontLeft.setDesiredState(swerveModuleStates[0])
self.frontRight.setDesiredState(swerveModuleStates[1])

24
robotpyExamples/SwerveBot/robot.py
View File

@@ -18,8 +18,8 @@ class MyRobot(wpilib.TimedRobot):
self.swerve = drivetrain.Drivetrain()
# Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
self.xspeedLimiter = wpimath.SlewRateLimiter(3)
self.yspeedLimiter = wpimath.SlewRateLimiter(3)
self.xvelocityLimiter = wpimath.SlewRateLimiter(3)
self.yvelocityLimiter = wpimath.SlewRateLimiter(3)
self.rotLimiter = wpimath.SlewRateLimiter(3)
def autonomousPeriodic(self) -> None:
@@ -30,23 +30,23 @@ class MyRobot(wpilib.TimedRobot):
self.driveWithJoystick(True)
def driveWithJoystick(self, fieldRelative: bool) -> None:
# Get the x speed. We are inverting this because Xbox controllers return
# Get the x velocity. We are inverting this because Xbox controllers return
# negative values when we push forward.
xSpeed = (
-self.xspeedLimiter.calculate(
xVelocity = (
-self.xvelocityLimiter.calculate(
wpimath.applyDeadband(self.controller.getLeftY(), 0.02)
)
* drivetrain.kMaxSpeed
* drivetrain.kMaxVelocity
)
# Get the y speed or sideways/strafe speed. We are inverting this because
# Get the y velocity or sideways/strafe velocity. We are inverting this because
# we want a positive value when we pull to the left. Xbox controllers
# return positive values when you pull to the right by default.
ySpeed = (
-self.yspeedLimiter.calculate(
yVelocity = (
-self.yvelocityLimiter.calculate(
wpimath.applyDeadband(self.controller.getLeftX(), 0.02)
)
* drivetrain.kMaxSpeed
* drivetrain.kMaxVelocity
)
# Get the rate of angular rotation. We are inverting this because we want a
@@ -57,7 +57,7 @@ class MyRobot(wpilib.TimedRobot):
-self.rotLimiter.calculate(
wpimath.applyDeadband(self.controller.getRightX(), 0.02)
)
* drivetrain.kMaxAngularSpeed
* drivetrain.kMaxAngularVelocity
)
self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod())
self.swerve.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

8
robotpyExamples/SwerveBot/swervemodule.py
View File

@@ -99,7 +99,7 @@ class SwerveModule:
def setDesiredState(self, desiredState: wpimath.SwerveModuleState) -> None:
"""Sets the desired state for the module.
:param desiredState: Desired state with speed and angle.
:param desiredState: Desired state with velocity and angle.
"""
encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
@@ -107,17 +107,17 @@ class SwerveModule:
# Optimize the reference state to avoid spinning further than 90 degrees
desiredState.optimize(encoderRotation)
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
# Scale velocity by cosine of angle error. This scales down movement perpendicular to the desired
# direction of travel that can occur when modules change directions. This results in smoother
# driving.
desiredState.cosineScale(encoderRotation)
# Calculate the drive output from the drive PID controller.
driveOutput = self.drivePIDController.calculate(
self.driveEncoder.getRate(), desiredState.speed
self.driveEncoder.getRate(), desiredState.velocity
)
driveFeedforward = self.driveFeedforward.calculate(desiredState.speed)
driveFeedforward = self.driveFeedforward.calculate(desiredState.velocity)
# Calculate the turning motor output from the turning PID controller.
turnOutput = self.turningPIDController.calculate(