Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

4
robotpyExamples/AddressableLED/robot.py
View File

@@ -32,8 +32,8 @@ class MyRobot(wpilib.TimedRobot):
self.kLedSpacing = 1 / 120.0
# Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a
# speed of 1 meter per second.
self.scrollingRainbow = self.rainbow.scrollAtAbsoluteSpeed(
# velocity of 1 meter per second.
self.scrollingRainbow = self.rainbow.scrollAtAbsoluteVelocity(
1,
self.kLedSpacing,
)

28
robotpyExamples/DifferentialDriveBot/drivetrain.py
View File

@@ -12,8 +12,8 @@ import wpimath
class Drivetrain:
"""Represents a differential drive style drivetrain."""
kMaxSpeed = 3.0 # meters per second
kMaxAngularSpeed = 2 * math.pi # one rotation per second
kMaxVelocity = 3.0 # meters per second
kMaxAngularVelocity = 2 * math.pi # one rotation per second
kTrackwidth = 0.381 * 2 # meters
kWheelRadius = 0.0508 # meters
@@ -68,35 +68,35 @@ class Drivetrain:
self.rightEncoder.getDistance(),
)
def setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None:
"""Sets the desired wheel speeds.
def setVelocities(self, velocities: wpimath.DifferentialDriveWheelVelocities) -> None:
"""Sets the desired wheel velocities.
:param speeds: The desired wheel speeds.
:param velocities: The desired wheel velocities.
"""
leftFeedforward = self.feedforward.calculate(speeds.left)
rightFeedforward = self.feedforward.calculate(speeds.right)
leftFeedforward = self.feedforward.calculate(velocities.left)
rightFeedforward = self.feedforward.calculate(velocities.right)
leftOutput = self.leftPIDController.calculate(
self.leftEncoder.getRate(), speeds.left
self.leftEncoder.getRate(), velocities.left
)
rightOutput = self.rightPIDController.calculate(
self.rightEncoder.getRate(), speeds.right
self.rightEncoder.getRate(), velocities.right
)
# Controls the left and right sides of the robot using the calculated outputs
self.leftLeader.setVoltage(leftOutput + leftFeedforward)
self.rightLeader.setVoltage(rightOutput + rightFeedforward)
def drive(self, xSpeed: float, rot: float) -> None:
def drive(self, xVelocity: float, rot: float) -> None:
"""Drives the robot with the given linear velocity and angular velocity.
:param xSpeed: Linear velocity in m/s.
:param xVelocity: Linear velocity in m/s.
:param rot: Angular velocity in rad/s.
"""
wheelSpeeds = self.kinematics.toWheelSpeeds(
wpimath.ChassisSpeeds(xSpeed, 0.0, rot)
wheelVelocities = self.kinematics.toWheelVelocities(
wpimath.ChassisVelocities(xVelocity, 0.0, rot)
)
self.setSpeeds(wheelSpeeds)
self.setVelocities(wheelVelocities)
def updateOdometry(self) -> None:
"""Updates the field-relative position."""

14
robotpyExamples/DifferentialDriveBot/robot.py
View File

@@ -19,7 +19,7 @@ class MyRobot(wpilib.TimedRobot):
self.drive = Drivetrain()
# Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
self.speedLimiter = wpimath.SlewRateLimiter(3)
self.velocityLimiter = wpimath.SlewRateLimiter(3)
self.rotLimiter = wpimath.SlewRateLimiter(3)
def autonomousPeriodic(self) -> None:
@@ -27,11 +27,11 @@ class MyRobot(wpilib.TimedRobot):
self.drive.updateOdometry()
def teleopPeriodic(self) -> 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.speedLimiter.calculate(self.controller.getLeftY())
* Drivetrain.kMaxSpeed
xVelocity = (
-self.velocityLimiter.calculate(self.controller.getLeftY())
* Drivetrain.kMaxVelocity
)
# Get the rate of angular rotation. We are inverting this because we want a
@@ -40,7 +40,7 @@ class MyRobot(wpilib.TimedRobot):
# the right by default.
rot = (
-self.rotLimiter.calculate(self.controller.getRightX())
* Drivetrain.kMaxAngularSpeed
* Drivetrain.kMaxAngularVelocity
)
self.drive.drive(xSpeed, rot)
self.drive.drive(xVelocity, rot)

28
robotpyExamples/DifferentialDrivePoseEstimator/drivetrain.py
View File

@@ -17,8 +17,8 @@ import robotpy_apriltag
class Drivetrain:
"""Represents a differential drive style drivetrain."""
kMaxSpeed = 3.0 # meters per second
kMaxAngularSpeed = math.tau # one rotation per second
kMaxVelocity = 3.0 # meters per second
kMaxAngularVelocity = math.tau # one rotation per second
kTrackwidth = 0.381 * 2 # meters
kWheelRadius = 0.0508 # meters
@@ -111,33 +111,33 @@ class Drivetrain:
wpilib.SmartDashboard.putData("Field", self.fieldSim)
wpilib.SmartDashboard.putData("FieldEstimation", self.fieldApproximation)
def setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None:
"""Sets the desired wheel speeds.
def setVelocities(self, velocities: wpimath.DifferentialDriveWheelVelocities) -> None:
"""Sets the desired wheel velocities.
:param speeds: The desired wheel speeds.
:param velocities: The desired wheel velocities.
"""
leftFeedforward = self.feedforward.calculate(speeds.left)
rightFeedforward = self.feedforward.calculate(speeds.right)
leftFeedforward = self.feedforward.calculate(velocities.left)
rightFeedforward = self.feedforward.calculate(velocities.right)
leftOutput = self.leftPIDController.calculate(
self.leftEncoder.getRate(), speeds.left
self.leftEncoder.getRate(), velocities.left
)
rightOutput = self.rightPIDController.calculate(
self.rightEncoder.getRate(), speeds.right
self.rightEncoder.getRate(), velocities.right
)
self.leftLeader.setVoltage(leftOutput + leftFeedforward)
self.rightLeader.setVoltage(rightOutput + rightFeedforward)
def drive(self, xSpeed: float, rot: float) -> None:
def drive(self, xVelocity: float, rot: float) -> None:
"""Drives the robot with the given linear velocity and angular velocity.
:param xSpeed: Linear velocity in m/s.
:param xVelocity: Linear velocity in m/s.
:param rot: Angular velocity in rad/s.
"""
wheelSpeeds = self.kinematics.toWheelSpeeds(
wpimath.ChassisSpeeds(xSpeed, 0.0, rot)
wheelVelocities = self.kinematics.toWheelVelocities(
wpimath.ChassisVelocities(xVelocity, 0.0, rot)
)
self.setSpeeds(wheelSpeeds)
self.setVelocities(wheelVelocities)
def publishCameraToObject(
self,

12
robotpyExamples/DifferentialDrivePoseEstimator/robot.py
View File

@@ -23,7 +23,7 @@ class MyRobot(wpilib.TimedRobot):
self.drive = Drivetrain(self.doubleArrayTopic)
# Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
self.speedLimiter = wpimath.SlewRateLimiter(3)
self.velocityLimiter = wpimath.SlewRateLimiter(3)
self.rotLimiter = wpimath.SlewRateLimiter(3)
def autonomousPeriodic(self) -> None:
@@ -37,16 +37,16 @@ class MyRobot(wpilib.TimedRobot):
self.drive.periodic()
def teleopPeriodic(self) -> 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.speedLimiter.calculate(self.controller.getLeftY())
xSpeed *= Drivetrain.kMaxSpeed
xVelocity = -self.velocityLimiter.calculate(self.controller.getLeftY())
xVelocity *= Drivetrain.kMaxVelocity
# Get the rate of angular rotation. We are inverting this because we want a
# positive value when we pull to the left (remember, CCW is positive in
# mathematics). Xbox controllers return positive values when you pull to
# the right by default.
rot = -self.rotLimiter.calculate(self.controller.getRightX())
rot *= Drivetrain.kMaxAngularSpeed
rot *= Drivetrain.kMaxAngularVelocity
self.drive.drive(xSpeed, rot)
self.drive.drive(xVelocity, rot)

2
robotpyExamples/DriveDistanceOffboard/constants.py
View File

@@ -32,7 +32,7 @@ class DriveConstants:
kp = 1.0
kMaxSpeed = 3.0 # m/s
kMaxVelocity = 3.0 # m/s
kMaxAcceleration = 3.0 # m/s²

12
robotpyExamples/DriveDistanceOffboard/examplesmartmotorcontroller.py
View File

@@ -25,7 +25,7 @@ class ExampleSmartMotorController(wpilib.MotorController):
port: The port for the controller.
"""
super().__init__()
self._speed = 0.0
self._velocity = 0.0
self._inverted = False
self._leader = None
@@ -79,11 +79,11 @@ class ExampleSmartMotorController(wpilib.MotorController):
"""Resets the encoder to zero distance."""
pass
def set(self, speed: float) -> None:
self._speed = -speed if self._inverted else speed
def set(self, velocity: float) -> None:
self._velocity = -velocity if self._inverted else velocity
def get(self) -> float:
return self._speed
return self._velocity
def setInverted(self, isInverted: bool) -> None:
self._inverted = isInverted
@@ -92,7 +92,7 @@ class ExampleSmartMotorController(wpilib.MotorController):
return self._inverted
def disable(self) -> None:
self._speed = 0.0
self._velocity = 0.0
def stopMotor(self) -> None:
self._speed = 0.0
self._velocity = 0.0

10
robotpyExamples/DriveDistanceOffboard/robotcontainer.py
View File

@@ -25,10 +25,10 @@ class RobotContainer:
self.robotDrive = subsystems.drivesubsystem.DriveSubsystem()
# Retained command references
self.driveFullSpeed = commands2.cmd.runOnce(
self.driveFullVelocity = commands2.cmd.runOnce(
lambda: self.robotDrive.setMaxOutput(1), self.robotDrive
)
self.driveHalfSpeed = commands2.cmd.runOnce(
self.driveHalfVelocity = commands2.cmd.runOnce(
lambda: self.robotDrive.setMaxOutput(0.5), self.robotDrive
)
@@ -65,9 +65,9 @@ class RobotContainer:
# We can bind commands while retaining references to them in RobotContainer
# Drive at half speed when the bumper is held
self.driverController.rightBumper().onTrue(self.driveHalfSpeed).onFalse(
self.driveFullSpeed
# Drive at half velocity when the bumper is held
self.driverController.rightBumper().onTrue(self.driveHalfVelocity).onFalse(
self.driveFullVelocity
)
# Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds

2
robotpyExamples/DriveDistanceOffboard/subsystems/drivesubsystem.py
View File

@@ -63,7 +63,7 @@ class DriveSubsystem(commands2.Subsystem):
self.profile = wpimath.TrapezoidProfile(
wpimath.TrapezoidProfile.Constraints(
constants.DriveConstants.kMaxSpeed,
constants.DriveConstants.kMaxVelocity,
constants.DriveConstants.kMaxAcceleration,
)
)

2
robotpyExamples/ElevatorExponentialProfile/examplesmartmotorcontroller.py
View File

@@ -74,7 +74,7 @@ class ExampleSmartMotorController:
"""Resets the encoder to zero distance."""
pass
def set(self, speed: float) -> None:
def set(self, velocity: float) -> None:
pass
def get(self) -> float:

2
robotpyExamples/ElevatorExponentialSimulation/subsystems/elevator.py
View File

@@ -84,7 +84,7 @@ class Elevator:
# In this method, we update our simulation of what our elevator is doing
# First, we set our "inputs" (voltages)
self.elevatorSim.setInputVoltage(
self.motorSim.getSpeed() * wpilib.RobotController.getBatteryVoltage()
self.motorSim.getVelocity() * wpilib.RobotController.getBatteryVoltage()
)
# Next, we update it. The standard loop time is 20ms.

2
robotpyExamples/ElevatorSimulation/subsystems/elevator.py
View File

@@ -68,7 +68,7 @@ class Elevator:
# In this method, we update our simulation of what our elevator is doing
# First, we set our "inputs" (voltages)
self.elevatorSim.setInputVoltage(
self.motorSim.getSpeed() * wpilib.RobotController.getBatteryVoltage()
self.motorSim.getVelocity() * wpilib.RobotController.getBatteryVoltage()
)
# Next, we update it. The standard loop time is 20ms.

12
robotpyExamples/ElevatorTrapezoidProfile/examplesmartmotorcontroller.py
View File

@@ -25,7 +25,7 @@ class ExampleSmartMotorController(wpilib.MotorController):
port: The port for the controller.
"""
super().__init__()
self._speed = 0.0
self._velocity = 0.0
self._inverted = False
self._leader = None
@@ -79,11 +79,11 @@ class ExampleSmartMotorController(wpilib.MotorController):
"""Resets the encoder to zero distance."""
pass
def set(self, speed: float) -> None:
self._speed = -speed if self._inverted else speed
def set(self, velocity: float) -> None:
self._velocity = -velocity if self._inverted else velocity
def get(self) -> float:
return self._speed
return self._velocity
def setInverted(self, isInverted: bool) -> None:
self._inverted = isInverted
@@ -92,7 +92,7 @@ class ExampleSmartMotorController(wpilib.MotorController):
return self._inverted
def disable(self) -> None:
self._speed = 0.0
self._velocity = 0.0
def stopMotor(self) -> None:
self._speed = 0.0
self._velocity = 0.0

4
robotpyExamples/FlywheelBangBangController/robot.py
View File

@@ -74,7 +74,7 @@ class MyRobot(wpilib.TimedRobot):
wpilib.SmartDashboard.putData(self.bangBangControler)
def teleopPeriodic(self) -> None:
"""Controls flywheel to a set speed (RPM) controlled by a joystick."""
"""Controls flywheel to a set velocity (RPM) controlled by a joystick."""
# Scale setpoint value between 0 and maxSetpointValue
setpoint = max(
@@ -91,7 +91,7 @@ class MyRobot(wpilib.TimedRobot):
)
# Controls a motor with the output of the BangBang controller and a
# feedforward. The feedforward is reduced slightly to avoid overspeeding
# feedforward. The feedforward is reduced slightly to avoid overvelocitying
# the shooter.
self.flywheelMotor.setVoltage(
bangOutput + 0.9 * self.feedforward.calculate(setpoint)

2
robotpyExamples/GettingStarted/robot.py
View File

@@ -35,7 +35,7 @@ class MyRobot(wpilib.TimedRobot):
# Drive for two seconds
if self.timer.get() < 2.0:
# Drive forwards half speed, make sure to turn input squaring off
# Drive forwards half velocity, make sure to turn input squaring off
self.robotDrive.arcadeDrive(0.5, 0, squareInputs=False)
else:
self.robotDrive.stopMotor() # Stop robot

2
robotpyExamples/Gyro/robot.py
View File

@@ -39,7 +39,7 @@ class MyRobot(wpilib.TimedRobot):
self.rightDrive.setInverted(True)
def teleopPeriodic(self) -> None:
# The motor speed is set from the joystick while the DifferentialDrive turning value is assigned
# The motor velocity is set from the joystick while the DifferentialDrive turning value is assigned
# from the error between the setpoint and the gyro angle.
turningValue = (
self.kAngleSetpoint - self.imu.getRotation2d().degrees()

6
robotpyExamples/HatchbotInlined/commands/autos.py
View File

@@ -26,7 +26,7 @@ class Autos:
# Reset encoders on command start
drive.resetEncoders,
# Drive forward while the command is executing,
lambda: drive.arcadeDrive(constants.kAutoDriveSpeed, 0),
lambda: drive.arcadeDrive(constants.kAutoDriveVelocity, 0),
# Stop driving at the end of the command
lambda interrupt: drive.arcadeDrive(0, 0),
# End the command when the robot's driven distance exceeds the desired value
@@ -47,7 +47,7 @@ class Autos:
# Reset encoders on command start
driveSubsystem.resetEncoders,
# Drive forward while the command is executing
lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0),
lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveVelocity, 0),
# Stop driving at the end of the command
lambda interrupt: driveSubsystem.arcadeDrive(0, 0),
# End the command when the robot's driven distance exceeds the desired value
@@ -63,7 +63,7 @@ class Autos:
# Reset encoders on command start
driveSubsystem.resetEncoders,
# Drive backwards while the command is executing
lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0),
lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveVelocity, 0),
# Stop driving at the end of the command
lambda interrupt: driveSubsystem.arcadeDrive(0, 0),
# End the command when the robot's driven distance exceeds the desired value

2
robotpyExamples/HatchbotInlined/constants.py
View File

@@ -37,7 +37,7 @@ kHatchSolenoidPorts = (0, 1)
# Autonomous
kAutoDriveDistanceInches = 60
kAutoBackupDistanceInches = 20
kAutoDriveSpeed = 0.5
kAutoDriveVelocity = 0.5
# Operator Interface
kDriverControllerPort = 0

2
robotpyExamples/HatchbotInlined/robotcontainer.py
View File

@@ -86,7 +86,7 @@ class RobotContainer:
# Release the hatch when the Square button is pressed.
self.driverController.square().onTrue(self.hatchSubsystem.releaseHatch())
# While holding R1, drive at half speed
# While holding R1, drive at half velocity
self.driverController.R1().onTrue(
commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(0.5))
).onFalse(commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(1)))

2
robotpyExamples/HatchbotInlined/subsystems/drivesubsystem.py
View File

@@ -20,7 +20,7 @@ class DriveSubsystem(commands2.Subsystem):
self.left1.addFollower(self.left2)
self.right1.addFollower(self.right2)
# We need to invert one side of the drivetrain so that positive speeds
# We need to invert one side of the drivetrain so that positive velocities
# result in both sides moving forward. Depending on how your robot's
# drivetrain is constructed, you might have to invert the left side instead.
self.right1.setInverted(True)

4
robotpyExamples/HatchbotTraditional/commands/complexauto.py
View File

@@ -24,12 +24,12 @@ class ComplexAuto(commands2.SequentialCommandGroup):
super().__init__(
# Drive forward the specified distance
DriveDistance(
constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, drive
constants.kAutoDriveDistanceInches, constants.kAutoDriveVelocity, drive
),
# Release the hatch
ReleaseHatch(hatch),
# Drive backward the specified distance
DriveDistance(
constants.kAutoBackupDistanceInches, -constants.kAutoDriveSpeed, drive
constants.kAutoBackupDistanceInches, -constants.kAutoDriveVelocity, drive
),
)

8
robotpyExamples/HatchbotTraditional/commands/drivedistance.py
View File

@@ -10,18 +10,18 @@ from subsystems.drivesubsystem import DriveSubsystem
class DriveDistance(commands2.Command):
def __init__(self, inches: float, speed: float, drive: DriveSubsystem) -> None:
def __init__(self, inches: float, velocity: float, drive: DriveSubsystem) -> None:
self.distance = inches
self.speed = speed
self.velocity = velocity
self.drive = drive
self.addRequirements(drive)
def initialize(self) -> None:
self.drive.resetEncoders()
self.drive.arcadeDrive(self.speed, 0)
self.drive.arcadeDrive(self.velocity, 0)
def execute(self) -> None:
self.drive.arcadeDrive(self.speed, 0)
self.drive.arcadeDrive(self.velocity, 0)
def end(self, interrupted: bool) -> None:
self.drive.arcadeDrive(0, 0)

2
robotpyExamples/HatchbotTraditional/commands/halvedrivespeed.py → robotpyExamples/HatchbotTraditional/commands/halvedrivevelocity.py
View File

@@ -9,7 +9,7 @@ import commands2
from subsystems.drivesubsystem import DriveSubsystem
class HalveDriveSpeed(commands2.Command):
class HalveDriveVelocity(commands2.Command):
def __init__(self, drive: DriveSubsystem) -> None:
self.drive = drive

2
robotpyExamples/HatchbotTraditional/constants.py
View File

@@ -37,7 +37,7 @@ kHatchSolenoidPorts = (0, 1)
# Autonomous
kAutoDriveDistanceInches = 60
kAutoBackupDistanceInches = 20
kAutoDriveSpeed = 0.5
kAutoDriveVelocity = 0.5
# Operator Interface
kDriverControllerPort = 0

6
robotpyExamples/HatchbotTraditional/robotcontainer.py
View File

@@ -15,7 +15,7 @@ from commands.complexauto import ComplexAuto
from commands.drivedistance import DriveDistance
from commands.defaultdrive import DefaultDrive
from commands.grabhatch import GrabHatch
from commands.halvedrivespeed import HalveDriveSpeed
from commands.halvedrivevelocity import HalveDriveVelocity
from commands.releasehatch import ReleaseHatch
from subsystems.drivesubsystem import DriveSubsystem
@@ -43,7 +43,7 @@ class RobotContainer:
# A simple auto routine that drives forward a specified distance, and then stops.
self.simpleAuto = DriveDistance(
constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, self.drive
constants.kAutoDriveDistanceInches, constants.kAutoDriveVelocity, self.drive
)
# A complex auto routine that drives forward, drops a hatch, and then drives backward.
@@ -86,7 +86,7 @@ class RobotContainer:
)
commands2.button.JoystickButton(self.driverController, 3).whileTrue(
HalveDriveSpeed(self.drive)
HalveDriveVelocity(self.drive)
)
def getAutonomousCommand(self) -> commands2.Command:

2
robotpyExamples/HatchbotTraditional/subsystems/drivesubsystem.py
View File

@@ -17,7 +17,7 @@ class DriveSubsystem(commands2.Subsystem):
self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port)
self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port)
# We need to invert one side of the drivetrain so that positive speeds
# We need to invert one side of the drivetrain so that positive velocities
# result in both sides moving forward. Depending on how your robot's
# drivetrain is constructed, you might have to invert the left side instead.
self.right1.setInverted(True)

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:

24
robotpyExamples/MecanumBot/robot.py
View File

@@ -19,8 +19,8 @@ class MyRobot(wpilib.TimedRobot):
self.mecanum = 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:
@@ -31,19 +31,19 @@ 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(self.controller.getLeftY())
* Drivetrain.kMaxSpeed
xVelocity = (
-self.xvelocityLimiter.calculate(self.controller.getLeftY())
* 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(self.controller.getLeftX())
* Drivetrain.kMaxSpeed
yVelocity = (
-self.yvelocityLimiter.calculate(self.controller.getLeftX())
* Drivetrain.kMaxVelocity
)
# Get the rate of angular rotation. We are inverting this because we want a
@@ -52,7 +52,7 @@ class MyRobot(wpilib.TimedRobot):
# the right by default.
rot = (
-self.rotLimiter.calculate(self.controller.getRightX())
* Drivetrain.kMaxAngularSpeed
* Drivetrain.kMaxAngularVelocity
)
self.mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod())
self.mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

50
robotpyExamples/MecanumDrivePoseEstimator/drivetrain.py
View File

@@ -16,8 +16,8 @@ from exampleglobalmeasurementsensor import ExampleGlobalMeasurementSensor
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)
@@ -71,12 +71,12 @@ 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.
:returns: The current state of the drivetrain.
"""
return wpimath.MecanumDriveWheelSpeeds(
return wpimath.MecanumDriveWheelVelocities(
self.frontLeftEncoder.getRate(),
self.frontRightEncoder.getRate(),
self.backLeftEncoder.getRate(),
@@ -95,27 +95,27 @@ class Drivetrain:
positions.rearRight = self.backRightEncoder.getDistance()
return positions
def setSpeeds(self, speeds: wpimath.MecanumDriveWheelSpeeds) -> None:
"""Set the desired speeds for each wheel.
def setVelocities(self, velocities: wpimath.MecanumDriveWheelVelocities) -> None:
"""Set the desired velocities for each wheel.
:param speeds: The desired wheel speeds.
:param velocities: The desired wheel velocities.
"""
frontLeftFeedforward = self.feedforward.calculate(speeds.frontLeft)
frontRightFeedforward = self.feedforward.calculate(speeds.frontRight)
backLeftFeedforward = self.feedforward.calculate(speeds.rearLeft)
backRightFeedforward = self.feedforward.calculate(speeds.rearRight)
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.backLeftPIDController.calculate(
self.backLeftEncoder.getRate(), speeds.rearLeft
self.backLeftEncoder.getRate(), velocities.rearLeft
)
backRightOutput = self.backRightPIDController.calculate(
self.backRightEncoder.getRate(), speeds.rearRight
self.backRightEncoder.getRate(), velocities.rearRight
)
self.frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward)
@@ -125,27 +125,27 @@ class Drivetrain:
def drive(
self,
xSpeed: float,
ySpeed: float,
xVelocity: float,
yVelocity: float,
rot: float,
fieldRelative: bool,
period: 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.
"""
chassisSpeeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot)
chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
if fieldRelative:
chassisSpeeds = chassisSpeeds.toRobotRelative(
chassisVelocities = chassisVelocities.toRobotRelative(
self.poseEstimator.getEstimatedPosition().rotation()
)
self.setSpeeds(
self.kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(
self.kMaxSpeed
self.setVelocities(
self.kinematics.toWheelVelocities(chassisVelocities.discretize(period)).desaturate(
self.kMaxVelocity
)
)

20
robotpyExamples/MecanumDrivePoseEstimator/robot.py
View File

@@ -19,8 +19,8 @@ class MyRobot(wpilib.TimedRobot):
self.mecanum = 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:
@@ -31,22 +31,22 @@ 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(self.controller.getLeftY())
xSpeed *= Drivetrain.kMaxSpeed
xVelocity = -self.xvelocityLimiter.calculate(self.controller.getLeftY())
xVelocity *= 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(self.controller.getLeftX())
ySpeed *= Drivetrain.kMaxSpeed
yVelocity = -self.yvelocityLimiter.calculate(self.controller.getLeftX())
yVelocity *= Drivetrain.kMaxVelocity
# Get the rate of angular rotation. We are inverting this because we want a
# positive value when we pull to the left (remember, CCW is positive in
# mathematics). Xbox controllers return positive values when you pull to
# the right by default.
rot = -self.rotLimiter.calculate(self.controller.getRightX())
rot *= Drivetrain.kMaxAngularSpeed
rot *= Drivetrain.kMaxAngularVelocity
self.mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod())
self.mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

2
robotpyExamples/PotentiometerPID/robot.py
View File

@@ -25,7 +25,7 @@ class MyRobot(wpilib.TimedRobot):
# Bottom, middle, and top elevator setpoints
kSetpointMeters = [0.2, 0.8, 1.4]
# proportional, integral, and derivative speed constants
# proportional, integral, and derivative velocity constants
# DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
# and kD may cause dangerous, uncontrollable, or undesired behavior!
kP = 0.7

6
robotpyExamples/RapidReactCommandBot/constants.py
View File

@@ -63,10 +63,10 @@ class ShooterConstants:
# On a real robot the feedforward constants should be empirically determined; these are
# reasonable guesses.
kS = 0.05 # V
# Should have value 12V at free speed
# Should have value 12V at free velocity
kV = 12.0 / kShooterFreeRPS # V/(rot/s)
kFeederSpeed = 0.5
kFeederVelocity = 0.5
class IntakeConstants:
@@ -82,7 +82,7 @@ class StorageConstants:
class AutoConstants:
kTimeout = 3
kDriveDistance = 2.0 # m
kDriveSpeed = 0.5
kDriveVelocity = 0.5
class OIConstants:

4
robotpyExamples/RapidReactCommandBot/rapidreactcommandbot.py
View File

@@ -82,7 +82,7 @@ class RapidReactCommandBot:
Scheduled during :meth:`.Robot.autonomousInit`.
"""
# Drive forward for 2 meters at half speed with a 3 second timeout
# Drive forward for 2 meters at half velocity with a 3 second timeout
return self.drive.driveDistanceCommand(
AutoConstants.kDriveDistance, AutoConstants.kDriveSpeed
AutoConstants.kDriveDistance, AutoConstants.kDriveVelocity
).withTimeout(AutoConstants.kTimeout)

8
robotpyExamples/RapidReactCommandBot/subsystems/drive.py
View File

@@ -91,12 +91,12 @@ class Drive(Subsystem):
"arcadeDrive"
)
def driveDistanceCommand(self, distance: float, speed: float) -> Command:
def driveDistanceCommand(self, distance: float, velocity: float) -> Command:
"""Returns a command that drives the robot forward a specified distance at a specified
speed.
velocity.
:param distance: The distance to drive forward in meters
:param speed: The fraction of max speed at which to drive
:param velocity: The fraction of max velocity at which to drive
"""
return (
self.runOnce(
@@ -105,7 +105,7 @@ class Drive(Subsystem):
self.rightEncoder.reset(),
)
)
.andThen(self.run(lambda: self.drive.arcadeDrive(speed, 0)))
.andThen(self.run(lambda: self.drive.arcadeDrive(velocity, 0)))
.until(
lambda: max(
self.leftEncoder.getDistance(), self.rightEncoder.getDistance()

6
robotpyExamples/RomiReference/commands/arcadedrive.py
View File

@@ -16,12 +16,12 @@ class ArcadeDrive(commands2.Command):
forward: typing.Callable[[], float],
rotation: typing.Callable[[], float],
) -> None:
"""Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier
"""Creates a new ArcadeDrive. This command will drive your robot according to the velocity supplier
lambdas. This command does not terminate.
:param drivetrain: The drivetrain subsystem on which this command will run
:param forward: Callable supplier of forward/backward speed
:param rotation: Callable supplier of rotational speed
:param forward: Callable supplier of forward/backward velocity
:param rotation: Callable supplier of rotational velocity
"""
self.drive = drive

10
robotpyExamples/RomiReference/commands/drivedistance.py
View File

@@ -10,17 +10,17 @@ from subsystems.drivetrain import Drivetrain
class DriveDistance(commands2.Command):
def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None:
def __init__(self, velocity: float, inches: float, drive: Drivetrain) -> None:
"""Creates a new DriveDistance. This command will drive your your robot for a desired distance at
a desired speed.
a desired velocity.
:param speed: The speed at which the robot will drive
:param velocity: The velocity at which the robot will drive
:param inches: The number of inches the robot will drive
:param drive: The drivetrain subsystem on which this command will run
"""
self.distance = inches
self.speed = speed
self.velocity = velocity
self.drive = drive
self.addRequirements(drive)
@@ -31,7 +31,7 @@ class DriveDistance(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(self.speed, 0)
self.drive.arcadeDrive(self.velocity, 0)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

12
robotpyExamples/RomiReference/commands/drivetime.py
View File

@@ -11,17 +11,17 @@ from subsystems.drivetrain import Drivetrain
class DriveTime(commands2.Command):
"""Creates a new DriveTime. This command will drive your robot for a desired speed and time."""
"""Creates a new DriveTime. This command will drive your robot for a desired velocity and time."""
def __init__(self, speed: float, time: float, drive: Drivetrain) -> None:
"""Creates a new DriveTime. This command will drive your robot for a desired speed and time.
def __init__(self, velocity: float, time: float, drive: Drivetrain) -> None:
"""Creates a new DriveTime. This command will drive your robot for a desired velocity and time.
:param speed: The speed which the robot will drive. Negative is in reverse.
:param velocity: The velocity which the robot will drive. Negative is in reverse.
:param time: How much time to drive in seconds
:param drive: The drivetrain subsystem on which this command will run
"""
self.speed = speed
self.velocity = velocity
self.duration = time
self.drive = drive
self.addRequirements(drive)
@@ -35,7 +35,7 @@ class DriveTime(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(self.speed, 0)
self.drive.arcadeDrive(self.velocity, 0)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

10
robotpyExamples/RomiReference/commands/turndegrees.py
View File

@@ -11,17 +11,17 @@ from subsystems.drivetrain import Drivetrain
class TurnDegrees(commands2.Command):
def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None:
def __init__(self, velocity: float, degrees: float, drive: Drivetrain) -> None:
"""Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
degrees) and rotational speed.
degrees) and rotational velocity.
:param speed: The speed which the robot will drive. Negative is in reverse.
:param velocity: The velocity which the robot will drive. Negative is in reverse.
:param degrees: Degrees to turn. Leverages encoders to compare distance.
:param drive: The drive subsystem on which this command will run
"""
self.degrees = degrees
self.speed = speed
self.velocity = velocity
self.drive = drive
self.addRequirements(drive)
@@ -33,7 +33,7 @@ class TurnDegrees(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(0, self.speed)
self.drive.arcadeDrive(0, self.velocity)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

10
robotpyExamples/RomiReference/commands/turntime.py
View File

@@ -12,18 +12,18 @@ from subsystems.drivetrain import Drivetrain
class TurnTime(commands2.Command):
"""Creates a new TurnTime command. This command will turn your robot for a
desired rotational speed and time.
desired rotational velocity and time.
"""
def __init__(self, speed: float, time: float, drive: Drivetrain) -> None:
def __init__(self, velocity: float, time: float, drive: Drivetrain) -> None:
"""Creates a new TurnTime.
:param speed: The speed which the robot will turn. Negative is in reverse.
:param velocity: The velocity which the robot will turn. Negative is in reverse.
:param time: How much time to turn in seconds
:param drive: The drive subsystem on which this command will run
"""
self.rotationalSpeed = speed
self.rotationalVelocity = velocity
self.duration = time
self.drive = drive
self.addRequirements(drive)
@@ -37,7 +37,7 @@ class TurnTime(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(0, self.rotationalSpeed)
self.drive.arcadeDrive(0, self.rotationalVelocity)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

24
robotpyExamples/SimpleDifferentialDriveSimulation/drivetrain.py
View File

@@ -15,9 +15,9 @@ class Drivetrain:
"""Represents a differential drive style drivetrain."""
# 3 meters per second.
kMaxSpeed = 3.0
kMaxVelocity = 3.0
# 1/2 rotation per second.
kMaxAngularSpeed = math.pi
kMaxAngularVelocity = math.pi
kTrackwidth = 0.381 * 2
kWheelRadius = 0.0508
@@ -88,28 +88,28 @@ class Drivetrain:
self.rightLeader.setInverted(True)
wpilib.SmartDashboard.putData("Field", self.fieldSim)
def setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None:
"""Sets speeds to the drivetrain motors."""
leftFeedforward = self.feedforward.calculate(speeds.left)
rightFeedforward = self.feedforward.calculate(speeds.right)
def setVelocities(self, velocities: wpimath.DifferentialDriveWheelVelocities) -> None:
"""Sets velocities to the drivetrain motors."""
leftFeedforward = self.feedforward.calculate(velocities.left)
rightFeedforward = self.feedforward.calculate(velocities.right)
leftOutput = self.leftPIDController.calculate(
self.leftEncoder.getRate(), speeds.left
self.leftEncoder.getRate(), velocities.left
)
rightOutput = self.rightPIDController.calculate(
self.rightEncoder.getRate(), speeds.right
self.rightEncoder.getRate(), velocities.right
)
self.leftLeader.setVoltage(leftOutput + leftFeedforward)
self.rightLeader.setVoltage(rightOutput + rightFeedforward)
def drive(self, xSpeed: float, rot: float) -> None:
def drive(self, xVelocity: float, rot: float) -> None:
"""Controls the robot using arcade drive.
:param xSpeed: the speed for the x axis
:param xVelocity: the velocity for the x axis
:param rot: the rotation
"""
self.setSpeeds(
self.kinematics.toWheelSpeeds(wpimath.ChassisSpeeds(xSpeed, 0, rot))
self.setVelocities(
self.kinematics.toWheelVelocities(wpimath.ChassisVelocities(xVelocity, 0, rot))
)
def updateOdometry(self) -> None:

18
robotpyExamples/SimpleDifferentialDriveSimulation/robot.py
View File

@@ -18,7 +18,7 @@ class MyRobot(wpilib.TimedRobot):
# Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
# to 1.
self.speedLimiter = wpimath.SlewRateLimiter(3)
self.velocityLimiter = wpimath.SlewRateLimiter(3)
self.rotLimiter = wpimath.SlewRateLimiter(3)
self.drive = Drivetrain()
@@ -43,15 +43,15 @@ class MyRobot(wpilib.TimedRobot):
def autonomousPeriodic(self) -> None:
elapsed = self.timer.get()
reference = self.trajectory.sample(elapsed)
speeds = self.feedback.calculate(self.drive.getPose(), reference)
self.drive.drive(speeds.vx, speeds.omega)
velocities = self.feedback.calculate(self.drive.getPose(), reference)
self.drive.drive(velocities.vx, velocities.omega)
def teleopPeriodic(self) -> 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.speedLimiter.calculate(self.controller.getLeftY())
* Drivetrain.kMaxSpeed
xVelocity = (
-self.velocityLimiter.calculate(self.controller.getLeftY())
* Drivetrain.kMaxVelocity
)
# Get the rate of angular rotation. We are inverting this because we want a
@@ -60,9 +60,9 @@ class MyRobot(wpilib.TimedRobot):
# the right by default.
rot = (
-self.rotLimiter.calculate(self.controller.getRightX())
* Drivetrain.kMaxAngularSpeed
* Drivetrain.kMaxAngularVelocity
)
self.drive.drive(xSpeed, rot)
self.drive.drive(xVelocity, rot)
def simulationPeriodic(self) -> None:
self.drive.simulationPeriodic()

2
robotpyExamples/StateSpaceArm/robot.py
View File

@@ -35,7 +35,7 @@ class MyRobot(wpilib.TimedRobot):
self.profile = wpimath.TrapezoidProfile(
wpimath.TrapezoidProfile.Constraints(
wpimath.units.degreesToRadians(45),
wpimath.units.degreesToRadians(90), # Max arm speed and acceleration.
wpimath.units.degreesToRadians(90), # Max arm velocity and acceleration.
)
)

2
robotpyExamples/StateSpaceElevator/robot.py
View File

@@ -39,7 +39,7 @@ class MyRobot(wpilib.TimedRobot):
self.profile = wpimath.TrapezoidProfile(
wpimath.TrapezoidProfile.Constraints(
wpimath.units.feetToMeters(3.0),
wpimath.units.feetToMeters(6.0), # Max elevator speed and acceleration.
wpimath.units.feetToMeters(6.0), # Max elevator velocity and acceleration.
)
)

2
robotpyExamples/StateSpaceFlywheel/robot.py
View File

@@ -89,7 +89,7 @@ class MyRobot(wpilib.TimedRobot):
def teleopPeriodic(self) -> None:
# Sets the target speed of our flywheel. This is similar to setting the setpoint of a
# Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
# PID controller.
if self.joystick.getTriggerPressed():
# We just pressed the trigger, so let's set our next reference

2
robotpyExamples/StateSpaceFlywheelSysId/robot.py
View File

@@ -77,7 +77,7 @@ class MyRobot(wpilib.TimedRobot):
self.loop.reset([self.encoder.getRate()])
def teleopPeriodic(self) -> None:
# Sets the target speed of our flywheel. This is similar to setting the setpoint of a
# Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
# PID controller.
if self.joystick.getTriggerPressed():
# We just pressed the trigger, so let's set our next reference

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(

24
robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py
View File

@@ -17,8 +17,8 @@ from exampleglobalmeasurementsensor import ExampleGlobalMeasurementSensor
class Drivetrain:
"""Represents a swerve 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.frontLeftLocation = wpimath.Translation2d(0.381, 0.381)
@@ -60,29 +60,29 @@ class Drivetrain:
def drive(
self,
xSpeed: float,
ySpeed: float,
xVelocity: float,
yVelocity: float,
rot: float,
fieldRelative: bool,
period: 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.
"""
chassisSpeeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot)
chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
if fieldRelative:
chassisSpeeds = chassisSpeeds.toRobotRelative(
chassisVelocities = chassisVelocities.toRobotRelative(
self.poseEstimator.getEstimatedPosition().rotation()
)
chassisSpeeds = chassisSpeeds.discretize(period)
chassisVelocities = chassisVelocities.discretize(period)
states = self.kinematics.toSwerveModuleStates(chassisSpeeds)
wpimath.SwerveDrive4Kinematics.desaturateWheelSpeeds(states, self.kMaxSpeed)
states = self.kinematics.toSwerveModuleStates(chassisVelocities)
wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(states, self.kMaxVelocity)
self.frontLeft.setDesiredState(states[0])
self.frontRight.setDesiredState(states[1])

20
robotpyExamples/SwerveDrivePoseEstimator/robot.py
View File

@@ -19,8 +19,8 @@ class MyRobot(wpilib.TimedRobot):
self.swerve = 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:
@@ -31,22 +31,22 @@ 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(self.controller.getLeftY())
xSpeed *= Drivetrain.kMaxSpeed
xVelocity = -self.xvelocityLimiter.calculate(self.controller.getLeftY())
xVelocity *= 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(self.controller.getLeftX())
ySpeed *= Drivetrain.kMaxSpeed
yVelocity = -self.yvelocityLimiter.calculate(self.controller.getLeftX())
yVelocity *= Drivetrain.kMaxVelocity
# Get the rate of angular rotation. We are inverting this because we want a
# positive value when we pull to the left (remember, CCW is positive in
# mathematics). Xbox controllers return positive values when you pull to
# the right by default.
rot = -self.rotLimiter.calculate(self.controller.getRightX())
rot *= Drivetrain.kMaxAngularSpeed
rot *= Drivetrain.kMaxAngularVelocity
self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod())
self.swerve.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

8
robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py
View File

@@ -100,24 +100,24 @@ 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())
# 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
# 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(

6
robotpyExamples/SysId/constants.py
View File

@@ -46,11 +46,11 @@ class ShooterConstants:
# On a real robot the feedforward constants should be empirically determined; these are
# reasonable guesses.
kS = 0.05 # V
# Should have value 12V at free speed
# Should have value 12V at free velocity
kV = 12.0 / kShooterFreeRPS # V/(rot/s)
kA = 0.0 # V/(rot/s^2)
kFeederSpeed = 0.5
kFeederVelocity = 0.5
class IntakeConstants:
@@ -66,7 +66,7 @@ class StorageConstants:
class AutoConstants:
kTimeout = 3
kDriveDistance = 2.0 # m
kDriveSpeed = 0.5
kDriveVelocity = 0.5
class OIConstants:

16
robotpyExamples/SysId/subsystems/shooter.py
View File

@@ -77,23 +77,23 @@ class Shooter(Subsystem):
self.shooter_encoder.getRate()
)
def runShooter(self, shooterSpeed: Callable[[], float]) -> Command:
def runShooter(self, shooterVelocity: Callable[[], float]) -> Command:
"""Returns a command that runs the shooter at a specifc velocity.
:param shooterSpeed: The commanded shooter wheel speed in rotations per second
:param shooterVelocity: The commanded shooter wheel velocity in rotations per second
"""
# Run shooter wheel at the desired speed using a PID controller and feedforward.
# Run shooter wheel at the desired velocity using a PID controller and feedforward.
def _run_shooter() -> None:
target_speed = shooterSpeed()
target_speed_radians = wpimath.units.rotationsToRadians(target_speed)
target_velocity = shooterVelocity()
target_velocity_radians = wpimath.units.rotationsToRadians(target_velocity)
self.shooter_motor.setVoltage(
self.shooter_feedback.calculate(
self.shooter_encoder.getRate(), target_speed
self.shooter_encoder.getRate(), target_velocity
)
+ self.shooter_feedforward.calculate(target_speed_radians)
+ self.shooter_feedforward.calculate(target_velocity_radians)
)
self.feeder_motor.set(ShooterConstants.kFeederSpeed)
self.feeder_motor.set(ShooterConstants.kFeederVelocity)
def _stop_motors(interrupted: bool) -> None:
self.shooter_motor.stopMotor()

2
robotpyExamples/UnitTest/constants.py
View File

@@ -10,7 +10,7 @@ class IntakeConstants:
kPistonFwdChannel = 0
kPistonRevChannel = 1
kIntakeSpeed = 0.5
kIntakeVelocity = 0.5
kJoystickIndex = 0

2
robotpyExamples/UnitTest/robot.py
View File

@@ -27,7 +27,7 @@ class MyRobot(wpilib.TimedRobot):
"""This function is called periodically during operator control."""
# Activate the intake while the trigger is held
if self.joystick.getTrigger():
self.intake.activate(constants.IntakeConstants.kIntakeSpeed)
self.intake.activate(constants.IntakeConstants.kIntakeVelocity)
else:
self.intake.activate(0)

4
robotpyExamples/UnitTest/subsystems/intake.py
View File

@@ -26,9 +26,9 @@ class Intake:
self.piston.set(wpilib.DoubleSolenoid.Value.kReverse)
self.motor.set(0) # turn off the motor
def activate(self, speed: float) -> None:
def activate(self, velocity: float) -> None:
if self.isDeployed():
self.motor.set(speed)
self.motor.set(velocity)
else: # if piston isn't open, do nothing
self.motor.set(0)

12
robotpyExamples/XrpReference/commands/arcadedrive.py
View File

@@ -13,22 +13,22 @@ class ArcadeDrive(commands2.Command):
def __init__(
self,
drive: Drivetrain,
xaxisSpeedSupplier: typing.Callable[[], float],
xaxisVelocitySupplier: typing.Callable[[], float],
zaxisRotateSupplier: typing.Callable[[], float],
) -> None:
"""Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier
"""Creates a new ArcadeDrive. This command will drive your robot according to the velocity supplier
lambdas. This command does not terminate.
:param drivetrain: The drivetrain subsystem on which this command will run
:param xaxisSpeedSupplier: Callable supplier of forward/backward speed
:param zaxisRotateSupplier: Callable supplier of rotational speed
:param xaxisVelocitySupplier: Callable supplier of forward/backward velocity
:param zaxisRotateSupplier: Callable supplier of rotational velocity
"""
self.drive = drive
self.xaxisSpeedSupplier = xaxisSpeedSupplier
self.xaxisVelocitySupplier = xaxisVelocitySupplier
self.zaxisRotateSupplier = zaxisRotateSupplier
self.addRequirements(self.drive)
def execute(self) -> None:
self.drive.arcadeDrive(self.xaxisSpeedSupplier(), self.zaxisRotateSupplier())
self.drive.arcadeDrive(self.xaxisVelocitySupplier(), self.zaxisRotateSupplier())

10
robotpyExamples/XrpReference/commands/drivedistance.py
View File

@@ -10,17 +10,17 @@ from subsystems.drivetrain import Drivetrain
class DriveDistance(commands2.Command):
def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None:
def __init__(self, velocity: float, inches: float, drive: Drivetrain) -> None:
"""Creates a new DriveDistance. This command will drive your your robot for a desired distance at
a desired speed.
a desired velocity.
:param speed: The speed at which the robot will drive
:param velocity: The velocity at which the robot will drive
:param inches: The number of inches the robot will drive
:param drive: The drivetrain subsystem on which this command will run
"""
self.distance = inches
self.speed = speed
self.velocity = velocity
self.drive = drive
self.addRequirements(drive)
@@ -31,7 +31,7 @@ class DriveDistance(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(self.speed, 0)
self.drive.arcadeDrive(self.velocity, 0)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

12
robotpyExamples/XrpReference/commands/drivetime.py
View File

@@ -11,17 +11,17 @@ from subsystems.drivetrain import Drivetrain
class DriveTime(commands2.Command):
"""Creates a new DriveTime. This command will drive your robot for a desired speed and time."""
"""Creates a new DriveTime. This command will drive your robot for a desired velocity and time."""
def __init__(self, speed: float, time: float, drive: Drivetrain) -> None:
"""Creates a new DriveTime. This command will drive your robot for a desired speed and time.
def __init__(self, velocity: float, time: float, drive: Drivetrain) -> None:
"""Creates a new DriveTime. This command will drive your robot for a desired velocity and time.
:param speed: The speed which the robot will drive. Negative is in reverse.
:param velocity: The velocity which the robot will drive. Negative is in reverse.
:param time: How much time to drive in seconds
:param drive: The drivetrain subsystem on which this command will run
"""
self.speed = speed
self.velocity = velocity
self.duration = time
self.drive = drive
self.addRequirements(drive)
@@ -35,7 +35,7 @@ class DriveTime(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(self.speed, 0)
self.drive.arcadeDrive(self.velocity, 0)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

10
robotpyExamples/XrpReference/commands/turndegrees.py
View File

@@ -11,17 +11,17 @@ from subsystems.drivetrain import Drivetrain
class TurnDegrees(commands2.Command):
def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None:
def __init__(self, velocity: float, degrees: float, drive: Drivetrain) -> None:
"""Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
degrees) and rotational speed.
degrees) and rotational velocity.
:param speed: The speed which the robot will drive. Negative is in reverse.
:param velocity: The velocity which the robot will drive. Negative is in reverse.
:param degrees: Degrees to turn. Leverages encoders to compare distance.
:param drive: The drive subsystem on which this command will run
"""
self.degrees = degrees
self.speed = speed
self.velocity = velocity
self.drive = drive
self.addRequirements(drive)
@@ -33,7 +33,7 @@ class TurnDegrees(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(0, self.speed)
self.drive.arcadeDrive(0, self.velocity)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

10
robotpyExamples/XrpReference/commands/turntime.py
View File

@@ -12,18 +12,18 @@ from subsystems.drivetrain import Drivetrain
class TurnTime(commands2.Command):
"""Creates a new TurnTime command. This command will turn your robot for a
desired rotational speed and time.
desired rotational velocity and time.
"""
def __init__(self, speed: float, time: float, drive: Drivetrain) -> None:
def __init__(self, velocity: float, time: float, drive: Drivetrain) -> None:
"""Creates a new TurnTime.
:param speed: The speed which the robot will turn. Negative is in reverse.
:param velocity: The velocity which the robot will turn. Negative is in reverse.
:param time: How much time to turn in seconds
:param drive: The drive subsystem on which this command will run
"""
self.rotationalSpeed = speed
self.rotationalVelocity = velocity
self.duration = time
self.drive = drive
self.addRequirements(drive)
@@ -37,7 +37,7 @@ class TurnTime(commands2.Command):
def execute(self) -> None:
"""Called every time the scheduler runs while the command is scheduled."""
self.drive.arcadeDrive(0, self.rotationalSpeed)
self.drive.arcadeDrive(0, self.rotationalVelocity)
def end(self, interrupted: bool) -> None:
"""Called once the command ends or is interrupted."""

6
robotpyExamples/XrpReference/subsystems/drivetrain.py
View File

@@ -54,14 +54,14 @@ class Drivetrain(commands2.Subsystem):
)
self.resetEncoders()
def arcadeDrive(self, xaxisSpeed: float, zaxisRotate: float) -> None:
def arcadeDrive(self, xaxisVelocity: float, zaxisRotate: float) -> None:
"""
Drives the robot using arcade controls.
:param xaxisSpeed: the commanded forward movement
:param xaxisVelocity: the commanded forward movement
:param zaxisRotate: the commanded rotation
"""
self.drive.arcadeDrive(xaxisSpeed, zaxisRotate)
self.drive.arcadeDrive(xaxisVelocity, zaxisRotate)
def resetEncoders(self) -> None:
"""Resets the drive encoders to currently read a position of 0."""