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

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibcExamples/src/main/cpp/examples/AddressableLED/include/Robot.hpp
View File

@@ -31,7 +31,7 @@ class Robot : public wpi::TimedRobot {
wpi::LEDPattern m_rainbow = wpi::LEDPattern::Rainbow(255, 128);
// Create a new pattern that scrolls the rainbow pattern across the LED
// strip, moving at a speed of 1 meter per second.
// strip, moving at a velocity of 1 meter per second.
wpi::LEDPattern m_scrollingRainbow =
m_rainbow.ScrollAtAbsoluteSpeed(1_mps, kLedSpacing);
m_rainbow.ScrollAtAbsoluteVelocity(1_mps, kLedSpacing);
};

4
wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
View File

@@ -15,8 +15,8 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_leftMotor{0};
wpi::PWMSparkMax m_rightMotor{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_leftMotor.Set(output); },
[&](double output) { m_rightMotor.Set(output); }};
[&](double output) { m_leftMotor.SetDutyCycle(output); },
[&](double output) { m_rightMotor.SetDutyCycle(output); }};
wpi::Joystick m_stick{0};
public:

4
wpilibcExamples/src/main/cpp/examples/ArcadeDriveGamepad/cpp/Robot.cpp
View File

@@ -15,8 +15,8 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_leftMotor{0};
wpi::PWMSparkMax m_rightMotor{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_leftMotor.Set(output); },
[&](double output) { m_rightMotor.Set(output); }};
[&](double output) { m_leftMotor.SetDutyCycle(output); },
[&](double output) { m_rightMotor.SetDutyCycle(output); }};
wpi::Gamepad m_driverController{0};
public:

5
wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp
View File

@@ -4,7 +4,6 @@
#include "subsystems/Arm.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/RobotController.hpp"
#include "wpi/util/Preferences.hpp"
@@ -25,7 +24,7 @@ void Arm::SimulationPeriodic() {
// In this method, we update our simulation of what our arm is doing
// First, we set our "inputs" (voltages)
m_armSim.SetInput(wpi::math::Vectord<1>{
m_motor.Get() * wpi::RobotController::GetInputVoltage()});
m_motor.GetDutyCycle() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_armSim.Update(20_ms);
@@ -60,5 +59,5 @@ void Arm::ReachSetpoint() {
}
void Arm::Stop() {
m_motor.Set(0.0);
m_motor.SetDutyCycle(0.0);
}

16
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
View File

@@ -4,22 +4,22 @@
#include "Drivetrain.hpp"
void Drivetrain::SetSpeeds(
const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
void Drivetrain::SetVelocities(
const wpi::math::DifferentialDriveWheelVelocities& velocities) {
const auto leftFeedforward = m_feedforward.Calculate(velocities.left);
const auto rightFeedforward = m_feedforward.Calculate(velocities.right);
const double leftOutput = m_leftPIDController.Calculate(
m_leftEncoder.GetRate(), speeds.left.value());
m_leftEncoder.GetRate(), velocities.left.value());
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.value());
m_rightEncoder.GetRate(), velocities.right.value());
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot) {
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
SetVelocities(m_kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
}
void Drivetrain::UpdateOdometry() {

13
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
View File

@@ -15,19 +15,20 @@ class Robot : public wpi::TimedRobot {
}
void TeleopPeriodic() override {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxSpeed;
const auto xVelocity =
-m_velocityLimiter.Calculate(m_controller.GetLeftY()) *
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). Gamepads return positive values when you pull to
// the right by default.
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
Drivetrain::kMaxAngularVelocity;
m_drive.Drive(xSpeed, rot);
m_drive.Drive(xVelocity, rot);
}
private:
@@ -35,7 +36,7 @@ class Robot : public wpi::TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_speedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;

9
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.hpp
View File

@@ -45,13 +45,14 @@ class Drivetrain {
m_rightEncoder.Reset();
}
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxVelocity =
3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularVelocity{
std::numbers::pi}; // 1/2 rotation per second
void SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
void SetVelocities(
const wpi::math::DifferentialDriveWheelVelocities& velocities);
void Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot);
void UpdateOdometry();

25
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -37,22 +37,22 @@ Drivetrain::Drivetrain() {
wpi::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
}
void Drivetrain::SetSpeeds(
const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
void Drivetrain::SetVelocities(
const wpi::math::DifferentialDriveWheelVelocities& velocities) {
const auto leftFeedforward = m_feedforward.Calculate(velocities.left);
const auto rightFeedforward = m_feedforward.Calculate(velocities.right);
const double leftOutput = m_leftPIDController.Calculate(
m_leftEncoder.GetRate(), speeds.left.value());
m_leftEncoder.GetRate(), velocities.left.value());
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.value());
m_rightEncoder.GetRate(), velocities.right.value());
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot) {
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
SetVelocities(m_kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
}
void Drivetrain::PublishCameraToObject(
@@ -122,10 +122,11 @@ void Drivetrain::SimulationPeriodic() {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro.
m_drivetrainSimulator.SetInputs(wpi::units::volt_t{m_leftLeader.Get()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{m_rightLeader.Get()} *
wpi::RobotController::GetInputVoltage());
m_drivetrainSimulator.SetInputs(
wpi::units::volt_t{m_leftLeader.GetDutyCycle()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{m_rightLeader.GetDutyCycle()} *
wpi::RobotController::GetInputVoltage());
m_drivetrainSimulator.Update(20_ms);
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());

13
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp
View File

@@ -17,19 +17,20 @@ class Robot : public wpi::TimedRobot {
void RobotPeriodic() override { m_drive.Periodic(); }
void TeleopPeriodic() override {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxSpeed;
const auto xVelocity =
-m_velocityLimiter.Calculate(m_controller.GetLeftY()) *
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). Gamepads return positive values when you pull to
// the right by default.
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
Drivetrain::kMaxAngularVelocity;
m_drive.Drive(xSpeed, rot);
m_drive.Drive(xVelocity, rot);
}
void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
@@ -39,7 +40,7 @@ class Robot : public wpi::TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_speedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;

15
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp
View File

@@ -36,24 +36,25 @@ class Drivetrain {
public:
Drivetrain();
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxVelocity =
3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularVelocity{
std::numbers::pi}; // 1/2 rotation per second
/**
* Sets the desired wheel speeds.
* Sets the desired wheel velocities.
*
* @param speeds The desired wheel speeds.
* @param velocities The desired wheel velocities.
*/
void SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds);
void SetVelocities(
const wpi::math::DifferentialDriveWheelVelocities& velocities);
/** Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity.
* @param xVelocity Linear velocity.
* @param rot Angular Velocity.
*/
void Drive(wpi::units::meters_per_second_t xSpeed,
void Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot);
/**

6
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
View File

@@ -24,10 +24,10 @@ RobotContainer::RobotContainer() {
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// While holding the bumper button, drive at half speed
// While holding the bumper button, drive at half velocity
m_driverController.RightBumper()
.OnTrue(m_driveHalfSpeed.get())
.OnFalse(m_driveFullSpeed.get());
.OnTrue(m_driveHalfVelocity.get())
.OnFalse(m_driveFullVelocity.get());
// Drive forward by 3 meters when the 'South Face' button is pressed, with a
// timeout of 10 seconds

2
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.hpp
View File

@@ -36,7 +36,7 @@ inline constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
inline constexpr double kp = 1;
inline constexpr auto kMaxSpeed = 3_mps;
inline constexpr auto kMaxVelocity = 3_mps;
inline constexpr auto kMaxAcceleration = 3_mps_sq;
} // namespace DriveConstants

2
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.hpp
View File

@@ -66,7 +66,7 @@ class ExampleSmartMotorController {
*/
void ResetEncoder() {}
void Set(double speed) { m_value = speed; }
void Set(double velocity) { m_value = velocity; }
double Get() const { return m_value; }

4
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.hpp
View File

@@ -35,9 +35,9 @@ class RobotContainer {
DriveSubsystem m_drive;
// RobotContainer-owned commands
wpi::cmd::CommandPtr m_driveHalfSpeed =
wpi::cmd::CommandPtr m_driveHalfVelocity =
wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {});
wpi::cmd::CommandPtr m_driveFullSpeed =
wpi::cmd::CommandPtr m_driveFullVelocity =
wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {});
void ConfigureButtonBindings();

2
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.hpp
View File

@@ -96,7 +96,7 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
private:
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{
{DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}};
{DriveConstants::kMaxVelocity, DriveConstants::kMaxAcceleration}};
wpi::Timer m_timer;
wpi::units::meter_t m_initialLeftDistance;
wpi::units::meter_t m_initialRightDistance;

2
wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.hpp
View File

@@ -66,7 +66,7 @@ class ExampleSmartMotorController {
*/
void ResetEncoder() {}
void Set(double speed) {}
void Set(double velocity) {}
double Get() const { return 0; }

5
wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp
View File

@@ -4,7 +4,6 @@
#include "subsystems/Elevator.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/RobotController.hpp"
@@ -21,7 +20,7 @@ void Elevator::SimulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.SetInput(wpi::math::Vectord<1>{
m_motorSim.GetSpeed() * wpi::RobotController::GetInputVoltage()});
m_motorSim.GetDutyCycle() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.Update(20_ms);
@@ -60,5 +59,5 @@ void Elevator::Reset() {
}
void Elevator::Stop() {
m_motor.Set(0.0);
m_motor.SetDutyCycle(0.0);
}

5
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.cpp
View File

@@ -4,7 +4,6 @@
#include "subsystems/Elevator.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/RobotController.hpp"
@@ -21,7 +20,7 @@ void Elevator::SimulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.SetInput(wpi::math::Vectord<1>{
m_motorSim.GetSpeed() * wpi::RobotController::GetInputVoltage()});
m_motorSim.GetDutyCycle() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.Update(20_ms);
@@ -51,5 +50,5 @@ void Elevator::ReachGoal(wpi::units::meter_t goal) {
void Elevator::Stop() {
m_controller.SetGoal(0.0_m);
m_motor.Set(0.0);
m_motor.SetDutyCycle(0.0);
}

2
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.hpp
View File

@@ -66,7 +66,7 @@ class ExampleSmartMotorController {
*/
void ResetEncoder() {}
void Set(double speed) {}
void Set(double velocity) {}
double Get() const { return 0; }

12
wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp
View File

@@ -36,14 +36,14 @@ class Robot : public wpi::TimedRobot {
// and there is not a ball at the kicker
&& !isBallAtKicker)
// activate the intake
.IfHigh([&intake = m_intake] { intake.Set(0.5); });
.IfHigh([&intake = m_intake] { intake.SetDutyCycle(0.5); });
// if the thumb button is not held
(!intakeButton
// or there is a ball in the kicker
|| isBallAtKicker)
// stop the intake
.IfHigh([&intake = m_intake] { intake.Set(0.0); });
.IfHigh([&intake = m_intake] { intake.SetDutyCycle(0.0); });
wpi::BooleanEvent shootTrigger{
&m_loop, [&joystick = m_joystick] { return joystick.GetTrigger(); }};
@@ -59,7 +59,9 @@ class Robot : public wpi::TimedRobot {
ff.Calculate(wpi::units::radians_per_second_t{SHOT_VELOCITY}));
});
// if not, stop
(!shootTrigger).IfHigh([&shooter = m_shooter] { shooter.Set(0.0); });
(!shootTrigger).IfHigh([&shooter = m_shooter] {
shooter.SetDutyCycle(0.0);
});
wpi::BooleanEvent atTargetVelocity =
wpi::BooleanEvent(
@@ -69,13 +71,13 @@ class Robot : public wpi::TimedRobot {
.Debounce(0.2_s);
// if we're at the target velocity, kick the ball into the shooter wheel
atTargetVelocity.IfHigh([&kicker = m_kicker] { kicker.Set(0.7); });
atTargetVelocity.IfHigh([&kicker = m_kicker] { kicker.SetDutyCycle(0.7); });
// when we stop being at the target velocity, it means the ball was shot
atTargetVelocity
.Falling()
// so stop the kicker
.IfHigh([&kicker = m_kicker] { kicker.Set(0.0); });
.IfHigh([&kicker = m_kicker] { kicker.SetDutyCycle(0.0); });
}
void RobotPeriodic() override { m_loop.Poll(); }

6
wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp
View File

@@ -16,12 +16,12 @@
/**
* This is a sample program to demonstrate the use of a
* wpi::math::BangBangController with a flywheel to control speed.
* wpi::math::BangBangController with a flywheel to control velocity.
*/
class Robot : public wpi::TimedRobot {
public:
/**
* Controls flywheel to a set speed (RPM) controlled by a joystick.
* Controls flywheel to a set velocity (RPM) controlled by a joystick.
*/
void TeleopPeriodic() override {
// Scale setpoint value between 0 and maxSetpointValue
@@ -52,7 +52,7 @@ class Robot : public wpi::TimedRobot {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated velocities to our simulated encoder
m_flywheelSim.SetInputVoltage(
m_flywheelMotor.Get() *
m_flywheelMotor.GetDutyCycle() *
wpi::units::volt_t{wpi::RobotController::GetInputVoltage()});
m_flywheelSim.Update(20_ms);
m_encoderSim.SetRate(m_flywheelSim.GetAngularVelocity().value());

6
wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
View File

@@ -27,7 +27,7 @@ class Robot : public wpi::TimedRobot {
void AutonomousPeriodic() override {
// Drive for 2 seconds
if (m_timer.Get() < 2_s) {
// Drive forwards half speed, make sure to turn input squaring off
// Drive forwards half velocity, make sure to turn input squaring off
m_robotDrive.ArcadeDrive(0.5, 0.0, false);
} else {
// Stop robot
@@ -52,8 +52,8 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_left{0};
wpi::PWMSparkMax m_right{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_left.Set(output); },
[&](double output) { m_right.Set(output); }};
[&](double output) { m_left.SetDutyCycle(output); },
[&](double output) { m_right.SetDutyCycle(output); }};
wpi::Gamepad m_controller{0};
wpi::Timer m_timer;

7
wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
View File

@@ -28,7 +28,7 @@ class Robot : public wpi::TimedRobot {
}
/**
* The motor speed is set from the joystick while the DifferentialDrive
* 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.
*/
@@ -50,8 +50,9 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_left{kLeftMotorPort};
wpi::PWMSparkMax m_right{kRightMotorPort};
wpi::DifferentialDrive m_drive{[&](double output) { m_left.Set(output); },
[&](double output) { m_right.Set(output); }};
wpi::DifferentialDrive m_drive{
[&](double output) { m_left.SetDutyCycle(output); },
[&](double output) { m_right.SetDutyCycle(output); }};
wpi::OnboardIMU m_imu{kIMUMountOrientation};
wpi::Joystick m_joystick{kJoystickPort};

8
wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
View File

@@ -49,10 +49,10 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_frontRight{kFrontRightMotorPort};
wpi::PWMSparkMax m_rearRight{kRearRightMotorPort};
wpi::MecanumDrive m_robotDrive{
[&](double output) { m_frontLeft.Set(output); },
[&](double output) { m_rearLeft.Set(output); },
[&](double output) { m_frontRight.Set(output); },
[&](double output) { m_rearRight.Set(output); }};
[&](double output) { m_frontLeft.SetDutyCycle(output); },
[&](double output) { m_rearLeft.SetDutyCycle(output); },
[&](double output) { m_frontRight.SetDutyCycle(output); },
[&](double output) { m_rearRight.SetDutyCycle(output); }};
wpi::OnboardIMU m_imu{kIMUMountOrientation};
wpi::Joystick m_joystick{kJoystickPort};

2
wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
View File

@@ -39,7 +39,7 @@ void RobotContainer::ConfigureButtonBindings() {
m_driverController.EastFace().OnTrue(m_hatch.GrabHatchCommand());
// Release the hatch when the 'West Face' button is pressed.
m_driverController.WestFace().OnTrue(m_hatch.ReleaseHatchCommand());
// While holding Right Bumper, drive at half speed
// While holding Right Bumper, drive at half velocity
m_driverController.RightBumper()
.OnTrue(wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}))
.OnFalse(

8
wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/Autos.cpp
View File

@@ -15,7 +15,9 @@ wpi::cmd::CommandPtr autos::SimpleAuto(DriveSubsystem* drive) {
// Reset encoders on command start
[drive] { drive->ResetEncoders(); },
// Drive forward while the command is executing
[drive] { drive->ArcadeDrive(AutoConstants::kAutoDriveSpeed, 0); },
[drive] {
drive->ArcadeDrive(AutoConstants::kAutoDriveVelocity, 0);
},
// Stop driving at the end of the command
[drive](bool interrupted) { drive->ArcadeDrive(0, 0); },
// End the command when the robot's driven distance exceeds the
@@ -37,7 +39,7 @@ wpi::cmd::CommandPtr autos::ComplexAuto(DriveSubsystem* drive,
// Reset encoders on command start
[drive] { drive->ResetEncoders(); },
// Drive forward while the command is executing
[drive] { drive->ArcadeDrive(kAutoDriveSpeed, 0); },
[drive] { drive->ArcadeDrive(kAutoDriveVelocity, 0); },
// Stop driving at the end of the command
[drive](bool interrupted) { drive->ArcadeDrive(0, 0); },
// End the command when the robot's driven distance exceeds the
@@ -57,7 +59,7 @@ wpi::cmd::CommandPtr autos::ComplexAuto(DriveSubsystem* drive,
// Reset encoders on command start
[drive] { drive->ResetEncoders(); },
// Drive backward while the command is executing
[drive] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); },
[drive] { drive->ArcadeDrive(-kAutoDriveVelocity, 0); },
// Stop driving at the end of the command
[drive](bool interrupted) { drive->ArcadeDrive(0, 0); },
// End the command when the robot's driven distance exceeds the

2
wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.hpp
View File

@@ -42,7 +42,7 @@ inline constexpr int kHatchSolenoidPorts[]{0, 1};
namespace AutoConstants {
inline constexpr double kAutoDriveDistanceInches = 60;
inline constexpr double kAutoBackupDistanceInches = 20;
inline constexpr double kAutoDriveSpeed = 0.5;
inline constexpr double kAutoDriveVelocity = 0.5;
} // namespace AutoConstants
namespace OIConstants {

5
wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.hpp
View File

@@ -62,8 +62,9 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
wpi::PWMSparkMax m_right2;
// The robot's drive
wpi::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
[&](double output) { m_right1.Set(output); }};
wpi::DifferentialDrive m_drive{
[&](double output) { m_left1.SetDutyCycle(output); },
[&](double output) { m_right1.SetDutyCycle(output); }};
// The left-side drive encoder
wpi::Encoder m_leftEncoder;

6
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
View File

@@ -6,7 +6,7 @@
#include "commands/DefaultDrive.hpp"
#include "commands/GrabHatch.hpp"
#include "commands/HalveDriveSpeed.hpp"
#include "commands/HalveDriveVelocity.hpp"
#include "commands/ReleaseHatch.hpp"
#include "wpi/commands2/button/JoystickButton.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
@@ -46,10 +46,10 @@ void RobotContainer::ConfigureButtonBindings() {
// Release the hatch when the 'East Face' button is pressed.
wpi::cmd::JoystickButton(&m_driverController, wpi::Gamepad::Button::kEastFace)
.OnTrue(ReleaseHatch(&m_hatch).ToPtr());
// While holding the bumper button, drive at half speed
// While holding the bumper button, drive at half velocity
wpi::cmd::JoystickButton(&m_driverController,
wpi::Gamepad::Button::kRightBumper)
.WhileTrue(HalveDriveSpeed(&m_drive).ToPtr());
.WhileTrue(HalveDriveVelocity(&m_drive).ToPtr());
}
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {

4
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp
View File

@@ -9,9 +9,9 @@ using namespace AutoConstants;
ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) {
AddCommands(
// Drive forward the specified distance
DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive),
DriveDistance(kAutoDriveDistanceInches, kAutoDriveVelocity, drive),
// Release the hatch
ReleaseHatch(hatch),
// Drive backward the specified distance
DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive));
DriveDistance(kAutoBackupDistanceInches, -kAutoDriveVelocity, drive));
}

8
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp
View File

@@ -6,19 +6,19 @@
#include <cmath>
DriveDistance::DriveDistance(double inches, double speed,
DriveDistance::DriveDistance(double inches, double velocity,
DriveSubsystem* subsystem)
: m_drive(subsystem), m_distance(inches), m_speed(speed) {
: m_drive(subsystem), m_distance(inches), m_velocity(velocity) {
AddRequirements(subsystem);
}
void DriveDistance::Initialize() {
m_drive->ResetEncoders();
m_drive->ArcadeDrive(m_speed, 0);
m_drive->ArcadeDrive(m_velocity, 0);
}
void DriveDistance::Execute() {
m_drive->ArcadeDrive(m_speed, 0);
m_drive->ArcadeDrive(m_velocity, 0);
}
void DriveDistance::End(bool interrupted) {

8
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp → wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveVelocity.cpp
View File

@@ -2,15 +2,15 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "commands/HalveDriveSpeed.hpp"
#include "commands/HalveDriveVelocity.hpp"
HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem)
HalveDriveVelocity::HalveDriveVelocity(DriveSubsystem* subsystem)
: m_drive(subsystem) {}
void HalveDriveSpeed::Initialize() {
void HalveDriveVelocity::Initialize() {
m_drive->SetMaxOutput(0.5);
}
void HalveDriveSpeed::End(bool interrupted) {
void HalveDriveVelocity::End(bool interrupted) {
m_drive->SetMaxOutput(1);
}

2
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.hpp
View File

@@ -42,7 +42,7 @@ inline constexpr int kHatchSolenoidPorts[]{0, 1};
namespace AutoConstants {
constexpr double kAutoDriveDistanceInches = 60;
constexpr double kAutoBackupDistanceInches = 20;
constexpr double kAutoDriveSpeed = 0.5;
constexpr double kAutoDriveVelocity = 0.5;
} // namespace AutoConstants
namespace OIConstants {

2
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.hpp
View File

@@ -36,7 +36,7 @@ class RobotContainer {
// The autonomous routines
DriveDistance m_simpleAuto{AutoConstants::kAutoDriveDistanceInches,
AutoConstants::kAutoDriveSpeed, &m_drive};
AutoConstants::kAutoDriveVelocity, &m_drive};
ComplexAuto m_complexAuto{&m_drive, &m_hatch};
// The chooser for the autonomous routines

6
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.hpp
View File

@@ -15,10 +15,10 @@ class DriveDistance
* Creates a new DriveDistance.
*
* @param inches The number of inches the robot will drive
* @param speed The speed at which the robot will drive
* @param velocity The velocity at which the robot will drive
* @param drive The drive subsystem on which this command will run
*/
DriveDistance(double inches, double speed, DriveSubsystem* subsystem);
DriveDistance(double inches, double velocity, DriveSubsystem* subsystem);
void Initialize() override;
@@ -31,5 +31,5 @@ class DriveDistance
private:
DriveSubsystem* m_drive;
double m_distance;
double m_speed;
double m_velocity;
};

6
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.hpp → wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveVelocity.hpp
View File

@@ -8,10 +8,10 @@
#include "wpi/commands2/Command.hpp"
#include "wpi/commands2/CommandHelper.hpp"
class HalveDriveSpeed
: public wpi::cmd::CommandHelper<wpi::cmd::Command, HalveDriveSpeed> {
class HalveDriveVelocity
: public wpi::cmd::CommandHelper<wpi::cmd::Command, HalveDriveVelocity> {
public:
explicit HalveDriveSpeed(DriveSubsystem* subsystem);
explicit HalveDriveVelocity(DriveSubsystem* subsystem);
void Initialize() override;

5
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.hpp
View File

@@ -62,8 +62,9 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
wpi::PWMSparkMax m_right2;
// The robot's drive
wpi::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
[&](double output) { m_right1.Set(output); }};
wpi::DifferentialDrive m_drive{
[&](double output) { m_left1.SetDutyCycle(output); },
[&](double output) { m_right1.SetDutyCycle(output); }};
// The left-side drive encoder
wpi::Encoder m_leftEncoder;

51
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -4,14 +4,7 @@
#include "Drivetrain.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
wpi::math::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {wpi::units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
#include "wpi/math/kinematics/ChassisVelocities.hpp"
wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances()
const {
@@ -21,25 +14,33 @@ wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances()
wpi::units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(
const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
wpi::math::MecanumDriveWheelVelocities Drivetrain::GetCurrentWheelVelocities()
const {
return {wpi::units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
void Drivetrain::SetVelocities(
const wpi::math::MecanumDriveWheelVelocities& wheelVelocities) {
const auto frontLeftFeedforward =
m_feedforward.Calculate(wheelSpeeds.frontLeft);
m_feedforward.Calculate(wheelVelocities.frontLeft);
const auto frontRightFeedforward =
m_feedforward.Calculate(wheelSpeeds.frontRight);
m_feedforward.Calculate(wheelVelocities.frontRight);
const auto backLeftFeedforward =
m_feedforward.Calculate(wheelSpeeds.rearLeft);
m_feedforward.Calculate(wheelVelocities.rearLeft);
const auto backRightFeedforward =
m_feedforward.Calculate(wheelSpeeds.rearRight);
m_feedforward.Calculate(wheelVelocities.rearRight);
const double frontLeftOutput = m_frontLeftPIDController.Calculate(
m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.value());
m_frontLeftEncoder.GetRate(), wheelVelocities.frontLeft.value());
const double frontRightOutput = m_frontRightPIDController.Calculate(
m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.value());
m_frontRightEncoder.GetRate(), wheelVelocities.frontRight.value());
const double backLeftOutput = m_backLeftPIDController.Calculate(
m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.value());
m_backLeftEncoder.GetRate(), wheelVelocities.rearLeft.value());
const double backRightOutput = m_backRightPIDController.Calculate(
m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.value());
m_backRightEncoder.GetRate(), wheelVelocities.rearRight.value());
m_frontLeftMotor.SetVoltage(wpi::units::volt_t{frontLeftOutput} +
frontLeftFeedforward);
@@ -51,16 +52,18 @@ void Drivetrain::SetSpeeds(
backRightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period) {
wpi::math::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
wpi::math::ChassisVelocities chassisVelocities{xVelocity, yVelocity, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_imu.GetRotation2d());
chassisVelocities =
chassisVelocities.ToRobotRelative(m_imu.GetRotation2d());
}
SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period))
.Desaturate(kMaxSpeed));
SetVelocities(
m_kinematics.ToWheelVelocities(chassisVelocities.Discretize(period))
.Desaturate(kMaxVelocity));
}
void Drivetrain::UpdateOdometry() {

24
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
View File

@@ -22,30 +22,32 @@ class Robot : public wpi::TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxSpeed;
const auto xVelocity =
-m_xVelocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// Get the y velocity or sideways/strafe velocity. We are inverting this
// because we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
Drivetrain::kMaxSpeed;
const auto yVelocity =
-m_yVelocityLimiter.Calculate(m_controller.GetLeftX()) *
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). Gamepads return positive values when you pull to
// the right by default.
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
Drivetrain::kMaxAngularVelocity;
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
m_mecanum.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
}
};

15
wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.hpp
View File

@@ -14,7 +14,7 @@
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveOdometry.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
/**
* Represents a mecanum drive style drivetrain.
@@ -30,18 +30,19 @@ class Drivetrain {
m_backRightMotor.SetInverted(true);
}
wpi::math::MecanumDriveWheelSpeeds GetCurrentState() const;
wpi::math::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
void SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
wpi::math::MecanumDriveWheelVelocities GetCurrentWheelVelocities() const;
void SetVelocities(
const wpi::math::MecanumDriveWheelVelocities& wheelVelocities);
void Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxVelocity =
3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularVelocity{
std::numbers::pi}; // 1/2 rotation per second
private:

8
wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
View File

@@ -46,10 +46,10 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_frontRight{kFrontRightChannel};
wpi::PWMSparkMax m_rearRight{kRearRightChannel};
wpi::MecanumDrive m_robotDrive{
[&](double output) { m_frontLeft.Set(output); },
[&](double output) { m_rearLeft.Set(output); },
[&](double output) { m_frontRight.Set(output); },
[&](double output) { m_rearRight.Set(output); }};
[&](double output) { m_frontLeft.SetDutyCycle(output); },
[&](double output) { m_rearLeft.SetDutyCycle(output); },
[&](double output) { m_frontRight.SetDutyCycle(output); },
[&](double output) { m_rearRight.SetDutyCycle(output); }};
wpi::Joystick m_stick{kJoystickChannel};
};

66
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -7,60 +7,64 @@
#include "ExampleGlobalMeasurementSensor.hpp"
#include "wpi/system/Timer.hpp"
wpi::math::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {wpi::units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentDistances() const {
wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances()
const {
return {wpi::units::meter_t{m_frontLeftEncoder.GetDistance()},
wpi::units::meter_t{m_frontRightEncoder.GetDistance()},
wpi::units::meter_t{m_backLeftEncoder.GetDistance()},
wpi::units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(
const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
wpi::math::MecanumDriveWheelVelocities Drivetrain::GetCurrentWheelVelocities()
const {
return {wpi::units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
void Drivetrain::SetVelocities(
const wpi::math::MecanumDriveWheelVelocities& wheelVelocities) {
std::function<void(wpi::units::meters_per_second_t, const wpi::Encoder&,
wpi::math::PIDController&, wpi::PWMSparkMax&)>
calcAndSetSpeeds = [&m_feedforward = m_feedforward](
wpi::units::meters_per_second_t speed,
const auto& encoder, auto& controller,
auto& motor) {
auto feedforward = m_feedforward.Calculate(speed);
double output = controller.Calculate(encoder.GetRate(), speed.value());
calcAndSetVelocities = [&m_feedforward = m_feedforward](
wpi::units::meters_per_second_t velocity,
const auto& encoder, auto& controller,
auto& motor) {
auto feedforward = m_feedforward.Calculate(velocity);
double output =
controller.Calculate(encoder.GetRate(), velocity.value());
motor.SetVoltage(wpi::units::volt_t{output} + feedforward);
};
calcAndSetSpeeds(wheelSpeeds.frontLeft, m_frontLeftEncoder,
m_frontLeftPIDController, m_frontLeftMotor);
calcAndSetSpeeds(wheelSpeeds.frontRight, m_frontRightEncoder,
m_frontRightPIDController, m_frontRightMotor);
calcAndSetSpeeds(wheelSpeeds.rearLeft, m_backLeftEncoder,
m_backLeftPIDController, m_backLeftMotor);
calcAndSetSpeeds(wheelSpeeds.rearRight, m_backRightEncoder,
m_backRightPIDController, m_backRightMotor);
calcAndSetVelocities(wheelVelocities.frontLeft, m_frontLeftEncoder,
m_frontLeftPIDController, m_frontLeftMotor);
calcAndSetVelocities(wheelVelocities.frontRight, m_frontRightEncoder,
m_frontRightPIDController, m_frontRightMotor);
calcAndSetVelocities(wheelVelocities.rearLeft, m_backLeftEncoder,
m_backLeftPIDController, m_backLeftMotor);
calcAndSetVelocities(wheelVelocities.rearRight, m_backRightEncoder,
m_backRightPIDController, m_backRightMotor);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period) {
wpi::math::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
wpi::math::ChassisVelocities chassisVelocities{xVelocity, yVelocity, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(
chassisVelocities = chassisVelocities.ToRobotRelative(
m_poseEstimator.GetEstimatedPosition().Rotation());
}
SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period))
.Desaturate(kMaxSpeed));
SetVelocities(
m_kinematics.ToWheelVelocities(chassisVelocities.Discretize(period))
.Desaturate(kMaxVelocity));
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(
m_imu.GetRotation2d(),
GetCurrentDistances()); // TODO(Ryan): fixup when sim implemented
GetCurrentWheelDistances()); // TODO(Ryan): fixup when sim implemented
// Also apply vision measurements. We use 0.3 seconds in the past as an
// example -- on a real robot, this must be calculated based either on latency

24
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp
View File

@@ -22,30 +22,32 @@ class Robot : public wpi::TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxSpeed;
const auto xVelocity =
-m_xVelocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// Get the y velocity or sideways/strafe velocity. We are inverting this
// because we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
Drivetrain::kMaxSpeed;
const auto yVelocity =
-m_yVelocityLimiter.Calculate(m_controller.GetLeftX()) *
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). Gamepads return positive values when you pull to
// the right by default.
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
Drivetrain::kMaxAngularVelocity;
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
m_mecanum.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
}
};

19
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.hpp
View File

@@ -14,7 +14,7 @@
#include "wpi/math/estimator/MecanumDrivePoseEstimator.hpp"
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
/**
* Represents a mecanum drive style drivetrain.
@@ -30,17 +30,18 @@ class Drivetrain {
m_backRightMotor.SetInverted(true);
}
wpi::math::MecanumDriveWheelSpeeds GetCurrentState() const;
wpi::math::MecanumDriveWheelPositions GetCurrentDistances() const;
void SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
wpi::math::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
wpi::math::MecanumDriveWheelVelocities GetCurrentWheelVelocities() const;
void SetVelocities(
const wpi::math::MecanumDriveWheelVelocities& wheelVelocities);
void Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
static constexpr auto kMaxVelocity = 3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularVelocity{
std::numbers::pi}; // 1/2 rotation per second
private:
@@ -78,6 +79,6 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::MecanumDrivePoseEstimator m_poseEstimator{
m_kinematics, m_imu.GetRotation2d(), GetCurrentDistances(),
m_kinematics, m_imu.GetRotation2d(), GetCurrentWheelDistances(),
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
};

5
wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp
View File

@@ -7,7 +7,6 @@
#include "wpi/hardware/motor/PWMSparkMax.hpp"
#include "wpi/hardware/rotation/AnalogPotentiometer.hpp"
#include "wpi/hardware/rotation/Encoder.hpp"
#include "wpi/simulation/SimHooks.hpp"
#include "wpi/smartdashboard/Mechanism2d.hpp"
#include "wpi/smartdashboard/MechanismLigament2d.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
@@ -45,8 +44,8 @@ class Robot : public wpi::TimedRobot {
}
void TeleopPeriodic() override {
m_elevatorMotor.Set(m_joystick.GetRawAxis(0));
m_wristMotor.Set(m_joystick.GetRawAxis(1));
m_elevatorMotor.SetDutyCycle(m_joystick.GetRawAxis(0));
m_wristMotor.SetDutyCycle(m_joystick.GetRawAxis(1));
}
private:

2
wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp
View File

@@ -23,7 +23,7 @@
*/
class Robot : public wpi::TimedRobot {
public:
void TeleopPeriodic() override { m_motor.Set(m_stick.GetY()); }
void TeleopPeriodic() override { m_motor.SetDutyCycle(m_stick.GetY()); }
/*
* The RobotPeriodic function is called every control packet no matter the

2
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
View File

@@ -18,7 +18,7 @@ void Robot::TeleopPeriodic() {
double pidOut = m_pidController.Calculate(position.value());
// Apply PID output
m_elevatorMotor.Set(pidOut);
m_elevatorMotor.SetDutyCycle(pidOut);
// when the button is pressed once, the selected elevator setpoint is
// incremented

6
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.hpp
View File

@@ -35,13 +35,13 @@ class Robot : public wpi::TimedRobot {
{0.2_m, 0.8_m, 1.4_m}};
private:
// proportional speed constant
// proportional velocity constant
// negative because applying positive voltage will bring us closer to the
// target
static constexpr double kP = 0.7;
// integral speed constant
// integral velocity constant
static constexpr double kI = 0.35;
// derivative speed constant
// derivative velocity constant
static constexpr double kD = 0.25;
// Scaling is handled internally

2
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp
View File

@@ -45,6 +45,6 @@ void RapidReactCommandBot::ConfigureBindings() {
wpi::cmd::CommandPtr RapidReactCommandBot::GetAutonomousCommand() {
return m_drive
.DriveDistanceCommand(AutoConstants::kDriveDistance,
AutoConstants::kDriveSpeed)
AutoConstants::kDriveVelocity)
.WithTimeout(AutoConstants::kTimeout);
}

6
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp
View File

@@ -43,14 +43,14 @@ wpi::cmd::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
}
wpi::cmd::CommandPtr Drive::DriveDistanceCommand(wpi::units::meter_t distance,
double speed) {
double velocity) {
return RunOnce([this] {
// Reset encoders at the start of the command
m_leftEncoder.Reset();
m_rightEncoder.Reset();
})
// Drive forward at specified speed
.AndThen(Run([this, speed] { m_drive.ArcadeDrive(speed, 0.0); }))
// Drive forward at specified velocity
.AndThen(Run([this, velocity] { m_drive.ArcadeDrive(velocity, 0.0); }))
.Until([this, distance] {
return wpi::units::math::max(
wpi::units::meter_t(m_leftEncoder.GetDistance()),

2
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Intake.cpp
View File

@@ -6,7 +6,7 @@
wpi::cmd::CommandPtr Intake::IntakeCommand() {
return RunOnce([this] { m_piston.Set(wpi::DoubleSolenoid::kForward); })
.AndThen(Run([this] { m_motor.Set(1.0); }))
.AndThen(Run([this] { m_motor.SetDutyCycle(1.0); }))
.WithName("Intake");
}

2
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Shooter.cpp
View File

@@ -34,6 +34,6 @@ wpi::cmd::CommandPtr Shooter::ShootCommand(
// run the feeder
wpi::cmd::cmd::WaitUntil([this] {
return m_shooterFeedback.AtSetpoint();
}).AndThen([this] { m_feederMotor.Set(1.0); }))
}).AndThen([this] { m_feederMotor.SetDutyCycle(1.0); }))
.WithName("Shoot");
}

2
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Storage.cpp
View File

@@ -10,5 +10,5 @@ Storage::Storage() {
}
wpi::cmd::CommandPtr Storage::RunCommand() {
return Run([this] { m_motor.Set(1.0); }).WithName("Run");
return Run([this] { m_motor.SetDutyCycle(1.0); }).WithName("Run");
}

4
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.hpp
View File

@@ -80,7 +80,7 @@ inline constexpr wpi::units::volt_t kS = 0.05_V;
// Should have value 12V at free speed
inline constexpr auto kV = 12_V / kShooterFree;
inline constexpr double kFeederSpeed = 0.5;
inline constexpr double kFeederVelocity = 0.5;
} // namespace ShooterConstants
namespace OIConstants {
@@ -90,5 +90,5 @@ inline constexpr int kDriverControllerPort = 0;
namespace AutoConstants {
constexpr wpi::units::second_t kTimeout = 3_s;
constexpr wpi::units::meter_t kDriveDistance = 2_m;
constexpr double kDriveSpeed = 0.5;
constexpr double kDriveVelocity = 0.5;
} // namespace AutoConstants

10
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.hpp
View File

@@ -32,13 +32,13 @@ class Drive : public wpi::cmd::SubsystemBase {
/**
* Returns a command that drives the robot forward a specified distance at a
* specified speed.
* specified 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
*/
wpi::cmd::CommandPtr DriveDistanceCommand(wpi::units::meter_t distance,
double speed);
double velocity);
/**
* Returns a command that turns to robot to the specified angle using a motion
@@ -55,8 +55,8 @@ class Drive : public wpi::cmd::SubsystemBase {
wpi::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port};
wpi::DifferentialDrive m_drive{
[&](double output) { m_leftLeader.Set(output); },
[&](double output) { m_rightLeader.Set(output); }};
[&](double output) { m_leftLeader.SetDutyCycle(output); },
[&](double output) { m_rightLeader.SetDutyCycle(output); }};
wpi::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
DriveConstants::kLeftEncoderPorts[1],

2
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveDistance.cpp
View File

@@ -12,7 +12,7 @@ void DriveDistance::Initialize() {
}
void DriveDistance::Execute() {
m_drive->ArcadeDrive(m_speed, 0);
m_drive->ArcadeDrive(m_velocity, 0);
}
void DriveDistance::End(bool interrupted) {

2
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveTime.cpp
View File

@@ -10,7 +10,7 @@ void DriveTime::Initialize() {
}
void DriveTime::Execute() {
m_drive->ArcadeDrive(m_speed, 0);
m_drive->ArcadeDrive(m_velocity, 0);
}
void DriveTime::End(bool interrupted) {

6
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp
View File

@@ -9,14 +9,14 @@
#include "subsystems/Drivetrain.hpp"
TeleopArcadeDrive::TeleopArcadeDrive(
Drivetrain* subsystem, std::function<double()> xaxisSpeedSupplier,
Drivetrain* subsystem, std::function<double()> xaxisVelocitySupplier,
std::function<double()> zaxisRotateSuppplier)
: m_drive{subsystem},
m_xaxisSpeedSupplier{std::move(xaxisSpeedSupplier)},
m_xaxisVelocitySupplier{std::move(xaxisVelocitySupplier)},
m_zaxisRotateSupplier{std::move(zaxisRotateSuppplier)} {
AddRequirements(subsystem);
}
void TeleopArcadeDrive::Execute() {
m_drive->ArcadeDrive(m_xaxisSpeedSupplier(), m_zaxisRotateSupplier());
m_drive->ArcadeDrive(m_xaxisVelocitySupplier(), m_zaxisRotateSupplier());
}

2
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp
View File

@@ -15,7 +15,7 @@ void TurnDegrees::Initialize() {
}
void TurnDegrees::Execute() {
m_drive->ArcadeDrive(0, m_speed);
m_drive->ArcadeDrive(0, m_velocity);
}
void TurnDegrees::End(bool interrupted) {

2
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnTime.cpp
View File

@@ -10,7 +10,7 @@ void TurnTime::Initialize() {
}
void TurnTime::Execute() {
m_drive->ArcadeDrive(0, m_speed);
m_drive->ArcadeDrive(0, m_velocity);
}
void TurnTime::End(bool interrupted) {

4
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp
View File

@@ -34,8 +34,8 @@ void Drivetrain::Periodic() {
// This method will be called once per scheduler run.
}
void Drivetrain::ArcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_drive.ArcadeDrive(xaxisSpeed, zaxisRotate);
void Drivetrain::ArcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_drive.ArcadeDrive(xaxisVelocity, zaxisRotate);
}
void Drivetrain::ResetEncoders() {

11
wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.hpp
View File

@@ -14,14 +14,15 @@ class DriveDistance
public:
/**
* Creates a new DriveDistance. This command will drive your your robot for a
* desired distance at a desired speed.
* desired distance at a desired velocity.
*
* @param speed The speed at which the robot will drive
* @param velocity The velocity at which the robot will drive
* @param distance The distance the robot will drive
* @param drive The drivetrain subsystem on which this command will run
*/
DriveDistance(double speed, wpi::units::meter_t distance, Drivetrain* drive)
: m_speed(speed), m_distance(distance), m_drive(drive) {
DriveDistance(double velocity, wpi::units::meter_t distance,
Drivetrain* drive)
: m_velocity(velocity), m_distance(distance), m_drive(drive) {
AddRequirements(m_drive);
}
@@ -31,7 +32,7 @@ class DriveDistance
bool IsFinished() override;
private:
double m_speed;
double m_velocity;
wpi::units::meter_t m_distance;
Drivetrain* m_drive;
};

11
wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.hpp
View File

@@ -14,14 +14,15 @@ class DriveTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveTime> {
public:
/**
* Creates a new DriveTime. This command will drive your robot for a desired
* speed and time.
* 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
* @param drive The drivetrain subsystem on which this command will run
*/
DriveTime(double speed, wpi::units::second_t time, Drivetrain* drive)
: m_speed(speed), m_duration(time), m_drive(drive) {
DriveTime(double velocity, wpi::units::second_t time, Drivetrain* drive)
: m_velocity(velocity), m_duration(time), m_drive(drive) {
AddRequirements(m_drive);
}
@@ -31,7 +32,7 @@ class DriveTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveTime> {
bool IsFinished() override;
private:
double m_speed;
double m_velocity;
wpi::units::second_t m_duration;
Drivetrain* m_drive;
wpi::Timer m_timer;

10
wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TeleopArcadeDrive.hpp
View File

@@ -15,19 +15,19 @@ class TeleopArcadeDrive
public:
/**
* Creates a new ArcadeDrive. This command will drive your robot according to
* the speed suppliers. This command does not terminate.
* the velocity suppliers. This command does not terminate.
*
* @param drivetrain The drivetrain subsystem on which this command will run
* @param xaxisSpeedSupplier Supplier of forward/backward speed
* @param zaxisRotateSupplier Supplier of rotational speed
* @param xaxisVelocitySupplier Supplier of forward/backward velocity
* @param zaxisRotateSupplier Supplier of rotational velocity
*/
TeleopArcadeDrive(Drivetrain* subsystem,
std::function<double()> xaxisSpeedSupplier,
std::function<double()> xaxisVelocitySupplier,
std::function<double()> zaxisRotateSupplier);
void Execute() override;
private:
Drivetrain* m_drive;
std::function<double()> m_xaxisSpeedSupplier;
std::function<double()> m_xaxisVelocitySupplier;
std::function<double()> m_zaxisRotateSupplier;
};

11
wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.hpp
View File

@@ -15,14 +15,15 @@ class TurnDegrees
public:
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired
* rotation (in degrees) and rotational speed.
* rotation (in 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
*/
TurnDegrees(double speed, wpi::units::degree_t angle, Drivetrain* drive)
: m_speed(speed), m_angle(angle), m_drive(drive) {
TurnDegrees(double velocity, wpi::units::degree_t angle, Drivetrain* drive)
: m_velocity(velocity), m_angle(angle), m_drive(drive) {
AddRequirements(m_drive);
}
@@ -32,7 +33,7 @@ class TurnDegrees
bool IsFinished() override;
private:
double m_speed;
double m_velocity;
wpi::units::degree_t m_angle;
Drivetrain* m_drive;

9
wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.hpp
View File

@@ -15,12 +15,13 @@ class TurnTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnTime> {
/**
* 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
* @param drive The drive subsystem on which this command will run
*/
TurnTime(double speed, wpi::units::second_t time, Drivetrain* drive)
: m_speed(speed), m_duration(time), m_drive(drive) {
TurnTime(double velocity, wpi::units::second_t time, Drivetrain* drive)
: m_velocity(velocity), m_duration(time), m_drive(drive) {
AddRequirements(m_drive);
}
@@ -30,7 +31,7 @@ class TurnTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnTime> {
bool IsFinished() override;
private:
double m_speed;
double m_velocity;
wpi::units::second_t m_duration;
Drivetrain* m_drive;
wpi::Timer m_timer;

9
wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.hpp
View File

@@ -9,7 +9,6 @@
#include "wpi/hardware/motor/Spark.hpp"
#include "wpi/hardware/rotation/Encoder.hpp"
#include "wpi/romi/RomiGyro.hpp"
#include "wpi/units/acceleration.hpp"
#include "wpi/units/angle.hpp"
#include "wpi/units/length.hpp"
@@ -28,10 +27,10 @@ class Drivetrain : public wpi::cmd::SubsystemBase {
/**
* Drives the robot using arcade controls.
*
* @param xaxisSpeed the commanded forward movement
* @param xaxisVelocity the commanded forward movement
* @param zaxisRotate the commanded rotation
*/
void ArcadeDrive(double xaxisSpeed, double zaxisRotate);
void ArcadeDrive(double xaxisVelocity, double zaxisRotate);
/**
* Resets the drive encoders to currently read a position of 0.
@@ -107,8 +106,8 @@ class Drivetrain : public wpi::cmd::SubsystemBase {
wpi::Encoder m_rightEncoder{6, 7};
wpi::DifferentialDrive m_drive{
[&](double output) { m_leftMotor.Set(output); },
[&](double output) { m_rightMotor.Set(output); }};
[&](double output) { m_leftMotor.SetDutyCycle(output); },
[&](double output) { m_rightMotor.SetDutyCycle(output); }};
wpi::romi::RomiGyro m_gyro;
};

25
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
View File

@@ -6,22 +6,22 @@
#include "wpi/system/RobotController.hpp"
void Drivetrain::SetSpeeds(
const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
auto leftFeedforward = m_feedforward.Calculate(speeds.left);
auto rightFeedforward = m_feedforward.Calculate(speeds.right);
void Drivetrain::SetVelocities(
const wpi::math::DifferentialDriveWheelVelocities& velocities) {
auto leftFeedforward = m_feedforward.Calculate(velocities.left);
auto rightFeedforward = m_feedforward.Calculate(velocities.right);
double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
speeds.left.value());
velocities.left.value());
double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
speeds.right.value());
velocities.right.value());
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot) {
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
SetVelocities(m_kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
}
void Drivetrain::UpdateOdometry() {
@@ -42,10 +42,11 @@ void Drivetrain::SimulationPeriodic() {
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro. We negate the right side so that positive
// voltages make the right side move forward.
m_drivetrainSimulator.SetInputs(wpi::units::volt_t{m_leftLeader.Get()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{m_rightLeader.Get()} *
wpi::RobotController::GetInputVoltage());
m_drivetrainSimulator.SetInputs(
wpi::units::volt_t{m_leftLeader.GetDutyCycle()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{m_rightLeader.GetDutyCycle()} *
wpi::RobotController::GetInputVoltage());
m_drivetrainSimulator.Update(20_ms);
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());

17
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp
View File

@@ -29,24 +29,25 @@ class Robot : public wpi::TimedRobot {
void AutonomousPeriodic() override {
auto elapsed = m_timer.Get();
auto reference = m_trajectory.Sample(elapsed);
auto speeds = m_feedback.Calculate(m_drive.GetPose(), reference);
m_drive.Drive(speeds.vx, speeds.omega);
auto velocities = m_feedback.Calculate(m_drive.GetPose(), reference);
m_drive.Drive(velocities.vx, velocities.omega);
}
void TeleopPeriodic() override {
// 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.
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxSpeed;
const auto xVelocity =
-m_velocityLimiter.Calculate(m_controller.GetLeftY()) *
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.
auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
Drivetrain::kMaxAngularVelocity;
m_drive.Drive(xSpeed, rot);
m_drive.Drive(xVelocity, rot);
}
void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
@@ -56,7 +57,7 @@ class Robot : public wpi::TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_speedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;

9
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.hpp
View File

@@ -54,13 +54,14 @@ class Drivetrain {
wpi::SmartDashboard::PutData("Field", &m_fieldSim);
}
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxVelocity =
3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularVelocity{
std::numbers::pi}; // 1/2 rotation per second
void SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
void SetVelocities(
const wpi::math::DifferentialDriveWheelVelocities& velocities);
void Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot);
void UpdateOdometry();
void ResetOdometry(const wpi::math::Pose2d& pose);

2
wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
View File

@@ -89,7 +89,7 @@ class Robot : public wpi::TimedRobot {
}
void TeleopPeriodic() override {
// Sets the target speed of our flywheel. This is similar to setting the
// Sets the target velocity of our flywheel. This is similar to setting the
// setpoint of a PID controller.
if (m_joystick.GetRightBumperButton()) {
// We pressed the bumper, so let's set our next reference

2
wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
View File

@@ -86,7 +86,7 @@ class Robot : public wpi::TimedRobot {
}
void TeleopPeriodic() override {
// Sets the target speed of our flywheel. This is similar to setting the
// Sets the target velocity of our flywheel. This is similar to setting the
// setpoint of a PID controller.
if (m_joystick.GetRightBumperButton()) {
// We pressed the bumper, so let's set our next reference

23
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -4,24 +4,25 @@
#include "Drivetrain.hpp"
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period) {
wpi::math::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
wpi::math::ChassisVelocities chassisVelocities{xVelocity, yVelocity, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_imu.GetRotation2d());
chassisVelocities =
chassisVelocities.ToRobotRelative(m_imu.GetRotation2d());
}
chassisSpeeds = chassisSpeeds.Discretize(period);
chassisVelocities = chassisVelocities.Discretize(period);
auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
auto states = m_kinematics.ToSwerveModuleVelocities(chassisVelocities);
m_kinematics.DesaturateWheelVelocities(&states, kMaxVelocity);
auto [fl, fr, bl, br] = states;
m_frontLeft.SetDesiredState(fl);
m_frontRight.SetDesiredState(fr);
m_backLeft.SetDesiredState(bl);
m_backRight.SetDesiredState(br);
m_frontLeft.SetDesiredVelocity(fl);
m_frontRight.SetDesiredVelocity(fr);
m_backLeft.SetDesiredVelocity(bl);
m_backRight.SetDesiredVelocity(br);
}
void Drivetrain::UpdateOdometry() {

28
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
View File

@@ -23,23 +23,25 @@ class Robot : public wpi::TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xSpeed = -m_xspeedLimiter.Calculate(wpi::math::ApplyDeadband(
m_controller.GetLeftY(), 0.02)) *
Drivetrain::kMaxSpeed;
const auto xVelocity =
-m_xVelocityLimiter.Calculate(
wpi::math::ApplyDeadband(m_controller.GetLeftY(), 0.02)) *
Drivetrain::kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// Get the y velocity or sideways/strafe velocity. We are inverting this
// because we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
const auto ySpeed = -m_yspeedLimiter.Calculate(wpi::math::ApplyDeadband(
m_controller.GetLeftX(), 0.02)) *
Drivetrain::kMaxSpeed;
const auto yVelocity =
-m_yVelocityLimiter.Calculate(
wpi::math::ApplyDeadband(m_controller.GetLeftX(), 0.02)) *
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
@@ -47,9 +49,9 @@ class Robot : public wpi::TimedRobot {
// the right by default.
const auto rot = -m_rotLimiter.Calculate(wpi::math::ApplyDeadband(
m_controller.GetRightX(), 0.02)) *
Drivetrain::kMaxAngularSpeed;
Drivetrain::kMaxAngularVelocity;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
m_swerve.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
}
};

28
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
View File

@@ -37,40 +37,40 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
wpi::units::radian_t{std::numbers::pi});
}
wpi::math::SwerveModuleState SwerveModule::GetState() const {
return {wpi::units::meters_per_second_t{m_driveEncoder.GetRate()},
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
wpi::math::SwerveModulePosition SwerveModule::GetPosition() const {
return {wpi::units::meter_t{m_driveEncoder.GetDistance()},
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
wpi::math::SwerveModuleState& referenceState) {
wpi::math::SwerveModuleVelocity SwerveModule::GetVelocity() const {
return {wpi::units::meters_per_second_t{m_driveEncoder.GetRate()},
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredVelocity(
wpi::math::SwerveModuleVelocity& desiredVelocity) {
wpi::math::Rotation2d encoderRotation{
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
// Optimize the reference state to avoid spinning further than 90 degrees
referenceState.Optimize(encoderRotation);
// Optimize the desired velocity to avoid spinning further than 90 degrees
desiredVelocity.Optimize(encoderRotation);
// Scale speed by cosine of angle error. This scales down movement
// 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.
referenceState.CosineScale(encoderRotation);
desiredVelocity.CosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(
m_driveEncoder.GetRate(), referenceState.speed.value());
m_driveEncoder.GetRate(), desiredVelocity.velocity.value());
const auto driveFeedforward =
m_driveFeedforward.Calculate(referenceState.speed);
m_driveFeedforward.Calculate(desiredVelocity.velocity);
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
wpi::units::radian_t{m_turningEncoder.GetDistance()},
referenceState.angle.Radians());
desiredVelocity.angle.Radians());
const auto turnFeedforward = m_turnFeedforward.Calculate(
m_turningPIDController.GetSetpoint().velocity);

8
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.hpp
View File

@@ -19,15 +19,15 @@ class Drivetrain {
public:
Drivetrain() { m_imu.ResetYaw(); }
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
void Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxVelocity =
3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularVelocity{
std::numbers::pi}; // 1/2 rotation per second
private:

6
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.hpp
View File

@@ -12,7 +12,7 @@
#include "wpi/math/controller/ProfiledPIDController.hpp"
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/time.hpp"
#include "wpi/units/velocity.hpp"
@@ -23,9 +23,9 @@ class SwerveModule {
SwerveModule(int driveMotorChannel, int turningMotorChannel,
int driveEncoderChannelA, int driveEncoderChannelB,
int turningEncoderChannelA, int turningEncoderChannelB);
wpi::math::SwerveModuleState GetState() const;
wpi::math::SwerveModulePosition GetPosition() const;
void SetDesiredState(wpi::math::SwerveModuleState& state);
wpi::math::SwerveModuleVelocity GetVelocity() const;
void SetDesiredVelocity(wpi::math::SwerveModuleVelocity& desiredVelocity);
private:
static constexpr double kWheelRadius = 0.0508;

22
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -7,25 +7,25 @@
#include "ExampleGlobalMeasurementSensor.hpp"
#include "wpi/system/Timer.hpp"
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period) {
wpi::math::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
wpi::math::ChassisVelocities chassisVelocities{xVelocity, yVelocity, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(
chassisVelocities = chassisVelocities.ToRobotRelative(
m_poseEstimator.GetEstimatedPosition().Rotation());
}
chassisSpeeds = chassisSpeeds.Discretize(period);
chassisVelocities = chassisVelocities.Discretize(period);
auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
auto states = m_kinematics.ToSwerveModuleVelocities(chassisVelocities);
m_kinematics.DesaturateWheelVelocities(&states, kMaxVelocity);
auto [fl, fr, bl, br] = states;
m_frontLeft.SetDesiredState(fl);
m_frontRight.SetDesiredState(fr);
m_backLeft.SetDesiredState(bl);
m_backRight.SetDesiredState(br);
m_frontLeft.SetDesiredVelocity(fl);
m_frontRight.SetDesiredVelocity(fr);
m_backLeft.SetDesiredVelocity(bl);
m_backRight.SetDesiredVelocity(br);
}
void Drivetrain::UpdateOdometry() {

24
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp
View File

@@ -22,30 +22,32 @@ class Robot : public wpi::TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxSpeed;
const auto xVelocity =
-m_xVelocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// Get the y velocity or sideways/strafe velocity. We are inverting this
// because we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
Drivetrain::kMaxSpeed;
const auto yVelocity =
-m_yVelocityLimiter.Calculate(m_controller.GetLeftX()) *
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). Gamepads return positive values when you pull to
// the right by default.
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
Drivetrain::kMaxAngularVelocity;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
m_swerve.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
}
};

28
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
View File

@@ -37,40 +37,40 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
wpi::units::radian_t{std::numbers::pi});
}
wpi::math::SwerveModuleState SwerveModule::GetState() const {
return {wpi::units::meters_per_second_t{m_driveEncoder.GetRate()},
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
wpi::math::SwerveModulePosition SwerveModule::GetPosition() const {
return {wpi::units::meter_t{m_driveEncoder.GetDistance()},
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
wpi::math::SwerveModuleState& referenceState) {
wpi::math::SwerveModuleVelocity SwerveModule::GetVelocity() const {
return {wpi::units::meters_per_second_t{m_driveEncoder.GetRate()},
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredVelocity(
wpi::math::SwerveModuleVelocity& desiredVelocity) {
wpi::math::Rotation2d encoderRotation{
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
// Optimize the reference state to avoid spinning further than 90 degrees
referenceState.Optimize(encoderRotation);
// Optimize the desired velocity to avoid spinning further than 90 degrees
desiredVelocity.Optimize(encoderRotation);
// Scale speed by cosine of angle error. This scales down movement
// 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.
referenceState.CosineScale(encoderRotation);
desiredVelocity.CosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(
m_driveEncoder.GetRate(), referenceState.speed.value());
m_driveEncoder.GetRate(), desiredVelocity.velocity.value());
const auto driveFeedforward =
m_driveFeedforward.Calculate(referenceState.speed);
m_driveFeedforward.Calculate(desiredVelocity.velocity);
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
wpi::units::radian_t{m_turningEncoder.GetDistance()},
referenceState.angle.Radians());
desiredVelocity.angle.Radians());
const auto turnFeedforward = m_turnFeedforward.Calculate(
m_turningPIDController.GetSetpoint().velocity);

8
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.hpp
View File

@@ -19,14 +19,14 @@ class Drivetrain {
public:
Drivetrain() { m_imu.ResetYaw(); }
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
void Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::meters_per_second_t yVelocity,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
static constexpr auto kMaxVelocity = 3.0_mps; // 3 meters per second
static constexpr wpi::units::radians_per_second_t kMaxAngularVelocity{
std::numbers::pi}; // 1/2 rotation per second
private:

6
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.hpp
View File

@@ -12,7 +12,7 @@
#include "wpi/math/controller/ProfiledPIDController.hpp"
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/time.hpp"
#include "wpi/units/velocity.hpp"
@@ -23,9 +23,9 @@ class SwerveModule {
SwerveModule(int driveMotorChannel, int turningMotorChannel,
int driveEncoderChannelA, int driveEncoderChannelB,
int turningEncoderChannelA, int turningEncoderChannelB);
wpi::math::SwerveModuleState GetState() const;
wpi::math::SwerveModulePosition GetPosition() const;
void SetDesiredState(wpi::math::SwerveModuleState& state);
wpi::math::SwerveModuleVelocity GetVelocity() const;
void SetDesiredVelocity(wpi::math::SwerveModuleVelocity& desiredVelocity);
private:
static constexpr auto kWheelRadius = 0.0508_m;

12
wpilibcExamples/src/main/cpp/examples/SysIdRoutine/cpp/subsystems/Shooter.cpp
View File

@@ -14,18 +14,18 @@ Shooter::Shooter() {
}
wpi::cmd::CommandPtr Shooter::RunShooterCommand(
std::function<double()> shooterSpeed) {
std::function<double()> shooterVelocity) {
return wpi::cmd::cmd::Run(
[this, shooterSpeed] {
[this, shooterVelocity] {
m_shooterMotor.SetVoltage(
wpi::units::volt_t{m_shooterFeedback.Calculate(
m_shooterEncoder.GetRate(), shooterSpeed())} +
m_shooterEncoder.GetRate(), shooterVelocity())} +
m_shooterFeedforward.Calculate(
wpi::units::turns_per_second_t{shooterSpeed()}));
m_feederMotor.Set(constants::shooter::kFeederSpeed);
wpi::units::turns_per_second_t{shooterVelocity()}));
m_feederMotor.SetDutyCycle(constants::shooter::kFeederVelocity);
},
{this})
.WithName("Set Shooter Speed");
.WithName("Set Shooter Velocity");
}
wpi::cmd::CommandPtr Shooter::SysIdQuasistatic(

7
wpilibcExamples/src/main/cpp/examples/SysIdRoutine/include/Constants.hpp
View File

@@ -54,7 +54,8 @@ inline constexpr int kShooterMotorPort = 4;
inline constexpr int kFeederMotorPort = 5;
inline constexpr wpi::units::turns_per_second_t kShooterFreeSpeed = 5300_tps;
inline constexpr wpi::units::turns_per_second_t kShooterTargetSpeed = 4000_tps;
inline constexpr wpi::units::turns_per_second_t kShooterTargetVelocity =
4000_tps;
inline constexpr wpi::units::turns_per_second_t kShooterTolerance = 50_tps;
inline constexpr double kP = 1.0;
@@ -63,7 +64,7 @@ inline constexpr wpi::units::volt_t kS = 0.05_V;
inline constexpr kv_unit_t kV = 12_V / kShooterFreeSpeed;
inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / 1_tr;
inline constexpr double kFeederSpeed = 0.5;
inline constexpr double kFeederVelocity = 0.5;
} // namespace shooter
namespace intake {
@@ -79,7 +80,7 @@ inline constexpr int kBallSensorPort = 6;
namespace autonomous {
inline constexpr wpi::units::second_t kTimeout = 3_s;
inline constexpr wpi::units::meter_t kDriveDistance = 2_m;
inline constexpr double kDriveSpeed = 0.5;
inline constexpr double kDriveVelocity = 0.5;
} // namespace autonomous
namespace oi {

9
wpilibcExamples/src/main/cpp/examples/SysIdRoutine/include/subsystems/Drive.hpp
View File

@@ -26,8 +26,9 @@ class Drive : public wpi::cmd::SubsystemBase {
private:
wpi::PWMSparkMax m_leftMotor{constants::drive::kLeftMotor1Port};
wpi::PWMSparkMax m_rightMotor{constants::drive::kRightMotor1Port};
wpi::DifferentialDrive m_drive{[this](auto val) { m_leftMotor.Set(val); },
[this](auto val) { m_rightMotor.Set(val); }};
wpi::DifferentialDrive m_drive{
[this](auto val) { m_leftMotor.SetDutyCycle(val); },
[this](auto val) { m_rightMotor.SetDutyCycle(val); }};
wpi::Encoder m_leftEncoder{constants::drive::kLeftEncoderPorts[0],
constants::drive::kLeftEncoderPorts[1],
@@ -47,13 +48,13 @@ class Drive : public wpi::cmd::SubsystemBase {
},
[this](wpi::sysid::SysIdRoutineLog* log) {
log->Motor("drive-left")
.voltage(m_leftMotor.Get() *
.voltage(m_leftMotor.GetDutyCycle() *
wpi::RobotController::GetBatteryVoltage())
.position(wpi::units::meter_t{m_leftEncoder.GetDistance()})
.velocity(
wpi::units::meters_per_second_t{m_leftEncoder.GetRate()});
log->Motor("drive-right")
.voltage(m_rightMotor.Get() *
.voltage(m_rightMotor.GetDutyCycle() *
wpi::RobotController::GetBatteryVoltage())
.position(wpi::units::meter_t{m_rightEncoder.GetDistance()})
.velocity(

5
wpilibcExamples/src/main/cpp/examples/SysIdRoutine/include/subsystems/Shooter.hpp
View File

@@ -19,7 +19,8 @@ class Shooter : public wpi::cmd::SubsystemBase {
public:
Shooter();
wpi::cmd::CommandPtr RunShooterCommand(std::function<double()> shooterSpeed);
wpi::cmd::CommandPtr RunShooterCommand(
std::function<double()> shooterVelocity);
wpi::cmd::CommandPtr SysIdQuasistatic(wpi::cmd::sysid::Direction direction);
wpi::cmd::CommandPtr SysIdDynamic(wpi::cmd::sysid::Direction direction);
@@ -40,7 +41,7 @@ class Shooter : public wpi::cmd::SubsystemBase {
},
[this](wpi::sysid::SysIdRoutineLog* log) {
log->Motor("shooter-wheel")
.voltage(m_shooterMotor.Get() *
.voltage(m_shooterMotor.GetDutyCycle() *
wpi::RobotController::GetBatteryVoltage())
.position(wpi::units::turn_t{m_shooterEncoder.GetDistance()})
.velocity(

4
wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp
View File

@@ -15,8 +15,8 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_leftMotor{0};
wpi::PWMSparkMax m_rightMotor{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_leftMotor.Set(output); },
[&](double output) { m_rightMotor.Set(output); }};
[&](double output) { m_leftMotor.SetDutyCycle(output); },
[&](double output) { m_rightMotor.SetDutyCycle(output); }};
wpi::Joystick m_leftStick{0};
wpi::Joystick m_rightStick{1};

4
wpilibcExamples/src/main/cpp/examples/TankDriveGamepad/cpp/Robot.cpp
View File

@@ -15,8 +15,8 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_leftMotor{0};
wpi::PWMSparkMax m_rightMotor{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_leftMotor.Set(output); },
[&](double output) { m_rightMotor.Set(output); }};
[&](double output) { m_leftMotor.SetDutyCycle(output); },
[&](double output) { m_rightMotor.SetDutyCycle(output); }};
wpi::Gamepad m_driverController{0};
public:

2
wpilibcExamples/src/main/cpp/examples/UnitTest/cpp/Robot.cpp
View File

@@ -10,7 +10,7 @@
void Robot::TeleopPeriodic() {
// Activate the intake while the trigger is held
if (m_joystick.GetTrigger()) {
m_intake.Activate(IntakeConstants::kIntakeSpeed);
m_intake.Activate(IntakeConstants::kIntakeVelocity);
} else {
m_intake.Activate(0);
}

8
wpilibcExamples/src/main/cpp/examples/UnitTest/cpp/subsystems/Intake.cpp
View File

@@ -10,14 +10,14 @@ void Intake::Deploy() {
void Intake::Retract() {
m_piston.Set(wpi::DoubleSolenoid::Value::kReverse);
m_motor.Set(0); // turn off the motor
m_motor.SetDutyCycle(0); // turn off the motor
}
void Intake::Activate(double speed) {
void Intake::Activate(double velocity) {
if (IsDeployed()) {
m_motor.Set(speed);
m_motor.SetDutyCycle(velocity);
} else { // if piston isn't open, do nothing
m_motor.Set(0);
m_motor.SetDutyCycle(0);
}
}

2
wpilibcExamples/src/main/cpp/examples/UnitTest/include/Constants.hpp
View File

@@ -18,7 +18,7 @@ inline constexpr int kMotorPort = 1;
inline constexpr int kPistonFwdChannel = 0;
inline constexpr int kPistonRevChannel = 1;
inline constexpr double kIntakeSpeed = 0.5;
inline constexpr double kIntakeVelocity = 0.5;
} // namespace IntakeConstants
namespace OperatorConstants {

2
wpilibcExamples/src/main/cpp/examples/UnitTest/include/subsystems/Intake.hpp
View File

@@ -12,7 +12,7 @@ class Intake {
public:
void Deploy();
void Retract();
void Activate(double speed);
void Activate(double velocity);
bool IsDeployed() const;
private:

2
wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/commands/DriveDistance.cpp
View File

@@ -12,7 +12,7 @@ void DriveDistance::Initialize() {
}
void DriveDistance::Execute() {
m_drive->ArcadeDrive(m_speed, 0);
m_drive->ArcadeDrive(m_velocity, 0);
}
void DriveDistance::End(bool interrupted) {

2
wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/commands/DriveTime.cpp
View File

@@ -10,7 +10,7 @@ void DriveTime::Initialize() {
}
void DriveTime::Execute() {
m_drive->ArcadeDrive(m_speed, 0);
m_drive->ArcadeDrive(m_velocity, 0);
}
void DriveTime::End(bool interrupted) {

6
wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/commands/TeleopArcadeDrive.cpp
View File

@@ -7,14 +7,14 @@
#include "subsystems/Drivetrain.hpp"
TeleopArcadeDrive::TeleopArcadeDrive(
Drivetrain* subsystem, std::function<double()> xaxisSpeedSupplier,
Drivetrain* subsystem, std::function<double()> xaxisVelocitySupplier,
std::function<double()> zaxisRotateSuppplier)
: m_drive{subsystem},
m_xaxisSpeedSupplier{xaxisSpeedSupplier},
m_xaxisVelocitySupplier{xaxisVelocitySupplier},
m_zaxisRotateSupplier{zaxisRotateSuppplier} {
AddRequirements(subsystem);
}
void TeleopArcadeDrive::Execute() {
m_drive->ArcadeDrive(m_xaxisSpeedSupplier(), m_zaxisRotateSupplier());
m_drive->ArcadeDrive(m_xaxisVelocitySupplier(), m_zaxisRotateSupplier());
}

2
wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/commands/TurnDegrees.cpp
View File

@@ -15,7 +15,7 @@ void TurnDegrees::Initialize() {
}
void TurnDegrees::Execute() {
m_drive->ArcadeDrive(0, m_speed);
m_drive->ArcadeDrive(0, m_velocity);
}
void TurnDegrees::End(bool interrupted) {

2
wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/commands/TurnTime.cpp
View File

@@ -10,7 +10,7 @@ void TurnTime::Initialize() {
}
void TurnTime::Execute() {
m_drive->ArcadeDrive(0, m_speed);
m_drive->ArcadeDrive(0, m_velocity);
}
void TurnTime::End(bool interrupted) {

4
wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp
View File

@@ -34,8 +34,8 @@ void Drivetrain::Periodic() {
// This method will be called once per scheduler run.
}
void Drivetrain::ArcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_drive.ArcadeDrive(xaxisSpeed, zaxisRotate);
void Drivetrain::ArcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_drive.ArcadeDrive(xaxisVelocity, zaxisRotate);
}
void Drivetrain::ResetEncoders() {

11
wpilibcExamples/src/main/cpp/examples/XRPReference/include/commands/DriveDistance.hpp
View File

@@ -14,14 +14,15 @@ class DriveDistance
public:
/**
* Creates a new DriveDistance. This command will drive your your robot for a
* desired distance at a desired speed.
* desired distance at a desired velocity.
*
* @param speed The speed at which the robot will drive
* @param velocity The velocity at which the robot will drive
* @param distance The distance the robot will drive
* @param drive The drivetrain subsystem on which this command will run
*/
DriveDistance(double speed, wpi::units::meter_t distance, Drivetrain* drive)
: m_speed(speed), m_distance(distance), m_drive(drive) {
DriveDistance(double velocity, wpi::units::meter_t distance,
Drivetrain* drive)
: m_velocity(velocity), m_distance(distance), m_drive(drive) {
AddRequirements(m_drive);
}
@@ -31,7 +32,7 @@ class DriveDistance
bool IsFinished() override;
private:
double m_speed;
double m_velocity;
wpi::units::meter_t m_distance;
Drivetrain* m_drive;
};

11
wpilibcExamples/src/main/cpp/examples/XRPReference/include/commands/DriveTime.hpp
View File

@@ -14,14 +14,15 @@ class DriveTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveTime> {
public:
/**
* Creates a new DriveTime. This command will drive your robot for a desired
* speed and time.
* 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
* @param drive The drivetrain subsystem on which this command will run
*/
DriveTime(double speed, wpi::units::second_t time, Drivetrain* drive)
: m_speed(speed), m_duration(time), m_drive(drive) {
DriveTime(double velocity, wpi::units::second_t time, Drivetrain* drive)
: m_velocity(velocity), m_duration(time), m_drive(drive) {
AddRequirements(m_drive);
}
@@ -31,7 +32,7 @@ class DriveTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveTime> {
bool IsFinished() override;
private:
double m_speed;
double m_velocity;
wpi::units::second_t m_duration;
Drivetrain* m_drive;
wpi::Timer m_timer;

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