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

2
glass/src/lib/native/cpp/hardware/PWM.cpp
View File

@@ -16,7 +16,7 @@
using namespace wpi::glass;
void wpi::glass::DisplayPWM(PWMModel* model, int index, bool outputsEnabled) {
auto data = model->GetSpeedData();
auto data = model->GetDutyCycleData();
if (!data) {
return;
}

10
glass/src/lib/native/cpp/other/Drive.cpp
View File

@@ -41,10 +41,10 @@ void wpi::glass::DisplayDrive(DriveModel* m) {
// Draw the primary rectangle.
draw->AddRect(ImVec2(x1, y1), ImVec2(x2, y2), color);
// Display the speed vector.
// Display the velocity vector.
ImVec2 center{(x1 + x2) / 2.0f, (y1 + y2) / 2.0f};
ImVec2 speed = m->GetSpeedVector();
ImVec2 arrow = center + speed * 50.0f;
ImVec2 velocity = m->GetVelocityVector();
ImVec2 arrow = center + velocity * 50.0f;
draw->AddLine(center, arrow, color, 2.0f);
@@ -61,8 +61,8 @@ void wpi::glass::DisplayDrive(DriveModel* m) {
};
// Draw the arrow if there is any translation; draw an X otherwise.
if (std::abs(speed.y) > 0 || std::abs(speed.x) > 0) {
drawArrow(arrow, std::atan2(speed.x, -speed.y));
if (std::abs(velocity.y) > 0 || std::abs(velocity.x) > 0) {
drawArrow(arrow, std::atan2(velocity.x, -velocity.y));
} else {
ImVec2 a{7.5f, +7.5f};
ImVec2 b{7.5f, -7.5f};

4
glass/src/lib/native/include/wpi/glass/hardware/PWM.hpp
View File

@@ -15,9 +15,9 @@ class DoubleSource;
class PWMModel : public Model {
public:
virtual DoubleSource* GetSpeedData() = 0;
virtual DoubleSource* GetDutyCycleData() = 0;
virtual void SetSpeed(double val) = 0;
virtual void SetDutyCycle(double dutyCycle) = 0;
};
class PWMsModel : public Model {

2
glass/src/lib/native/include/wpi/glass/other/Drive.hpp
View File

@@ -31,7 +31,7 @@ class DriveModel : public Model {
virtual const char* GetName() const = 0;
virtual const std::vector<WheelInfo>& GetWheels() const = 0;
virtual ImVec2 GetSpeedVector() const = 0;
virtual ImVec2 GetVelocityVector() const = 0;
// Clamped between -1 and 1 with -1 being full CCW.
virtual double GetRotation() const = 0;

12
glass/src/libnt/native/cpp/NTDifferentialDrive.cpp
View File

@@ -24,10 +24,12 @@ NTDifferentialDriveModel::NTDifferentialDriveModel(
m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
.Subscribe(false)},
m_lPercent{inst.GetDoubleTopic(fmt::format("{}/Left Motor Speed", path))
.GetEntry(0)},
m_rPercent{inst.GetDoubleTopic(fmt::format("{}/Right Motor Speed", path))
.GetEntry(0)},
m_lPercent{
inst.GetDoubleTopic(fmt::format("{}/Left Motor Velocity", path))
.GetEntry(0)},
m_rPercent{
inst.GetDoubleTopic(fmt::format("{}/Right Motor Velocity", path))
.GetEntry(0)},
m_nameValue{wpi::util::rsplit(path, '/').second},
m_lPercentData{fmt::format("NTDiffDriveL:{}", path)},
m_rPercentData{fmt::format("NTDiffDriveR:{}", path)} {
@@ -55,7 +57,7 @@ void NTDifferentialDriveModel::Update() {
double l = m_lPercentData.GetValue();
double r = m_rPercentData.GetValue();
m_speedVector = ImVec2(0.0, -(l + r) / 2.0);
m_velocityVector = ImVec2(0.0, -(l + r) / 2.0);
m_rotation = (l - r) / 2.0;
}

14
glass/src/libnt/native/cpp/NTMecanumDrive.cpp
View File

@@ -24,16 +24,16 @@ NTMecanumDriveModel::NTMecanumDriveModel(wpi::nt::NetworkTableInstance inst,
m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
.Subscribe(0)},
m_flPercent{
inst.GetDoubleTopic(fmt::format("{}/Front Left Motor Speed", path))
.GetEntry(0)},
m_frPercent{
inst.GetDoubleTopic(fmt::format("{}/Front Right Motor Speed", path))
inst.GetDoubleTopic(fmt::format("{}/Front Left Motor Velocity", path))
.GetEntry(0)},
m_frPercent{inst.GetDoubleTopic(
fmt::format("{}/Front Right Motor Velocity", path))
.GetEntry(0)},
m_rlPercent{
inst.GetDoubleTopic(fmt::format("{}/Rear Left Motor Speed", path))
inst.GetDoubleTopic(fmt::format("{}/Rear Left Motor Velocity", path))
.GetEntry(0)},
m_rrPercent{
inst.GetDoubleTopic(fmt::format("{}/Rear Right Motor Speed", path))
inst.GetDoubleTopic(fmt::format("{}/Rear Right Motor Velocity", path))
.GetEntry(0)},
m_nameValue{wpi::util::rsplit(path, '/').second},
m_flPercentData{fmt::format("NTMcnmDriveFL:{}", path)},
@@ -78,7 +78,7 @@ void NTMecanumDriveModel::Update() {
double rl = m_rlPercentData.GetValue();
double rr = m_rrPercentData.GetValue();
m_speedVector =
m_velocityVector =
ImVec2((fl - fr - rl + rr) / 4.0f, -(fl + fr + rl + rr) / 4.0f);
m_rotation = -(-fl + fr - rl + rr) / 4;
}

4
glass/src/libnt/native/include/wpi/glass/networktables/NTDifferentialDrive.hpp
View File

@@ -29,7 +29,7 @@ class NTDifferentialDriveModel : public DriveModel {
return m_wheels;
}
ImVec2 GetSpeedVector() const override { return m_speedVector; }
ImVec2 GetVelocityVector() const override { return m_velocityVector; }
double GetRotation() const override { return m_rotation; }
void Update() override;
@@ -49,7 +49,7 @@ class NTDifferentialDriveModel : public DriveModel {
DoubleSource m_rPercentData;
std::vector<DriveModel::WheelInfo> m_wheels;
ImVec2 m_speedVector;
ImVec2 m_velocityVector;
double m_rotation;
};
} // namespace wpi::glass

4
glass/src/libnt/native/include/wpi/glass/networktables/NTMecanumDrive.hpp
View File

@@ -29,7 +29,7 @@ class NTMecanumDriveModel : public DriveModel {
return m_wheels;
}
ImVec2 GetSpeedVector() const override { return m_speedVector; }
ImVec2 GetVelocityVector() const override { return m_velocityVector; }
double GetRotation() const override { return m_rotation; }
void Update() override;
@@ -53,7 +53,7 @@ class NTMecanumDriveModel : public DriveModel {
DoubleSource m_rrPercentData;
std::vector<DriveModel::WheelInfo> m_wheels;
ImVec2 m_speedVector;
ImVec2 m_velocityVector;
double m_rotation;
};
} // namespace wpi::glass

2
hal/src/main/java/org/wpilib/hardware/hal/CounterJNI.java
View File

@@ -66,7 +66,7 @@ public class CounterJNI extends JNIWrapper {
* Gets the Period of the most recent count.
*
* <p>Returns the time interval of the most recent count. This can be used for velocity
* calculations to determine shaft speed.
* calculations to determine shaft velocity.
*
* @param counterHandle the counter handle
* @return the period of the last two pulses in units of seconds

2
hal/src/main/java/org/wpilib/hardware/hal/EncoderJNI.java
View File

@@ -88,7 +88,7 @@ public class EncoderJNI extends JNIWrapper {
* Gets the Period of the most recent count.
*
* <p>Returns the time interval of the most recent count. This can be used for velocity
* calculations to determine shaft speed.
* calculations to determine shaft velocity.
*
* @param encoderHandle the encoder handle
* @return the period of the last two pulses in units of seconds

2
hal/src/main/native/include/wpi/hal/Counter.h
View File

@@ -79,7 +79,7 @@ int32_t HAL_GetCounter(HAL_CounterHandle counterHandle, int32_t* status);
* Gets the Period of the most recent count.
*
* Returns the time interval of the most recent count. This can be used for
* velocity calculations to determine shaft speed.
* velocity calculations to determine shaft velocity.
*
* @param[in] counterHandle the counter handle
* @param[out] status Error status variable. 0 on success.

2
hal/src/main/native/include/wpi/hal/Encoder.h
View File

@@ -118,7 +118,7 @@ void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status);
* Gets the Period of the most recent count.
*
* Returns the time interval of the most recent count. This can be used for
* velocity calculations to determine shaft speed.
* velocity calculations to determine shaft velocity.
*
* @param[in] encoderHandle the encoder handle
* @param[out] status Error status variable. 0 on success.

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."""

2
romiVendordep/src/main/java/org/wpilib/romi/RomiMotor.java
View File

@@ -16,7 +16,7 @@ public class RomiMotor extends PWMMotorController {
/** Common initialization code called by all constructors. */
protected final void initRomiMotor() {
m_pwm.setOutputPeriod(PWM.OutputPeriod.k5Ms);
setSpeed(0.0);
setDutyCycle(0.0);
}
/**

2
romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp
View File

@@ -8,5 +8,5 @@ using namespace wpi::romi;
RomiMotor::RomiMotor(int channel) : PWMMotorController("Romi Motor", channel) {
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
}

10
simulation/halsim_gui/src/main/native/cpp/PWMSimGui.cpp
View File

@@ -21,21 +21,21 @@ HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PWMPulseMicrosecond, "PWM");
class PWMSimModel : public wpi::glass::PWMModel {
public:
explicit PWMSimModel(int32_t index) : m_index{index}, m_speed{m_index} {}
explicit PWMSimModel(int32_t index) : m_index{index}, m_dutyCycle{m_index} {}
void Update() override {}
bool Exists() override { return HALSIM_GetPWMInitialized(m_index); }
wpi::glass::DoubleSource* GetSpeedData() override { return &m_speed; }
wpi::glass::DoubleSource* GetDutyCycleData() override { return &m_dutyCycle; }
void SetSpeed(double val) override {
HALSIM_SetPWMPulseMicrosecond(m_index, val);
void SetDutyCycle(double dutyCycle) override {
HALSIM_SetPWMPulseMicrosecond(m_index, dutyCycle);
}
private:
int32_t m_index;
PWMPulseMicrosecondSource m_speed;
PWMPulseMicrosecondSource m_dutyCycle;
};
class PWMsSimModel : public wpi::glass::PWMsModel {

2
simulation/halsim_ws_core/doc/hardware_ws_api.md
View File

@@ -260,7 +260,7 @@ A pneumatic control module is used to regulate the pressure in a pneumatic syste
[``"PWM"``]:#pwm-output-pwm
PWMs may be used to control either motor controllers or servos. Typically only one of either ``"<speed"`` (for a motor controller) ``"<position"`` (for a servo), or ``"raw"`` is used for a given PWM.
PWMs may be used to control either motor controllers or servos. Typically only one of either ``"<duty_cycle"`` (for a motor controller) ``"<position"`` (for a servo), or ``"raw"`` is used for a given PWM.
| Data Key | Type | Description |
| ------------------- | ------- | ------------------------------------------ |

2
simulation/halsim_ws_core/doc/wpilib-ws.yaml
View File

@@ -30,7 +30,7 @@ components:
type: PWM
device: "1"
data:
"<speed": 0.5
"<duty_cycle": 0.5
- payload:
type: DIO
device: "3"

4
simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
View File

@@ -128,8 +128,8 @@ void XRP::HandleMotorSimValueChanged(const wpi::util::json& data) {
deviceId = 3;
}
if (deviceId != -1 && motorData.find("<speed") != motorData.end()) {
m_motor_outputs[deviceId] = motorData["<speed"];
if (deviceId != -1 && motorData.find("<duty_cycle") != motorData.end()) {
m_motor_outputs[deviceId] = motorData["<duty_cycle"];
}
}

2
styleguide/spotbugs-exclude.xml
View File

@@ -74,7 +74,7 @@
<Class name="org.wpilib.math.controller.LinearSystemLoopTest" />
<Class name="~org\.wpilib\.math\.estimator\.[^.]*PoseEstimator(3d)?Test" />
<Class name="org.wpilib.math.filter.LinearFilterTest" />
<Class name="org.wpilib.math.kinematics.ChassisSpeedsTest" />
<Class name="org.wpilib.math.kinematics.ChassisVelocitiesTest" />
<Class name="org.wpilib.hardware.led.LEDPatternTest" />
<Class name="org.wpilib.simulation.AnalogInputSimTest" />
</Or>

2
tools/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
View File

@@ -179,7 +179,7 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
// Find maximum speed reached
const auto maxSpeed =
GetMaxSpeed(*data, [](auto&& pt) { return pt.velocity; });
// Find place where 90% of maximum speed exceeded
// Find place where 90% of maximum velocity exceeded
auto endIt = std::find_if(
data->begin(), data->end(), [&](const PreparedData& entry) {
return std::abs(entry.velocity) > maxSpeed * 0.9;

4
wpilibc/robotpy_pybind_build_info.bzl generated
View File

@@ -72,7 +72,7 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ
tmpl_class_names = [],
trampolines = [
("wpi::DifferentialDrive", "wpi__DifferentialDrive.hpp"),
("wpi::DifferentialDrive::WheelSpeeds", "wpi__DifferentialDrive__WheelSpeeds.hpp"),
("wpi::DifferentialDrive::WheelVelocities", "wpi__DifferentialDrive__WheelVelocities.hpp"),
],
),
struct(
@@ -83,7 +83,7 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ
tmpl_class_names = [],
trampolines = [
("wpi::MecanumDrive", "wpi__MecanumDrive.hpp"),
("wpi::MecanumDrive::WheelSpeeds", "wpi__MecanumDrive__WheelSpeeds.hpp"),
("wpi::MecanumDrive::WheelVelocities", "wpi__MecanumDrive__WheelVelocities.hpp"),
],
),
struct(

2
wpilibc/src/generate/main/native/cpp/hardware/motor/pwm_motor_controller.cpp.jinja
View File

@@ -13,7 +13,7 @@ using namespace wpi;
{{ name }}::{{ name }}(int channel) : PWMMotorController("{{ name }}", channel) {
SetBounds({{ pulse_width_ms.max }}_ms, {{ pulse_width_ms.deadbandMax }}_ms, {{ pulse_width_ms.center }}_ms, {{ pulse_width_ms.deadbandMin }}_ms, {{ pulse_width_ms.min }}_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_{{ OutputPeriod | default("5", true)}}Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "{{ ResourceName }}");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/Koors40.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
Koors40::Koors40(int channel) : PWMMotorController("Koors40", channel) {
SetBounds(2.004_ms, 1.52_ms, 1.5_ms, 1.48_ms, 0.997_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_20Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "Koors40");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/PWMSparkFlex.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
PWMSparkFlex::PWMSparkFlex(int channel) : PWMMotorController("PWMSparkFlex", channel) {
SetBounds(2.003_ms, 1.55_ms, 1.5_ms, 1.46_ms, 0.999_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "RevSparkFlexPWM");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/PWMSparkMax.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
PWMSparkMax::PWMSparkMax(int channel) : PWMMotorController("PWMSparkMax", channel) {
SetBounds(2.003_ms, 1.55_ms, 1.5_ms, 1.46_ms, 0.999_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "RevSparkMaxPWM");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/PWMTalonFX.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
PWMTalonFX::PWMTalonFX(int channel) : PWMMotorController("PWMTalonFX", channel) {
SetBounds(2.004_ms, 1.52_ms, 1.5_ms, 1.48_ms, 0.997_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "TalonFX");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/PWMTalonSRX.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
PWMTalonSRX::PWMTalonSRX(int channel) : PWMMotorController("PWMTalonSRX", channel) {
SetBounds(2.004_ms, 1.52_ms, 1.5_ms, 1.48_ms, 0.997_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "PWMTalonSRX");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/PWMVenom.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
PWMVenom::PWMVenom(int channel) : PWMMotorController("PWMVenom", channel) {
SetBounds(2.004_ms, 1.52_ms, 1.5_ms, 1.48_ms, 0.997_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "FusionVenom");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/PWMVictorSPX.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
PWMVictorSPX::PWMVictorSPX(int channel) : PWMMotorController("PWMVictorSPX", channel) {
SetBounds(2.004_ms, 1.52_ms, 1.5_ms, 1.48_ms, 0.997_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "PWMVictorSPX");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/Spark.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
Spark::Spark(int channel) : PWMMotorController("Spark", channel) {
SetBounds(2.003_ms, 1.55_ms, 1.5_ms, 1.46_ms, 0.999_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "RevSPARK");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/SparkMini.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
SparkMini::SparkMini(int channel) : PWMMotorController("SparkMini", channel) {
SetBounds(2.5_ms, 1.51_ms, 1.5_ms, 1.49_ms, 0.5_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "RevSPARK");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/Talon.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
Talon::Talon(int channel) : PWMMotorController("Talon", channel) {
SetBounds(2.037_ms, 1.539_ms, 1.513_ms, 1.487_ms, 0.989_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "Talon");
}

2
wpilibc/src/generated/main/native/cpp/hardware/motor/VictorSP.cpp generated
View File

@@ -13,7 +13,7 @@ using namespace wpi;
VictorSP::VictorSP(int channel) : PWMMotorController("VictorSP", channel) {
SetBounds(2.004_ms, 1.52_ms, 1.5_ms, 1.48_ms, 0.997_ms);
m_pwm.SetOutputPeriod(PWM::kOutputPeriod_5Ms);
SetSpeed(0.0);
SetDutyCycle(0.0);
HAL_ReportUsage("IO", GetChannel(), "VictorSP");
}

96
wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
View File

@@ -21,8 +21,9 @@ WPI_IGNORE_DEPRECATED
DifferentialDrive::DifferentialDrive(MotorController& leftMotor,
MotorController& rightMotor)
: DifferentialDrive{[&](double output) { leftMotor.Set(output); },
[&](double output) { rightMotor.Set(output); }} {
: DifferentialDrive{
[&](double output) { leftMotor.SetDutyCycle(output); },
[&](double output) { rightMotor.SetDutyCycle(output); }} {
wpi::util::SendableRegistry::AddChild(this, &leftMotor);
wpi::util::SendableRegistry::AddChild(this, &rightMotor);
}
@@ -37,7 +38,7 @@ DifferentialDrive::DifferentialDrive(std::function<void(double)> leftMotor,
wpi::util::SendableRegistry::Add(this, "DifferentialDrive", instances);
}
void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
void DifferentialDrive::ArcadeDrive(double xVelocity, double zRotation,
bool squareInputs) {
static bool reported = false;
if (!reported) {
@@ -45,10 +46,10 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
reported = true;
}
xSpeed = wpi::math::ApplyDeadband(xSpeed, m_deadband);
xVelocity = wpi::math::ApplyDeadband(xVelocity, m_deadband);
zRotation = wpi::math::ApplyDeadband(zRotation, m_deadband);
auto [left, right] = ArcadeDriveIK(xSpeed, zRotation, squareInputs);
auto [left, right] = ArcadeDriveIK(xVelocity, zRotation, squareInputs);
m_leftOutput = left * m_maxOutput;
m_rightOutput = right * m_maxOutput;
@@ -59,7 +60,7 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
Feed();
}
void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
void DifferentialDrive::CurvatureDrive(double xVelocity, double zRotation,
bool allowTurnInPlace) {
static bool reported = false;
if (!reported) {
@@ -67,10 +68,10 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
reported = true;
}
xSpeed = wpi::math::ApplyDeadband(xSpeed, m_deadband);
xVelocity = wpi::math::ApplyDeadband(xVelocity, m_deadband);
zRotation = wpi::math::ApplyDeadband(zRotation, m_deadband);
auto [left, right] = CurvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
auto [left, right] = CurvatureDriveIK(xVelocity, zRotation, allowTurnInPlace);
m_leftOutput = left * m_maxOutput;
m_rightOutput = right * m_maxOutput;
@@ -81,7 +82,7 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
Feed();
}
void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
void DifferentialDrive::TankDrive(double leftVelocity, double rightVelocity,
bool squareInputs) {
static bool reported = false;
if (!reported) {
@@ -89,10 +90,10 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
reported = true;
}
leftSpeed = wpi::math::ApplyDeadband(leftSpeed, m_deadband);
rightSpeed = wpi::math::ApplyDeadband(rightSpeed, m_deadband);
leftVelocity = wpi::math::ApplyDeadband(leftVelocity, m_deadband);
rightVelocity = wpi::math::ApplyDeadband(rightVelocity, m_deadband);
auto [left, right] = TankDriveIK(leftSpeed, rightSpeed, squareInputs);
auto [left, right] = TankDriveIK(leftVelocity, rightVelocity, squareInputs);
m_leftOutput = left * m_maxOutput;
m_rightOutput = right * m_maxOutput;
@@ -103,74 +104,75 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
Feed();
}
DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK(
double xSpeed, double zRotation, bool squareInputs) {
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
DifferentialDrive::WheelVelocities DifferentialDrive::ArcadeDriveIK(
double xVelocity, double zRotation, bool squareInputs) {
xVelocity = std::clamp(xVelocity, -1.0, 1.0);
zRotation = std::clamp(zRotation, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
xSpeed = wpi::math::CopyDirectionPow(xSpeed, 2);
xVelocity = wpi::math::CopyDirectionPow(xVelocity, 2);
zRotation = wpi::math::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 = (std::max)(std::abs(xSpeed), std::abs(zRotation));
double lesserInput = (std::min)(std::abs(xSpeed), std::abs(zRotation));
// the joystick is pointing, then desaturate the wheel velocities
double greaterInput = (std::max)(std::abs(xVelocity), std::abs(zRotation));
double lesserInput = (std::min)(std::abs(xVelocity), std::abs(zRotation));
if (greaterInput == 0.0) {
return {0.0, 0.0};
}
double saturatedInput = (greaterInput + lesserInput) / greaterInput;
leftSpeed /= saturatedInput;
rightSpeed /= saturatedInput;
leftVelocity /= saturatedInput;
rightVelocity /= saturatedInput;
return {leftSpeed, rightSpeed};
return {leftVelocity, rightVelocity};
}
DifferentialDrive::WheelSpeeds DifferentialDrive::CurvatureDriveIK(
double xSpeed, double zRotation, bool allowTurnInPlace) {
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
DifferentialDrive::WheelVelocities DifferentialDrive::CurvatureDriveIK(
double xVelocity, double zRotation, bool allowTurnInPlace) {
xVelocity = std::clamp(xVelocity, -1.0, 1.0);
zRotation = std::clamp(zRotation, -1.0, 1.0);
double leftSpeed = 0.0;
double rightSpeed = 0.0;
double leftVelocity = 0.0;
double rightVelocity = 0.0;
if (allowTurnInPlace) {
leftSpeed = xSpeed - zRotation;
rightSpeed = xSpeed + zRotation;
leftVelocity = xVelocity - zRotation;
rightVelocity = xVelocity + zRotation;
} else {
leftSpeed = xSpeed - std::abs(xSpeed) * zRotation;
rightSpeed = xSpeed + std::abs(xSpeed) * zRotation;
leftVelocity = xVelocity - std::abs(xVelocity) * zRotation;
rightVelocity = xVelocity + std::abs(xVelocity) * zRotation;
}
// Desaturate wheel speeds
double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed));
// Desaturate wheel velocities
double maxMagnitude =
std::max(std::abs(leftVelocity), std::abs(rightVelocity));
if (maxMagnitude > 1.0) {
leftSpeed /= maxMagnitude;
rightSpeed /= maxMagnitude;
leftVelocity /= maxMagnitude;
rightVelocity /= maxMagnitude;
}
return {leftSpeed, rightSpeed};
return {leftVelocity, rightVelocity};
}
DifferentialDrive::WheelSpeeds DifferentialDrive::TankDriveIK(
double leftSpeed, double rightSpeed, bool squareInputs) {
leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
DifferentialDrive::WheelVelocities DifferentialDrive::TankDriveIK(
double leftVelocity, double rightVelocity, bool squareInputs) {
leftVelocity = std::clamp(leftVelocity, -1.0, 1.0);
rightVelocity = std::clamp(rightVelocity, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
leftSpeed = wpi::math::CopyDirectionPow(leftSpeed, 2);
rightSpeed = wpi::math::CopyDirectionPow(rightSpeed, 2);
leftVelocity = wpi::math::CopyDirectionPow(leftVelocity, 2);
rightVelocity = wpi::math::CopyDirectionPow(rightVelocity, 2);
}
return {leftSpeed, rightSpeed};
return {leftVelocity, rightVelocity};
}
void DifferentialDrive::StopMotor() {
@@ -191,7 +193,7 @@ void DifferentialDrive::InitSendable(wpi::util::SendableBuilder& builder) {
builder.SetSmartDashboardType("DifferentialDrive");
builder.SetActuator(true);
builder.AddDoubleProperty(
"Left Motor Speed", [&] { return m_leftOutput; }, m_leftMotor);
"Left Motor Velocity", [&] { return m_leftOutput; }, m_leftMotor);
builder.AddDoubleProperty(
"Right Motor Speed", [&] { return m_rightOutput; }, m_rightMotor);
"Right Motor Velocity", [&] { return m_rightOutput; }, m_rightMotor);
}

60
wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
View File

@@ -23,10 +23,11 @@ MecanumDrive::MecanumDrive(MotorController& frontLeftMotor,
MotorController& rearLeftMotor,
MotorController& frontRightMotor,
MotorController& rearRightMotor)
: MecanumDrive{[&](double output) { frontLeftMotor.Set(output); },
[&](double output) { rearLeftMotor.Set(output); },
[&](double output) { frontRightMotor.Set(output); },
[&](double output) { rearRightMotor.Set(output); }} {
: MecanumDrive{
[&](double output) { frontLeftMotor.SetDutyCycle(output); },
[&](double output) { rearLeftMotor.SetDutyCycle(output); },
[&](double output) { frontRightMotor.SetDutyCycle(output); },
[&](double output) { rearRightMotor.SetDutyCycle(output); }} {
wpi::util::SendableRegistry::AddChild(this, &frontLeftMotor);
wpi::util::SendableRegistry::AddChild(this, &rearLeftMotor);
wpi::util::SendableRegistry::AddChild(this, &frontRightMotor);
@@ -48,7 +49,7 @@ MecanumDrive::MecanumDrive(std::function<void(double)> frontLeftMotor,
wpi::util::SendableRegistry::Add(this, "MecanumDrive", instances);
}
void MecanumDrive::DriveCartesian(double xSpeed, double ySpeed,
void MecanumDrive::DriveCartesian(double xVelocity, double yVelocity,
double zRotation,
wpi::math::Rotation2d gyroAngle) {
if (!reported) {
@@ -56,11 +57,11 @@ void MecanumDrive::DriveCartesian(double xSpeed, double ySpeed,
reported = true;
}
xSpeed = wpi::math::ApplyDeadband(xSpeed, m_deadband);
ySpeed = wpi::math::ApplyDeadband(ySpeed, m_deadband);
xVelocity = wpi::math::ApplyDeadband(xVelocity, m_deadband);
yVelocity = wpi::math::ApplyDeadband(yVelocity, m_deadband);
auto [frontLeft, frontRight, rearLeft, rearRight] =
DriveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
DriveCartesianIK(xVelocity, yVelocity, zRotation, gyroAngle);
m_frontLeftOutput = frontLeft * m_maxOutput;
m_rearLeftOutput = rearLeft * m_maxOutput;
@@ -100,28 +101,31 @@ void MecanumDrive::StopMotor() {
Feed();
}
MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(
double xSpeed, double ySpeed, double zRotation,
MecanumDrive::WheelVelocities MecanumDrive::DriveCartesianIK(
double xVelocity, double yVelocity, double zRotation,
wpi::math::Rotation2d gyroAngle) {
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
xVelocity = std::clamp(xVelocity, -1.0, 1.0);
yVelocity = std::clamp(yVelocity, -1.0, 1.0);
// Compensate for gyro angle.
auto input = wpi::math::Translation2d{
wpi::units::meter_t{xSpeed},
wpi::units::meter_t{
ySpeed}}.RotateBy(-gyroAngle);
auto input = wpi::math::Translation2d{wpi::units::meter_t{xVelocity},
wpi::units::meter_t{yVelocity}}
.RotateBy(-gyroAngle);
double wheelSpeeds[4];
wheelSpeeds[kFrontLeft] = input.X().value() + input.Y().value() + zRotation;
wheelSpeeds[kFrontRight] = input.X().value() - input.Y().value() - zRotation;
wheelSpeeds[kRearLeft] = input.X().value() - input.Y().value() + zRotation;
wheelSpeeds[kRearRight] = input.X().value() + input.Y().value() - zRotation;
double wheelVelocities[4];
wheelVelocities[kFrontLeft] =
input.X().value() + input.Y().value() + zRotation;
wheelVelocities[kFrontRight] =
input.X().value() - input.Y().value() - zRotation;
wheelVelocities[kRearLeft] =
input.X().value() - input.Y().value() + zRotation;
wheelVelocities[kRearRight] =
input.X().value() + input.Y().value() - zRotation;
Desaturate(wheelSpeeds);
Desaturate(wheelVelocities);
return {wheelSpeeds[kFrontLeft], wheelSpeeds[kFrontRight],
wheelSpeeds[kRearLeft], wheelSpeeds[kRearRight]};
return {wheelVelocities[kFrontLeft], wheelVelocities[kFrontRight],
wheelVelocities[kRearLeft], wheelVelocities[kRearRight]};
}
std::string MecanumDrive::GetDescription() const {
@@ -132,15 +136,15 @@ void MecanumDrive::InitSendable(wpi::util::SendableBuilder& builder) {
builder.SetSmartDashboardType("MecanumDrive");
builder.SetActuator(true);
builder.AddDoubleProperty(
"Front Left Motor Speed", [&] { return m_frontLeftOutput; },
"Front Left Motor Velocity", [&] { return m_frontLeftOutput; },
m_frontLeftMotor);
builder.AddDoubleProperty(
"Front Right Motor Speed", [&] { return m_frontRightOutput; },
"Front Right Motor Velocity", [&] { return m_frontRightOutput; },
m_frontRightMotor);
builder.AddDoubleProperty(
"Rear Left Motor Speed", [&] { return m_rearLeftOutput; },
"Rear Left Motor Velocity", [&] { return m_rearLeftOutput; },
m_rearLeftMotor);
builder.AddDoubleProperty(
"Rear Right Motor Speed", [&] { return m_rearRightOutput; },
"Rear Right Motor Velocity", [&] { return m_rearRightOutput; },
m_rearRightMotor);
}

15
wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
View File

@@ -4,12 +4,9 @@
#include "wpi/drive/RobotDriveBase.hpp"
#include <algorithm>
#include <cmath>
#include <cstddef>
#include "wpi/hardware/motor/MotorController.hpp"
using namespace wpi;
RobotDriveBase::RobotDriveBase() {
@@ -28,17 +25,17 @@ void RobotDriveBase::FeedWatchdog() {
Feed();
}
void RobotDriveBase::Desaturate(std::span<double> wheelSpeeds) {
double maxMagnitude = std::abs(wheelSpeeds[0]);
for (size_t i = 1; i < wheelSpeeds.size(); i++) {
double temp = std::abs(wheelSpeeds[i]);
void RobotDriveBase::Desaturate(std::span<double> wheelVelocities) {
double maxMagnitude = std::abs(wheelVelocities[0]);
for (size_t i = 1; i < wheelVelocities.size(); i++) {
double temp = std::abs(wheelVelocities[i]);
if (maxMagnitude < temp) {
maxMagnitude = temp;
}
}
if (maxMagnitude > 1.0) {
for (size_t i = 0; i < wheelSpeeds.size(); i++) {
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
for (size_t i = 0; i < wheelVelocities.size(); i++) {
wheelVelocities[i] = wheelVelocities[i] / maxMagnitude;
}
}
}

4
wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp
View File

@@ -92,10 +92,10 @@ ExpansionHubMotor::~ExpansionHubMotor() noexcept {
m_hub.UnreserveMotor(m_channel);
}
void ExpansionHubMotor::SetPercentagePower(double power) {
void ExpansionHubMotor::SetDutyCycle(double dutyCycle) {
SetEnabled(true);
m_modePublisher.Set(kPercentageMode);
m_setpointPublisher.Set(power);
m_setpointPublisher.Set(dutyCycle);
}
void ExpansionHubMotor::SetVoltage(wpi::units::volt_t voltage) {

4
wpilibc/src/main/native/cpp/hardware/led/LEDPattern.cpp
View File

@@ -69,7 +69,7 @@ LEDPattern LEDPattern::OffsetBy(int offset) {
});
}
LEDPattern LEDPattern::ScrollAtRelativeSpeed(wpi::units::hertz_t velocity) {
LEDPattern LEDPattern::ScrollAtRelativeVelocity(wpi::units::hertz_t velocity) {
// velocity is in terms of LED lengths per second (1_hz = full cycle per
// second, 0.5_hz = half cycle per second, 2_hz = two cycles per second)
// Invert and multiply by 1,000,000 to get microseconds
@@ -88,7 +88,7 @@ LEDPattern LEDPattern::ScrollAtRelativeSpeed(wpi::units::hertz_t velocity) {
});
}
LEDPattern LEDPattern::ScrollAtAbsoluteSpeed(
LEDPattern LEDPattern::ScrollAtAbsoluteVelocity(
wpi::units::meters_per_second_t velocity, wpi::units::meter_t ledSpacing) {
// Velocity is in terms of meters per second
// Multiply by 1,000,000 to use microseconds instead of seconds

Some files were not shown because too many files have changed in this diff Show More