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

Don't force public variables to use Hungarian notation (#8774)

Browse Source 
People generally have expressed a dislike for the Hungarian notation
used in member variables, especially in examples/templates, and our
styleguide shouldn't be forced on downstream consumers, so this removes
all Hungarian notation from the examples/templates.

There are _some_ benefits to Hungarian for private member variables
(like knowing what's a member vs. local in a PR review) so we'll keep
private member variables the same for now, but public variables should
no longer use Hungarian notation, since it looks much worse. A new PMD
XPath rule has been added to accomplish this goal. Some other
non-compliant variables were fixed for the new rule.
This commit is contained in:
Gold856
2026-04-25 14:32:08 -04:00
committed by GitHub
parent e7e51c9c05
commit 35e8abedeb
443 changed files with 4584 additions and 4789 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,30 +12,30 @@
* Runs the motors with split arcade steering and a Gamepad.
*/
class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_leftMotor{0};
wpi::PWMSparkMax m_rightMotor{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_leftMotor.SetThrottle(output); },
[&](double output) { m_rightMotor.SetThrottle(output); }};
wpi::Gamepad m_driverController{0};
wpi::PWMSparkMax leftMotor{0};
wpi::PWMSparkMax rightMotor{1};
wpi::DifferentialDrive robotDrive{
[&](double output) { leftMotor.SetThrottle(output); },
[&](double output) { rightMotor.SetThrottle(output); }};
wpi::Gamepad driverController{0};
public:
Robot() {
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
wpi::util::SendableRegistry::AddChild(&robotDrive, &leftMotor);
wpi::util::SendableRegistry::AddChild(&robotDrive, &rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.SetInverted(true);
rightMotor.SetInverted(true);
}
void TeleopPeriodic() override {
// Drive with split arcade style
// That means that the Y axis of the left stick moves forward
// and backward, and the X of the right stick turns left and right.
m_robotDrive.ArcadeDrive(-m_driverController.GetLeftY(),
-m_driverController.GetRightX());
robotDrive.ArcadeDrive(-driverController.GetLeftY(),
-driverController.GetRightX());
}
};

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

@@ -5,26 +5,26 @@
#include "Robot.hpp"
void Robot::SimulationPeriodic() {
m_arm.SimulationPeriodic();
arm.SimulationPeriodic();
}
void Robot::TeleopInit() {
m_arm.LoadPreferences();
arm.LoadPreferences();
}
void Robot::TeleopPeriodic() {
if (m_joystick.GetTrigger()) {
if (joystick.GetTrigger()) {
// Here, we run PID control like normal.
m_arm.ReachSetpoint();
arm.ReachSetpoint();
} else {
// Otherwise, we disable the motor.
m_arm.Stop();
arm.Stop();
}
}
void Robot::DisabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_arm.Stop();
arm.Stop();
}
#ifndef RUNNING_WPILIB_TESTS

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

@@ -9,55 +9,55 @@
#include "wpi/util/Preferences.hpp"
Arm::Arm() {
m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
// Put Mechanism 2d to SmartDashboard
wpi::SmartDashboard::PutData("Arm Sim", &m_mech2d);
wpi::SmartDashboard::PutData("Arm Sim", &mech2d);
// Set the Arm position setpoint and P constant to Preferences if the keys
// don't already exist
wpi::Preferences::InitDouble(kArmPositionKey, m_armSetpoint.value());
wpi::Preferences::InitDouble(kArmPKey, m_armKp);
wpi::Preferences::InitDouble(kArmPositionKey, armSetpoint.value());
wpi::Preferences::InitDouble(kArmPKey, armKp);
}
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.GetThrottle() * wpi::RobotController::GetInputVoltage()});
armSim.SetInput(wpi::math::Vectord<1>{
motor.GetThrottle() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_armSim.Update(20_ms);
armSim.Update(20_ms);
// Finally, we set our simulated encoder's readings and simulated battery
// voltage
m_encoderSim.SetDistance(m_armSim.GetAngle().value());
encoderSim.SetDistance(armSim.GetAngle().value());
// SimBattery estimates loaded battery voltages
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
wpi::sim::BatterySim::Calculate({armSim.GetCurrentDraw()}));
// Update the Mechanism Arm angle based on the simulated arm angle
m_arm->SetAngle(m_armSim.GetAngle());
arm->SetAngle(armSim.GetAngle());
}
void Arm::LoadPreferences() {
// Read Preferences for Arm setpoint and kP on entering Teleop
m_armSetpoint = wpi::units::degree_t{
wpi::Preferences::GetDouble(kArmPositionKey, m_armSetpoint.value())};
if (m_armKp != wpi::Preferences::GetDouble(kArmPKey, m_armKp)) {
m_armKp = wpi::Preferences::GetDouble(kArmPKey, m_armKp);
m_controller.SetP(m_armKp);
armSetpoint = wpi::units::degree_t{
wpi::Preferences::GetDouble(kArmPositionKey, armSetpoint.value())};
if (armKp != wpi::Preferences::GetDouble(kArmPKey, armKp)) {
armKp = wpi::Preferences::GetDouble(kArmPKey, armKp);
controller.SetP(armKp);
}
}
void Arm::ReachSetpoint() {
// Here, we run PID control like normal, with a setpoint read from
// preferences in degrees.
double pidOutput = m_controller.Calculate(
m_encoder.GetDistance(), (wpi::units::radian_t{m_armSetpoint}.value()));
m_motor.SetVoltage(wpi::units::volt_t{pidOutput});
double pidOutput = controller.Calculate(
encoder.GetDistance(), (wpi::units::radian_t{armSetpoint}.value()));
motor.SetVoltage(wpi::units::volt_t{pidOutput});
}
void Arm::Stop() {
m_motor.SetThrottle(0.0);
motor.SetThrottle(0.0);
}

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

@@ -20,6 +20,6 @@ class Robot : public wpi::TimedRobot {
void DisabledInit() override;
private:
wpi::Joystick m_joystick{kJoystickPort};
Arm m_arm;
wpi::Joystick joystick{kJoystickPort};
Arm arm;
};

30
wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.hpp
View File

@@ -29,23 +29,23 @@ class Arm {
private:
// The P gain for the PID controller that drives this arm.
double m_armKp = kDefaultArmKp;
wpi::units::degree_t m_armSetpoint = kDefaultArmSetpoint;
double armKp = kDefaultArmKp;
wpi::units::degree_t armSetpoint = kDefaultArmSetpoint;
// The arm gearbox represents a gearbox containing two Vex 775pro motors.
wpi::math::DCMotor m_armGearbox = wpi::math::DCMotor::Vex775Pro(2);
wpi::math::DCMotor armGearbox = wpi::math::DCMotor::Vex775Pro(2);
// Standard classes for controlling our arm
wpi::math::PIDController m_controller{m_armKp, 0, 0};
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
wpi::PWMSparkMax m_motor{kMotorPort};
wpi::math::PIDController controller{armKp, 0, 0};
wpi::Encoder encoder{kEncoderAChannel, kEncoderBChannel};
wpi::PWMSparkMax motor{kMotorPort};
// Simulation classes help us simulate what's going on, including gravity.
// This sim represents an arm with 2 775s, a 600:1 reduction, a mass of 5kg,
// 30in overall arm length, range of motion in [-75, 255] degrees, and noise
// with a standard deviation of 1 encoder tick.
wpi::sim::SingleJointedArmSim m_armSim{
m_armGearbox,
wpi::sim::SingleJointedArmSim armSim{
armGearbox,
kArmReduction,
wpi::sim::SingleJointedArmSim::EstimateMOI(kArmLength, kArmMass),
kArmLength,
@@ -54,16 +54,16 @@ class Arm {
true,
0_deg,
{kArmEncoderDistPerPulse}};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
wpi::sim::EncoderSim encoderSim{encoder};
// Create a Mechanism2d display of an Arm
wpi::Mechanism2d m_mech2d{60, 60};
wpi::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30);
wpi::MechanismLigament2d* m_armTower =
m_armBase->Append<wpi::MechanismLigament2d>(
wpi::Mechanism2d mech2d{60, 60};
wpi::MechanismRoot2d* armBase = mech2d.GetRoot("ArmBase", 30, 30);
wpi::MechanismLigament2d* armTower =
armBase->Append<wpi::MechanismLigament2d>(
"Arm Tower", 30, -90_deg, 6,
wpi::util::Color8Bit{wpi::util::Color::BLUE});
wpi::MechanismLigament2d* m_arm = m_armBase->Append<wpi::MechanismLigament2d>(
"Arm", 30, m_armSim.GetAngle(), 6,
wpi::MechanismLigament2d* arm = armBase->Append<wpi::MechanismLigament2d>(
"Arm", 30, armSim.GetAngle(), 6,
wpi::util::Color8Bit{wpi::util::Color::YELLOW});
};

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

@@ -6,24 +6,24 @@
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(), velocities.left.value());
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), velocities.right.value());
const auto leftFeedforward = feedforward.Calculate(velocities.left);
const auto rightFeedforward = feedforward.Calculate(velocities.right);
const double leftOutput = leftPIDController.Calculate(
leftEncoder.GetRate(), velocities.left.value());
const double rightOutput = rightPIDController.Calculate(
rightEncoder.GetRate(), velocities.right.value());
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot) {
SetVelocities(m_kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
SetVelocities(kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_imu.GetRotation2d(),
wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()});
odometry.Update(imu.GetRotation2d(),
wpi::units::meter_t{leftEncoder.GetDistance()},
wpi::units::meter_t{rightEncoder.GetDistance()});
}

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

@@ -11,35 +11,34 @@ class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
TeleopPeriodic();
m_drive.UpdateOdometry();
drive.UpdateOdometry();
}
void TeleopPeriodic() override {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xVelocity =
-m_velocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
const auto xVelocity = -velocityLimiter.Calculate(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()) *
const auto rot = -rotLimiter.Calculate(controller.GetRightX()) *
Drivetrain::kMaxAngularVelocity;
m_drive.Drive(xVelocity, rot);
drive.Drive(xVelocity, rot);
}
private:
wpi::Gamepad m_controller{0};
wpi::Gamepad controller{0};
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> rotLimiter{3 / 1_s};
Drivetrain m_drive;
Drivetrain drive;
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -24,25 +24,25 @@
class Drivetrain {
public:
Drivetrain() {
m_leftLeader.AddFollower(m_leftFollower);
m_rightLeader.AddFollower(m_rightFollower);
leftLeader.AddFollower(leftFollower);
rightLeader.AddFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.SetInverted(true);
rightLeader.SetInverted(true);
m_imu.ResetYaw();
imu.ResetYaw();
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
m_leftEncoder.Reset();
m_rightEncoder.Reset();
leftEncoder.Reset();
rightEncoder.Reset();
}
static constexpr wpi::units::meters_per_second_t kMaxVelocity =
@@ -61,26 +61,26 @@ class Drivetrain {
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
wpi::PWMSparkMax m_leftLeader{1};
wpi::PWMSparkMax m_leftFollower{2};
wpi::PWMSparkMax m_rightLeader{3};
wpi::PWMSparkMax m_rightFollower{4};
wpi::PWMSparkMax leftLeader{1};
wpi::PWMSparkMax leftFollower{2};
wpi::PWMSparkMax rightLeader{3};
wpi::PWMSparkMax rightFollower{4};
wpi::Encoder m_leftEncoder{0, 1};
wpi::Encoder m_rightEncoder{2, 3};
wpi::Encoder leftEncoder{0, 1};
wpi::Encoder rightEncoder{2, 3};
wpi::math::PIDController m_leftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_rightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController leftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController rightPIDController{1.0, 0.0, 0.0};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::FLAT};
wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
wpi::math::DifferentialDriveKinematics m_kinematics{kTrackwidth};
wpi::math::DifferentialDriveOdometry m_odometry{
m_imu.GetRotation2d(), wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()}};
wpi::math::DifferentialDriveKinematics kinematics{kTrackwidth};
wpi::math::DifferentialDriveOdometry odometry{
imu.GetRotation2d(), wpi::units::meter_t{leftEncoder.GetDistance()},
wpi::units::meter_t{rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward{
1_V, 3_V / 1_mps};
};

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

@@ -12,47 +12,47 @@
#include "wpi/system/RobotController.hpp"
Drivetrain::Drivetrain() {
m_leftLeader.AddFollower(m_leftFollower);
m_rightLeader.AddFollower(m_rightFollower);
leftLeader.AddFollower(leftFollower);
rightLeader.AddFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.SetInverted(true);
rightLeader.SetInverted(true);
m_imu.ResetYaw();
imu.ResetYaw();
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.SetDistancePerPulse(
leftEncoder.SetDistancePerPulse(
(2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value());
m_rightEncoder.SetDistancePerPulse(
rightEncoder.SetDistancePerPulse(
(2 * std::numbers::pi * kWheelRadius / kEncoderResolution).value());
m_leftEncoder.Reset();
m_rightEncoder.Reset();
leftEncoder.Reset();
rightEncoder.Reset();
wpi::SmartDashboard::PutData("FieldSim", &m_fieldSim);
wpi::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
wpi::SmartDashboard::PutData("FieldSim", &fieldSim);
wpi::SmartDashboard::PutData("Approximation", &fieldApproximation);
}
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(), velocities.left.value());
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), velocities.right.value());
const auto leftFeedforward = feedforward.Calculate(velocities.left);
const auto rightFeedforward = feedforward.Calculate(velocities.right);
const double leftOutput = leftPIDController.Calculate(
leftEncoder.GetRate(), velocities.left.value());
const double rightOutput = rightPIDController.Calculate(
rightEncoder.GetRate(), velocities.right.value());
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot) {
SetVelocities(m_kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
SetVelocities(kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
}
void Drivetrain::PublishCameraToObject(
@@ -93,19 +93,19 @@ wpi::math::Pose3d Drivetrain::ObjectToRobotPose(
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_imu.GetRotation2d(),
wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()});
poseEstimator.Update(imu.GetRotation2d(),
wpi::units::meter_t{leftEncoder.GetDistance()},
wpi::units::meter_t{rightEncoder.GetDistance()});
// Publish cameraToObject transformation to networktables --this would
// normally be handled by the computer vision solution.
PublishCameraToObject(m_objectInField, m_robotToCamera,
m_cameraToObjectEntryRef, m_drivetrainSimulator);
PublishCameraToObject(objectInField, robotToCamera, cameraToObjectEntryRef,
drivetrainSimulator);
// Compute the robot's field-relative position exclusively from vision
// measurements.
wpi::math::Pose3d visionMeasurement3d = ObjectToRobotPose(
m_objectInField, m_robotToCamera, m_cameraToObjectEntryRef);
wpi::math::Pose3d visionMeasurement3d =
ObjectToRobotPose(objectInField, robotToCamera, cameraToObjectEntryRef);
// Convert robot's pose from wpi::math::Pose3d to wpi::math::Pose2d needed to
// apply vision measurements.
@@ -114,32 +114,30 @@ void Drivetrain::UpdateOdometry() {
// Apply vision measurements. For simulation purposes only, we don't input a
// latency delay -- on a real robot, this must be calculated based either on
// known latency or timestamps.
m_poseEstimator.AddVisionMeasurement(visionMeasurement2d,
wpi::Timer::GetTimestamp());
poseEstimator.AddVisionMeasurement(visionMeasurement2d,
wpi::Timer::GetTimestamp());
}
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.GetThrottle()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{m_rightLeader.GetThrottle()} *
wpi::RobotController::GetInputVoltage());
m_drivetrainSimulator.Update(20_ms);
drivetrainSimulator.SetInputs(wpi::units::volt_t{leftLeader.GetThrottle()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{rightLeader.GetThrottle()} *
wpi::RobotController::GetInputVoltage());
drivetrainSimulator.Update(20_ms);
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
m_rightEncoderSim.SetDistance(
m_drivetrainSimulator.GetRightPosition().value());
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
// m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
leftEncoderSim.SetDistance(drivetrainSimulator.GetLeftPosition().value());
leftEncoderSim.SetRate(drivetrainSimulator.GetLeftVelocity().value());
rightEncoderSim.SetDistance(drivetrainSimulator.GetRightPosition().value());
rightEncoderSim.SetRate(drivetrainSimulator.GetRightVelocity().value());
// gyroSim.SetAngle(-drivetrainSimulator.GetHeading().Degrees().value());
// // TODO(Ryan): fixup when sim implemented
}
void Drivetrain::Periodic() {
UpdateOdometry();
m_fieldSim.SetRobotPose(m_drivetrainSimulator.GetPose());
m_fieldApproximation.SetRobotPose(m_poseEstimator.GetEstimatedPosition());
fieldSim.SetRobotPose(drivetrainSimulator.GetPose());
fieldApproximation.SetRobotPose(poseEstimator.GetEstimatedPosition());
}

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

@@ -11,39 +11,38 @@ class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
TeleopPeriodic();
m_drive.UpdateOdometry();
drive.UpdateOdometry();
}
void RobotPeriodic() override { m_drive.Periodic(); }
void RobotPeriodic() override { drive.Periodic(); }
void TeleopPeriodic() override {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xVelocity =
-m_velocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
const auto xVelocity = -velocityLimiter.Calculate(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()) *
const auto rot = -rotLimiter.Calculate(controller.GetRightX()) *
Drivetrain::kMaxAngularVelocity;
m_drive.Drive(xVelocity, rot);
drive.Drive(xVelocity, rot);
}
void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
void SimulationPeriodic() override { drive.SimulationPeriodic(); }
private:
wpi::Gamepad m_controller{0};
wpi::Gamepad controller{0};
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> rotLimiter{3 / 1_s};
Drivetrain m_drive;
Drivetrain drive;
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -114,64 +114,63 @@ class Drivetrain {
static constexpr std::array<double, 7> kDefaultVal{0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0};
wpi::math::Transform3d m_robotToCamera{
wpi::math::Transform3d robotToCamera{
wpi::math::Translation3d{1_m, 1_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi / 2}}};
wpi::nt::NetworkTableInstance m_inst{
wpi::nt::NetworkTableInstance inst{
wpi::nt::NetworkTableInstance::GetDefault()};
wpi::nt::DoubleArrayTopic m_cameraToObjectTopic{
m_inst.GetDoubleArrayTopic("m_cameraToObjectTopic")};
wpi::nt::DoubleArrayEntry m_cameraToObjectEntry =
m_cameraToObjectTopic.GetEntry(kDefaultVal);
wpi::nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
wpi::nt::DoubleArrayTopic cameraToObjectTopic{
inst.GetDoubleArrayTopic("cameraToObjectTopic")};
wpi::nt::DoubleArrayEntry cameraToObjectEntry =
cameraToObjectTopic.GetEntry(kDefaultVal);
wpi::nt::DoubleArrayEntry& cameraToObjectEntryRef = cameraToObjectEntry;
wpi::apriltag::AprilTagFieldLayout m_aprilTagFieldLayout{
wpi::apriltag::AprilTagFieldLayout aprilTagFieldLayout{
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo)};
wpi::math::Pose3d m_objectInField{
m_aprilTagFieldLayout.GetTagPose(0).value()};
wpi::math::Pose3d objectInField{aprilTagFieldLayout.GetTagPose(0).value()};
wpi::PWMSparkMax m_leftLeader{1};
wpi::PWMSparkMax m_leftFollower{2};
wpi::PWMSparkMax m_rightLeader{3};
wpi::PWMSparkMax m_rightFollower{4};
wpi::PWMSparkMax leftLeader{1};
wpi::PWMSparkMax leftFollower{2};
wpi::PWMSparkMax rightLeader{3};
wpi::PWMSparkMax rightFollower{4};
wpi::Encoder m_leftEncoder{0, 1};
wpi::Encoder m_rightEncoder{2, 3};
wpi::Encoder leftEncoder{0, 1};
wpi::Encoder rightEncoder{2, 3};
wpi::math::PIDController m_leftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_rightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController leftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController rightPIDController{1.0, 0.0, 0.0};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::FLAT};
wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
wpi::math::DifferentialDriveKinematics m_kinematics{kTrackwidth};
wpi::math::DifferentialDriveKinematics kinematics{kTrackwidth};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::DifferentialDrivePoseEstimator m_poseEstimator{
m_kinematics,
m_imu.GetRotation2d(),
wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()},
wpi::math::DifferentialDrivePoseEstimator poseEstimator{
kinematics,
imu.GetRotation2d(),
wpi::units::meter_t{leftEncoder.GetDistance()},
wpi::units::meter_t{rightEncoder.GetDistance()},
wpi::math::Pose2d{},
{0.01, 0.01, 0.01},
{0.1, 0.1, 0.1}};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward{
1_V, 3_V / 1_mps};
// Simulation classes
wpi::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
wpi::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
wpi::Field2d m_fieldSim;
wpi::Field2d m_fieldApproximation;
wpi::math::LinearSystem<2, 2, 2> m_drivetrainSystem =
wpi::sim::EncoderSim leftEncoderSim{leftEncoder};
wpi::sim::EncoderSim rightEncoderSim{rightEncoder};
wpi::Field2d fieldSim;
wpi::Field2d fieldApproximation;
wpi::math::LinearSystem<2, 2, 2> drivetrainSystem =
wpi::math::Models::DifferentialDriveFromSysId(
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
wpi::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
m_drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in};
wpi::sim::DifferentialDrivetrainSim drivetrainSimulator{
drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in};
};

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

@@ -35,11 +35,11 @@ void Robot::DisabledPeriodic() {}
* RobotContainer} class.
*/
void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
autonomousCommand = container.GetAutonomousCommand();
if (m_autonomousCommand) {
if (autonomousCommand) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(
m_autonomousCommand.value());
autonomousCommand.value());
}
}
@@ -50,9 +50,9 @@ void Robot::TeleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand) {
m_autonomousCommand->Cancel();
m_autonomousCommand.reset();
if (autonomousCommand) {
autonomousCommand->Cancel();
autonomousCommand.reset();
}
}

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

@@ -13,31 +13,31 @@ RobotContainer::RobotContainer() {
ConfigureButtonBindings();
// Set up default drive command
m_drive.SetDefaultCommand(wpi::cmd::Run(
drive.SetDefaultCommand(wpi::cmd::Run(
[this] {
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
-m_driverController.GetRightX());
drive.ArcadeDrive(-driverController.GetLeftY(),
-driverController.GetRightX());
},
{&m_drive}));
{&drive}));
}
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// While holding the bumper button, drive at half velocity
m_driverController.RightBumper()
.OnTrue(m_driveHalfVelocity.get())
.OnFalse(m_driveFullVelocity.get());
driverController.RightBumper()
.OnTrue(driveHalfVelocity.get())
.OnFalse(driveFullVelocity.get());
// Drive forward by 3 meters when the 'South Face' button is pressed, with a
// timeout of 10 seconds
m_driverController.SouthFace().OnTrue(
m_drive.ProfiledDriveDistance(3_m).WithTimeout(10_s));
driverController.SouthFace().OnTrue(
drive.ProfiledDriveDistance(3_m).WithTimeout(10_s));
// Do the same thing as above when the 'East Face' button is pressed, but
// without resetting the encoders
m_driverController.EastFace().OnTrue(
m_drive.DynamicProfiledDriveDistance(3_m).WithTimeout(10_s));
driverController.EastFace().OnTrue(
drive.DynamicProfiledDriveDistance(3_m).WithTimeout(10_s));
}
wpi::cmd::CommandPtr RobotContainer::GetAutonomousCommand() {

90
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -9,24 +9,24 @@
using namespace DriveConstants;
DriveSubsystem::DriveSubsystem()
: m_leftLeader{kLeftMotor1Port},
m_leftFollower{kLeftMotor2Port},
m_rightLeader{kRightMotor1Port},
m_rightFollower{kRightMotor2Port},
m_feedforward{ks, kv, ka} {
wpi::util::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
: leftLeader{kLeftMotor1Port},
leftFollower{kLeftMotor2Port},
rightLeader{kRightMotor1Port},
rightFollower{kRightMotor2Port},
feedforward{ks, kv, ka} {
wpi::util::SendableRegistry::AddChild(&drive, &leftLeader);
wpi::util::SendableRegistry::AddChild(&drive, &rightLeader);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.SetInverted(true);
rightLeader.SetInverted(true);
m_leftFollower.Follow(m_leftLeader);
m_rightFollower.Follow(m_rightLeader);
leftFollower.Follow(leftLeader);
rightFollower.Follow(rightLeader);
m_leftLeader.SetPID(kp, 0, 0);
m_rightLeader.SetPID(kp, 0, 0);
leftLeader.SetPID(kp, 0, 0);
rightLeader.SetPID(kp, 0, 0);
}
void DriveSubsystem::Periodic() {
@@ -39,37 +39,37 @@ void DriveSubsystem::SetDriveStates(
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextRight) {
// Feedforward is divided by battery voltage to normalize it to [-1, 1]
m_leftLeader.SetSetpoint(
leftLeader.SetSetpoint(
ExampleSmartMotorController::PIDMode::kPosition,
currentLeft.position.value(),
m_feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) /
feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) /
wpi::RobotController::GetBatteryVoltage());
m_rightLeader.SetSetpoint(
rightLeader.SetSetpoint(
ExampleSmartMotorController::PIDMode::kPosition,
currentRight.position.value(),
m_feedforward.Calculate(currentRight.velocity, nextRight.velocity) /
feedforward.Calculate(currentRight.velocity, nextRight.velocity) /
wpi::RobotController::GetBatteryVoltage());
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
drive.ArcadeDrive(fwd, rot);
}
void DriveSubsystem::ResetEncoders() {
m_leftLeader.ResetEncoder();
m_rightLeader.ResetEncoder();
leftLeader.ResetEncoder();
rightLeader.ResetEncoder();
}
wpi::units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
return wpi::units::meter_t{m_leftLeader.GetEncoderDistance()};
return wpi::units::meter_t{leftLeader.GetEncoderDistance()};
}
wpi::units::meter_t DriveSubsystem::GetRightEncoderDistance() {
return wpi::units::meter_t{m_rightLeader.GetEncoderDistance()};
return wpi::units::meter_t{rightLeader.GetEncoderDistance()};
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
drive.SetMaxOutput(maxOutput);
}
wpi::cmd::CommandPtr DriveSubsystem::ProfiledDriveDistance(
@@ -77,21 +77,21 @@ wpi::cmd::CommandPtr DriveSubsystem::ProfiledDriveDistance(
return StartRun(
[&] {
// Restart timer so profile setpoints start at the beginning
m_timer.Restart();
timer.Restart();
ResetEncoders();
},
[&] {
// Current state never changes, so we need to use a timer to get
// the setpoints we need to be at
auto currentTime = m_timer.Get();
auto currentTime = timer.Get();
auto currentSetpoint =
m_profile.Calculate(currentTime, {}, {distance, 0_mps});
auto nextSetpoint = m_profile.Calculate(currentTime + kDt, {},
{distance, 0_mps});
profile.Calculate(currentTime, {}, {distance, 0_mps});
auto nextSetpoint =
profile.Calculate(currentTime + kDt, {}, {distance, 0_mps});
SetDriveStates(currentSetpoint, currentSetpoint, nextSetpoint,
nextSetpoint);
})
.Until([&] { return m_profile.IsFinished(0_s); });
.Until([&] { return profile.IsFinished(0_s); });
}
wpi::cmd::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
@@ -99,32 +99,32 @@ wpi::cmd::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
return StartRun(
[&] {
// Restart timer so profile setpoints start at the beginning
m_timer.Restart();
timer.Restart();
// Store distance so we know the target distance for each encoder
m_initialLeftDistance = GetLeftEncoderDistance();
m_initialRightDistance = GetRightEncoderDistance();
initialLeftDistance = GetLeftEncoderDistance();
initialRightDistance = GetRightEncoderDistance();
},
[&] {
// Current state never changes for the duration of the command,
// so we need to use a timer to get the setpoints we need to be
// at
auto currentTime = m_timer.Get();
auto currentTime = timer.Get();
auto currentLeftSetpoint = m_profile.Calculate(
currentTime, {m_initialLeftDistance, 0_mps},
{m_initialLeftDistance + distance, 0_mps});
auto currentRightSetpoint = m_profile.Calculate(
currentTime, {m_initialRightDistance, 0_mps},
{m_initialRightDistance + distance, 0_mps});
auto currentLeftSetpoint =
profile.Calculate(currentTime, {initialLeftDistance, 0_mps},
{initialLeftDistance + distance, 0_mps});
auto currentRightSetpoint =
profile.Calculate(currentTime, {initialRightDistance, 0_mps},
{initialRightDistance + distance, 0_mps});
auto nextLeftSetpoint = m_profile.Calculate(
currentTime + kDt, {m_initialLeftDistance, 0_mps},
{m_initialLeftDistance + distance, 0_mps});
auto nextRightSetpoint = m_profile.Calculate(
currentTime + kDt, {m_initialRightDistance, 0_mps},
{m_initialRightDistance + distance, 0_mps});
auto nextLeftSetpoint = profile.Calculate(
currentTime + kDt, {initialLeftDistance, 0_mps},
{initialLeftDistance + distance, 0_mps});
auto nextRightSetpoint = profile.Calculate(
currentTime + kDt, {initialRightDistance, 0_mps},
{initialRightDistance + distance, 0_mps});
SetDriveStates(currentLeftSetpoint, currentRightSetpoint,
nextLeftSetpoint, nextRightSetpoint);
})
.Until([&] { return m_profile.IsFinished(0_s); });
.Until([&] { return profile.IsFinished(0_s); });
}

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

@@ -66,9 +66,9 @@ class ExampleSmartMotorController {
*/
void ResetEncoder() {}
void Set(double velocity) { m_value = velocity; }
void Set(double velocity) { value = velocity; }
double Get() const { return m_value; }
double Get() const { return value; }
void SetInverted(bool isInverted) {}
@@ -79,5 +79,5 @@ class ExampleSmartMotorController {
void StopMotor() {}
private:
double m_value = 0.0;
double value = 0.0;
};

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

@@ -25,7 +25,7 @@ class Robot : public wpi::TimedRobot {
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
std::optional<wpi::cmd::CommandPtr> m_autonomousCommand;
std::optional<wpi::cmd::CommandPtr> autonomousCommand;
RobotContainer m_container;
RobotContainer container;
};

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

@@ -26,19 +26,18 @@ class RobotContainer {
private:
// The driver's controller
wpi::cmd::CommandGamepad m_driverController{
OIConstants::kDriverControllerPort};
wpi::cmd::CommandGamepad driverController{OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
// The robot's subsystems
DriveSubsystem m_drive;
DriveSubsystem drive;
// RobotContainer-owned commands
wpi::cmd::CommandPtr m_driveHalfVelocity =
wpi::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {});
wpi::cmd::CommandPtr m_driveFullVelocity =
wpi::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {});
wpi::cmd::CommandPtr driveHalfVelocity =
wpi::cmd::RunOnce([this] { drive.SetMaxOutput(0.5); }, {});
wpi::cmd::CommandPtr driveFullVelocity =
wpi::cmd::RunOnce([this] { drive.SetMaxOutput(1); }, {});
void ConfigureButtonBindings();
};

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

@@ -95,25 +95,24 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
wpi::units::meter_t distance);
private:
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{
wpi::math::TrapezoidProfile<wpi::units::meters> profile{
{DriveConstants::kMaxVelocity, DriveConstants::kMaxAcceleration}};
wpi::Timer m_timer;
wpi::units::meter_t m_initialLeftDistance;
wpi::units::meter_t m_initialRightDistance;
wpi::Timer timer;
wpi::units::meter_t initialLeftDistance;
wpi::units::meter_t initialRightDistance;
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
// The motor controllers
ExampleSmartMotorController m_leftLeader;
ExampleSmartMotorController m_leftFollower;
ExampleSmartMotorController m_rightLeader;
ExampleSmartMotorController m_rightFollower;
ExampleSmartMotorController leftLeader;
ExampleSmartMotorController leftFollower;
ExampleSmartMotorController rightLeader;
ExampleSmartMotorController rightFollower;
// A feedforward component for the drive
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward;
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward;
// The robot's drive
wpi::DifferentialDrive m_drive{
[&](double output) { m_leftLeader.Set(output); },
[&](double output) { m_rightLeader.Set(output); }};
wpi::DifferentialDrive drive{[&](double output) { leftLeader.Set(output); },
[&](double output) { rightLeader.Set(output); }};
};

10
wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
View File

@@ -17,7 +17,7 @@ class Robot : public wpi::TimedRobot {
// to measure this is fairly easy. Set the value to 0, place the mechanism
// where you want "0" to be, and observe the value on the dashboard, That
// is the value to enter for the 3rd parameter.
wpi::DutyCycleEncoder m_dutyCycleEncoder{0, fullRange, expectedZero};
wpi::DutyCycleEncoder dutyCycleEncoder{0, fullRange, expectedZero};
public:
Robot() {
@@ -32,18 +32,18 @@ class Robot : public wpi::TimedRobot {
// those values. This number doesn't have to be perfect,
// just having a fairly close value will make the output readings
// much more stable.
m_dutyCycleEncoder.SetAssumedFrequency(967.8_Hz);
dutyCycleEncoder.SetAssumedFrequency(967.8_Hz);
}
void RobotPeriodic() override {
// Connected can be checked, and uses the frequency of the encoder
auto connected = m_dutyCycleEncoder.IsConnected();
auto connected = dutyCycleEncoder.IsConnected();
// Duty Cycle Frequency in Hz
auto frequency = m_dutyCycleEncoder.GetFrequency();
auto frequency = dutyCycleEncoder.GetFrequency();
// Output of encoder
auto output = m_dutyCycleEncoder.Get();
auto output = dutyCycleEncoder.Get();
// By default, the output will wrap around to the full range value
// when the sensor goes below 0. However, for moving mechanisms this

34
wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp
View File

@@ -19,44 +19,44 @@ class Robot : public wpi::TimedRobot {
Robot() {
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.SetPID(1.3, 0.0, 0.7);
motor.SetPID(1.3, 0.0, 0.7);
}
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
m_goal = {5_m, 0_mps};
} else if (m_joystick.GetRawButtonPressed(3)) {
m_goal = {0_m, 0_mps};
if (joystick.GetRawButtonPressed(2)) {
goal = {5_m, 0_mps};
} else if (joystick.GetRawButtonPressed(3)) {
goal = {0_m, 0_mps};
}
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
auto next = m_profile.Calculate(kDt, m_goal, m_setpoint);
auto next = profile.Calculate(kDt, goal, setpoint);
// Send setpoint to offboard controller PID
m_motor.SetSetpoint(
motor.SetSetpoint(
ExampleSmartMotorController::PIDMode::kPosition,
m_setpoint.position.value(),
m_feedforward.Calculate(m_setpoint.velocity, next.velocity) / 12_V);
setpoint.position.value(),
feedforward.Calculate(setpoint.velocity, next.velocity) / 12_V);
m_setpoint = next;
setpoint = next;
}
private:
wpi::Joystick m_joystick{1};
ExampleSmartMotorController m_motor{1};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
wpi::Joystick joystick{1};
ExampleSmartMotorController motor{1};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward{
// Note: These gains are fake, and will have to be tuned for your robot.
1_V, 1_V / 1_mps, 1_V / 1_mps_sq};
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint.
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>
m_profile{{10_V, 1_V / 1_mps, 1_V / 1_mps_sq}};
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts> profile{
{10_V, 1_V / 1_mps, 1_V / 1_mps_sq}};
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State
m_goal;
goal;
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State
m_setpoint;
setpoint;
};
#ifndef RUNNING_WPILIB_TESTS

14
wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/Robot.cpp
View File

@@ -9,32 +9,32 @@
void Robot::RobotPeriodic() {
// Update the telemetry, including mechanism visualization, regardless of
// mode.
m_elevator.UpdateTelemetry();
elevator.UpdateTelemetry();
}
void Robot::SimulationPeriodic() {
// Update the simulation model.
m_elevator.SimulationPeriodic();
elevator.SimulationPeriodic();
}
void Robot::TeleopInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_elevator.Reset();
elevator.Reset();
}
void Robot::TeleopPeriodic() {
if (m_joystick.GetTrigger()) {
if (joystick.GetTrigger()) {
// Here, we set the constant setpoint of 0.75 meters.
m_elevator.ReachGoal(Constants::kSetpoint);
elevator.ReachGoal(Constants::kSetpoint);
} else {
// Otherwise, we update the setpoint to 0.
m_elevator.ReachGoal(Constants::kLowerSetpoint);
elevator.ReachGoal(Constants::kLowerSetpoint);
}
}
void Robot::DisabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_elevator.Stop();
elevator.Stop();
}
#ifndef RUNNING_WPILIB_TESTS

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

@@ -8,56 +8,56 @@
#include "wpi/system/RobotController.hpp"
Elevator::Elevator() {
m_encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse);
encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse);
// Put Mechanism 2d to SmartDashboard
// To view the Elevator visualization, select Network Tables -> SmartDashboard
// -> Elevator Sim
wpi::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
wpi::SmartDashboard::PutData("Elevator Sim", &mech2d);
}
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.GetThrottle() * wpi::RobotController::GetInputVoltage()});
elevatorSim.SetInput(wpi::math::Vectord<1>{
motorSim.GetThrottle() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.Update(20_ms);
elevatorSim.Update(20_ms);
// Finally, we set our simulated encoder's readings and simulated battery
// voltage
m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
encoderSim.SetDistance(elevatorSim.GetPosition().value());
// SimBattery estimates loaded battery voltages
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
wpi::sim::BatterySim::Calculate({elevatorSim.GetCurrentDraw()}));
}
void Elevator::UpdateTelemetry() {
// Update the Elevator length based on the simulated elevator height
m_elevatorMech2d->SetLength(m_encoder.GetDistance());
elevatorMech2d->SetLength(encoder.GetDistance());
}
void Elevator::ReachGoal(wpi::units::meter_t goal) {
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State
goalState{goal, 0_mps};
auto next = m_profile.Calculate(20_ms, m_setpoint, goalState);
auto next = profile.Calculate(20_ms, setpoint, goalState);
auto pidOutput = m_controller.Calculate(m_encoder.GetDistance(),
m_setpoint.position / 1_m);
auto pidOutput =
controller.Calculate(encoder.GetDistance(), setpoint.position / 1_m);
auto feedforwardOutput =
m_feedforward.Calculate(m_setpoint.velocity, next.velocity);
feedforward.Calculate(setpoint.velocity, next.velocity);
m_motor.SetVoltage(wpi::units::volt_t{pidOutput} + feedforwardOutput);
motor.SetVoltage(wpi::units::volt_t{pidOutput} + feedforwardOutput);
m_setpoint = next;
setpoint = next;
}
void Elevator::Reset() {
m_setpoint = {m_encoder.GetDistance() * 1_m, 0_mps};
setpoint = {encoder.GetDistance() * 1_m, 0_mps};
}
void Elevator::Stop() {
m_motor.SetThrottle(0.0);
motor.SetThrottle(0.0);
}

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

@@ -21,6 +21,6 @@ class Robot : public wpi::TimedRobot {
void DisabledInit() override;
private:
wpi::Joystick m_joystick{Constants::kJoystickPort};
Elevator m_elevator;
wpi::Joystick joystick{Constants::kJoystickPort};
Elevator elevator;
};

54
wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/subsystems/Elevator.hpp
View File

@@ -31,45 +31,45 @@ class Elevator {
private:
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
wpi::math::DCMotor m_elevatorGearbox = wpi::math::DCMotor::NEO(2);
wpi::math::DCMotor elevatorGearbox = wpi::math::DCMotor::NEO(2);
// Standard classes for controlling our elevator
wpi::math::ExponentialProfile<wpi::units::meters,
wpi::units::volts>::Constraints m_constraints{
wpi::units::volts>::Constraints constraints{
Constants::kElevatorMaxV, Constants::kElevatorkV, Constants::kElevatorkA};
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>
m_profile{m_constraints};
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts> profile{
constraints};
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State
m_setpoint;
setpoint;
wpi::math::PIDController m_controller{
wpi::math::PIDController controller{
Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd};
wpi::math::ElevatorFeedforward m_feedforward{
wpi::math::ElevatorFeedforward feedforward{
Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV,
Constants::kElevatorkA};
wpi::Encoder m_encoder{Constants::kEncoderAChannel,
Constants::kEncoderBChannel};
wpi::PWMSparkMax m_motor{Constants::kMotorPort};
wpi::sim::PWMMotorControllerSim m_motorSim{m_motor};
wpi::Encoder encoder{Constants::kEncoderAChannel,
Constants::kEncoderBChannel};
wpi::PWMSparkMax motor{Constants::kMotorPort};
wpi::sim::PWMMotorControllerSim motorSim{motor};
// Simulation classes help us simulate what's going on, including gravity.
wpi::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
Constants::kElevatorGearing,
Constants::kCarriageMass,
Constants::kElevatorDrumRadius,
Constants::kMinElevatorHeight,
Constants::kMaxElevatorHeight,
true,
0_m,
{0.005}};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
wpi::sim::ElevatorSim elevatorSim{elevatorGearbox,
Constants::kElevatorGearing,
Constants::kCarriageMass,
Constants::kElevatorDrumRadius,
Constants::kMinElevatorHeight,
Constants::kMaxElevatorHeight,
true,
0_m,
{0.005}};
wpi::sim::EncoderSim encoderSim{encoder};
// Create a Mechanism2d display of an elevator
wpi::Mechanism2d m_mech2d{10_in / 1_m, 51_in / 1_m};
wpi::MechanismRoot2d* m_elevatorRoot =
m_mech2d.GetRoot("Elevator Root", 5_in / 1_m, 0.5_in / 1_m);
wpi::MechanismLigament2d* m_elevatorMech2d =
m_elevatorRoot->Append<wpi::MechanismLigament2d>(
"Elevator", m_elevatorSim.GetPosition().value(), 90_deg);
wpi::Mechanism2d mech2d{10_in / 1_m, 51_in / 1_m};
wpi::MechanismRoot2d* elevatorRoot =
mech2d.GetRoot("Elevator Root", 5_in / 1_m, 0.5_in / 1_m);
wpi::MechanismLigament2d* elevatorMech2d =
elevatorRoot->Append<wpi::MechanismLigament2d>(
"Elevator", elevatorSim.GetPosition().value(), 90_deg);
};

31
wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
View File

@@ -22,21 +22,20 @@ class Robot : public wpi::TimedRobot {
static constexpr wpi::units::second_t kDt = 20_ms;
Robot() {
m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
}
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
m_controller.SetGoal(5_m);
} else if (m_joystick.GetRawButtonPressed(3)) {
m_controller.SetGoal(0_m);
if (joystick.GetRawButtonPressed(2)) {
controller.SetGoal(5_m);
} else if (joystick.GetRawButtonPressed(3)) {
controller.SetGoal(0_m);
}
// Run controller and update motor output
m_motor.SetVoltage(
wpi::units::volt_t{m_controller.Calculate(
wpi::units::meter_t{m_encoder.GetDistance()})} +
m_feedforward.Calculate(m_controller.GetSetpoint().velocity));
motor.SetVoltage(wpi::units::volt_t{controller.Calculate(
wpi::units::meter_t{encoder.GetDistance()})} +
feedforward.Calculate(controller.GetSetpoint().velocity));
}
private:
@@ -50,17 +49,17 @@ class Robot : public wpi::TimedRobot {
static constexpr wpi::units::volt_t kG = 1.2_V;
static constexpr auto kV = 1.3_V / 1_mps;
wpi::Joystick m_joystick{1};
wpi::Encoder m_encoder{1, 2};
wpi::PWMSparkMax m_motor{1};
wpi::Joystick joystick{1};
wpi::Encoder encoder{1, 2};
wpi::PWMSparkMax motor{1};
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints m_constraints{
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints constraints{
kMaxVelocity, kMaxAcceleration};
wpi::math::ProfiledPIDController<wpi::units::meters> m_controller{
kP, kI, kD, m_constraints, kDt};
wpi::math::ElevatorFeedforward m_feedforward{kS, kG, kV};
wpi::math::ProfiledPIDController<wpi::units::meters> controller{
kP, kI, kD, constraints, kDt};
wpi::math::ElevatorFeedforward feedforward{kS, kG, kV};
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -9,27 +9,27 @@
void Robot::RobotPeriodic() {
// Update the telemetry, including mechanism visualization, regardless of
// mode.
m_elevator.UpdateTelemetry();
elevator.UpdateTelemetry();
}
void Robot::SimulationPeriodic() {
// Update the simulation model.
m_elevator.SimulationPeriodic();
elevator.SimulationPeriodic();
}
void Robot::TeleopPeriodic() {
if (m_joystick.GetTrigger()) {
if (joystick.GetTrigger()) {
// Here, we set the constant setpoint of 0.75 meters.
m_elevator.ReachGoal(Constants::kSetpoint);
elevator.ReachGoal(Constants::kSetpoint);
} else {
// Otherwise, we update the setpoint to 0.
m_elevator.ReachGoal(0.0_m);
elevator.ReachGoal(0.0_m);
}
}
void Robot::DisabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_elevator.Stop();
elevator.Stop();
}
#ifndef RUNNING_WPILIB_TESTS

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

@@ -8,47 +8,47 @@
#include "wpi/system/RobotController.hpp"
Elevator::Elevator() {
m_encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse);
encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse);
// Put Mechanism 2d to SmartDashboard
// To view the Elevator visualization, select Network Tables -> SmartDashboard
// -> Elevator Sim
wpi::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
wpi::SmartDashboard::PutData("Elevator Sim", &mech2d);
}
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.GetThrottle() * wpi::RobotController::GetInputVoltage()});
elevatorSim.SetInput(wpi::math::Vectord<1>{
motorSim.GetThrottle() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.Update(20_ms);
elevatorSim.Update(20_ms);
// Finally, we set our simulated encoder's readings and simulated battery
// voltage
m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
encoderSim.SetDistance(elevatorSim.GetPosition().value());
// SimBattery estimates loaded battery voltages
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
wpi::sim::BatterySim::Calculate({elevatorSim.GetCurrentDraw()}));
}
void Elevator::UpdateTelemetry() {
// Update the Elevator length based on the simulated elevator height
m_elevatorMech2d->SetLength(m_encoder.GetDistance());
elevatorMech2d->SetLength(encoder.GetDistance());
}
void Elevator::ReachGoal(wpi::units::meter_t goal) {
m_controller.SetGoal(goal);
controller.SetGoal(goal);
// With the setpoint value we run PID control like normal
double pidOutput =
m_controller.Calculate(wpi::units::meter_t{m_encoder.GetDistance()});
controller.Calculate(wpi::units::meter_t{encoder.GetDistance()});
wpi::units::volt_t feedforwardOutput =
m_feedforward.Calculate(m_controller.GetSetpoint().velocity);
m_motor.SetVoltage(wpi::units::volt_t{pidOutput} + feedforwardOutput);
feedforward.Calculate(controller.GetSetpoint().velocity);
motor.SetVoltage(wpi::units::volt_t{pidOutput} + feedforwardOutput);
}
void Elevator::Stop() {
m_controller.SetGoal(0.0_m);
m_motor.SetThrottle(0.0);
controller.SetGoal(0.0_m);
motor.SetThrottle(0.0);
}

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

@@ -20,6 +20,6 @@ class Robot : public wpi::TimedRobot {
void DisabledInit() override;
private:
wpi::Joystick m_joystick{Constants::kJoystickPort};
Elevator m_elevator;
wpi::Joystick joystick{Constants::kJoystickPort};
Elevator elevator;
};

49
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.hpp
View File

@@ -30,40 +30,39 @@ class Elevator {
private:
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
wpi::math::DCMotor m_elevatorGearbox = wpi::math::DCMotor::Vex775Pro(4);
wpi::math::DCMotor elevatorGearbox = wpi::math::DCMotor::Vex775Pro(4);
// Standard classes for controlling our elevator
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints m_constraints{
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints constraints{
2.45_mps, 2.45_mps_sq};
wpi::math::ProfiledPIDController<wpi::units::meters> m_controller{
wpi::math::ProfiledPIDController<wpi::units::meters> controller{
Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd,
m_constraints};
constraints};
wpi::math::ElevatorFeedforward m_feedforward{
wpi::math::ElevatorFeedforward feedforward{
Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV,
Constants::kElevatorkA};
wpi::Encoder m_encoder{Constants::kEncoderAChannel,
Constants::kEncoderBChannel};
wpi::PWMSparkMax m_motor{Constants::kMotorPort};
wpi::sim::PWMMotorControllerSim m_motorSim{m_motor};
wpi::Encoder encoder{Constants::kEncoderAChannel,
Constants::kEncoderBChannel};
wpi::PWMSparkMax motor{Constants::kMotorPort};
wpi::sim::PWMMotorControllerSim motorSim{motor};
// Simulation classes help us simulate what's going on, including gravity.
wpi::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
Constants::kElevatorGearing,
Constants::kCarriageMass,
Constants::kElevatorDrumRadius,
Constants::kMinElevatorHeight,
Constants::kMaxElevatorHeight,
true,
0_m,
{0.01}};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
wpi::sim::ElevatorSim elevatorSim{elevatorGearbox,
Constants::kElevatorGearing,
Constants::kCarriageMass,
Constants::kElevatorDrumRadius,
Constants::kMinElevatorHeight,
Constants::kMaxElevatorHeight,
true,
0_m,
{0.01}};
wpi::sim::EncoderSim encoderSim{encoder};
// Create a Mechanism2d display of an elevator
wpi::Mechanism2d m_mech2d{20, 50};
wpi::MechanismRoot2d* m_elevatorRoot =
m_mech2d.GetRoot("Elevator Root", 10, 0);
wpi::MechanismLigament2d* m_elevatorMech2d =
m_elevatorRoot->Append<wpi::MechanismLigament2d>(
"Elevator", m_elevatorSim.GetPosition().value(), 90_deg);
wpi::Mechanism2d mech2d{20, 50};
wpi::MechanismRoot2d* elevatorRoot = mech2d.GetRoot("Elevator Root", 10, 0);
wpi::MechanismLigament2d* elevatorMech2d =
elevatorRoot->Append<wpi::MechanismLigament2d>(
"Elevator", elevatorSim.GetPosition().value(), 90_deg);
};

30
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
View File

@@ -19,39 +19,39 @@ class Robot : public wpi::TimedRobot {
Robot() {
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.SetPID(1.3, 0.0, 0.7);
motor.SetPID(1.3, 0.0, 0.7);
}
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
m_goal = {5_m, 0_mps};
} else if (m_joystick.GetRawButtonPressed(3)) {
m_goal = {0_m, 0_mps};
if (joystick.GetRawButtonPressed(2)) {
goal = {5_m, 0_mps};
} else if (joystick.GetRawButtonPressed(3)) {
goal = {0_m, 0_mps};
}
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = m_profile.Calculate(kDt, m_setpoint, m_goal);
setpoint = profile.Calculate(kDt, setpoint, goal);
// Send setpoint to offboard controller PID
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
m_setpoint.position.value(),
m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
setpoint.position.value(),
feedforward.Calculate(setpoint.velocity) / 12_V);
}
private:
wpi::Joystick m_joystick{1};
ExampleSmartMotorController m_motor{1};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
wpi::Joystick joystick{1};
ExampleSmartMotorController motor{1};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward{
// Note: These gains are fake, and will have to be tuned for your robot.
1_V, 1.5_V * 1_s / 1_m};
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint.
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{
wpi::math::TrapezoidProfile<wpi::units::meters> profile{
{1.75_mps, 0.75_mps_sq}};
wpi::math::TrapezoidProfile<wpi::units::meters>::State m_goal;
wpi::math::TrapezoidProfile<wpi::units::meters>::State m_setpoint;
wpi::math::TrapezoidProfile<wpi::units::meters>::State goal;
wpi::math::TrapezoidProfile<wpi::units::meters>::State setpoint;
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -34,29 +34,29 @@ class Robot : public wpi::TimedRobot {
* On a quadrature encoder, values range from 1-255; larger values result in
* smoother but potentially less accurate rates than lower values.
*/
m_encoder.SetSamplesToAverage(5);
encoder.SetSamplesToAverage(5);
/* Defines how far the mechanism attached to the encoder moves per pulse. In
* this case, we assume that a 360 count encoder is directly attached to a 3
* inch diameter (1.5inch radius) wheel, and that we want to measure
* distance in inches.
*/
m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
/* Defines the lowest rate at which the encoder will not be considered
* stopped, for the purposes of the GetStopped() method. Units are in
* distance / second, where distance refers to the units of distance that
* you are using, in this case inches.
*/
m_encoder.SetMinRate(1.0);
encoder.SetMinRate(1.0);
}
void TeleopPeriodic() override {
// Retrieve the net displacement of the Encoder since the last Reset.
wpi::SmartDashboard::PutNumber("Encoder Distance", m_encoder.GetDistance());
wpi::SmartDashboard::PutNumber("Encoder Distance", encoder.GetDistance());
// Retrieve the current rate of the encoder.
wpi::SmartDashboard::PutNumber("Encoder Rate", m_encoder.GetRate());
wpi::SmartDashboard::PutNumber("Encoder Rate", encoder.GetRate());
}
private:
@@ -76,7 +76,7 @@ class Robot : public wpi::TimedRobot {
* and defaults to X4. Faster (X4) encoding gives greater positional
* precision but more noise in the rate.
*/
wpi::Encoder m_encoder{1, 2, false, wpi::Encoder::EncodingType::X4};
wpi::Encoder encoder{1, 2, false, wpi::Encoder::EncodingType::X4};
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -11,27 +11,27 @@
class Robot : public wpi::TimedRobot {
public:
Robot() {
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_left);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_right);
wpi::util::SendableRegistry::AddChild(&robotDrive, &left);
wpi::util::SendableRegistry::AddChild(&robotDrive, &right);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_right.SetInverted(true);
m_robotDrive.SetExpiration(100_ms);
m_timer.Start();
right.SetInverted(true);
robotDrive.SetExpiration(100_ms);
timer.Start();
}
void AutonomousInit() override { m_timer.Restart(); }
void AutonomousInit() override { timer.Restart(); }
void AutonomousPeriodic() override {
// Drive for 2 seconds
if (m_timer.Get() < 2_s) {
if (timer.Get() < 2_s) {
// Drive forwards half velocity, make sure to turn input squaring off
m_robotDrive.ArcadeDrive(0.5, 0.0, false);
robotDrive.ArcadeDrive(0.5, 0.0, false);
} else {
// Stop robot
m_robotDrive.ArcadeDrive(0.0, 0.0, false);
robotDrive.ArcadeDrive(0.0, 0.0, false);
}
}
@@ -39,8 +39,7 @@ class Robot : public wpi::TimedRobot {
void TeleopPeriodic() override {
// Drive with arcade style (use right stick to steer)
m_robotDrive.ArcadeDrive(-m_controller.GetLeftY(),
m_controller.GetRightX());
robotDrive.ArcadeDrive(-controller.GetLeftY(), controller.GetRightX());
}
void UtilityInit() override {}
@@ -49,14 +48,14 @@ class Robot : public wpi::TimedRobot {
private:
// Robot drive system
wpi::PWMSparkMax m_left{0};
wpi::PWMSparkMax m_right{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_left.SetThrottle(output); },
[&](double output) { m_right.SetThrottle(output); }};
wpi::PWMSparkMax left{0};
wpi::PWMSparkMax right{1};
wpi::DifferentialDrive robotDrive{
[&](double output) { left.SetThrottle(output); },
[&](double output) { right.SetThrottle(output); }};
wpi::Gamepad m_controller{0};
wpi::Timer m_timer;
wpi::Gamepad controller{0};
wpi::Timer timer;
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -21,10 +21,10 @@ class Robot : public wpi::TimedRobot {
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_right.SetInverted(true);
right.SetInverted(true);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_left);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_right);
wpi::util::SendableRegistry::AddChild(&drive, &left);
wpi::util::SendableRegistry::AddChild(&drive, &right);
}
/**
@@ -34,8 +34,8 @@ class Robot : public wpi::TimedRobot {
*/
void TeleopPeriodic() override {
double turningValue =
(kAngleSetpoint - m_imu.GetRotation2d().Degrees().value()) * kP;
m_drive.ArcadeDrive(-m_joystick.GetY(), -turningValue);
(kAngleSetpoint - imu.GetRotation2d().Degrees().value()) * kP;
drive.ArcadeDrive(-joystick.GetY(), -turningValue);
}
private:
@@ -48,14 +48,14 @@ class Robot : public wpi::TimedRobot {
wpi::OnboardIMU::FLAT;
static constexpr int kJoystickPort = 0;
wpi::PWMSparkMax m_left{kLeftMotorPort};
wpi::PWMSparkMax m_right{kRightMotorPort};
wpi::DifferentialDrive m_drive{
[&](double output) { m_left.SetThrottle(output); },
[&](double output) { m_right.SetThrottle(output); }};
wpi::PWMSparkMax left{kLeftMotorPort};
wpi::PWMSparkMax right{kRightMotorPort};
wpi::DifferentialDrive drive{
[&](double output) { left.SetThrottle(output); },
[&](double output) { right.SetThrottle(output); }};
wpi::OnboardIMU m_imu{kIMUMountOrientation};
wpi::Joystick m_joystick{kJoystickPort};
wpi::OnboardIMU imu{kIMUMountOrientation};
wpi::Joystick joystick{kJoystickPort};
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -44,10 +44,10 @@ void Robot::DisabledPeriodic() {}
* RobotContainer} class.
*/
void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
autonomousCommand = container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
if (autonomousCommand != nullptr) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(autonomousCommand);
}
}
@@ -58,9 +58,9 @@ void Robot::TeleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != nullptr) {
m_autonomousCommand->Cancel();
m_autonomousCommand = nullptr;
if (autonomousCommand != nullptr) {
autonomousCommand->Cancel();
autonomousCommand = nullptr;
}
}

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

@@ -11,41 +11,41 @@ RobotContainer::RobotContainer() {
// Add commands to the autonomous command chooser
// Note that we do *not* move ownership into the chooser
m_chooser.SetDefaultOption("Simple Auto", m_simpleAuto.get());
m_chooser.AddOption("Complex Auto", m_complexAuto.get());
chooser.SetDefaultOption("Simple Auto", simpleAuto.get());
chooser.AddOption("Complex Auto", complexAuto.get());
// Put the chooser on the dashboard
wpi::SmartDashboard::PutData("Autonomous", &m_chooser);
wpi::SmartDashboard::PutData("Autonomous", &chooser);
// Put subsystems to dashboard.
wpi::SmartDashboard::PutData("Drivetrain", &m_drive);
wpi::SmartDashboard::PutData("HatchSubsystem", &m_hatch);
wpi::SmartDashboard::PutData("Drivetrain", &drive);
wpi::SmartDashboard::PutData("HatchSubsystem", &hatch);
// Configure the button bindings
ConfigureButtonBindings();
// Set up default drive command
m_drive.SetDefaultCommand(wpi::cmd::Run(
drive.SetDefaultCommand(wpi::cmd::Run(
[this] {
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
-m_driverController.GetRightX());
drive.ArcadeDrive(-driverController.GetLeftY(),
-driverController.GetRightX());
},
{&m_drive}));
{&drive}));
}
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
// Grab the hatch when the 'East Face' button is pressed.
m_driverController.EastFace().OnTrue(m_hatch.GrabHatchCommand());
driverController.EastFace().OnTrue(hatch.GrabHatchCommand());
// Release the hatch when the 'West Face' button is pressed.
m_driverController.WestFace().OnTrue(m_hatch.ReleaseHatchCommand());
driverController.WestFace().OnTrue(hatch.ReleaseHatchCommand());
// While holding Right Bumper, drive at half velocity
m_driverController.RightBumper()
.OnTrue(wpi::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}))
.OnFalse(wpi::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); }, {}));
driverController.RightBumper()
.OnTrue(wpi::cmd::RunOnce([this] { drive.SetMaxOutput(0.5); }, {}))
.OnFalse(wpi::cmd::RunOnce([this] { drive.SetMaxOutput(1.0); }, {}));
}
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {
// Runs the chosen command in autonomous
return m_chooser.GetSelected();
return chooser.GetSelected();
}

41
wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -9,26 +9,26 @@
using namespace DriveConstants;
DriveSubsystem::DriveSubsystem()
: m_left1{kLeftMotor1Port},
m_left2{kLeftMotor2Port},
m_right1{kRightMotor1Port},
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
wpi::util::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_right1);
: left1{kLeftMotor1Port},
left2{kLeftMotor2Port},
right1{kRightMotor1Port},
right2{kRightMotor2Port},
leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
wpi::util::SendableRegistry::AddChild(&drive, &left1);
wpi::util::SendableRegistry::AddChild(&drive, &right1);
m_left1.AddFollower(m_left2);
m_right1.AddFollower(m_right2);
left1.AddFollower(left2);
right1.AddFollower(right2);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_right1.SetInverted(true);
right1.SetInverted(true);
// Set the distance per pulse for the encoders
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
}
void DriveSubsystem::Periodic() {
@@ -36,20 +36,20 @@ void DriveSubsystem::Periodic() {
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
drive.ArcadeDrive(fwd, rot);
}
void DriveSubsystem::ResetEncoders() {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
leftEncoder.Reset();
rightEncoder.Reset();
}
double DriveSubsystem::GetAverageEncoderDistance() {
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
return (leftEncoder.GetDistance() + rightEncoder.GetDistance()) / 2.0;
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
drive.SetMaxOutput(maxOutput);
}
void DriveSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
@@ -57,8 +57,7 @@ void DriveSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
// Publish encoder distances to telemetry.
builder.AddDoubleProperty(
"leftDistance", [this] { return m_leftEncoder.GetDistance(); }, nullptr);
"leftDistance", [this] { return leftEncoder.GetDistance(); }, nullptr);
builder.AddDoubleProperty(
"rightDistance", [this] { return m_rightEncoder.GetDistance(); },
nullptr);
"rightDistance", [this] { return rightEncoder.GetDistance(); }, nullptr);
}

10
wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp
View File

@@ -9,19 +9,19 @@
using namespace HatchConstants;
HatchSubsystem::HatchSubsystem()
: m_hatchSolenoid{0, wpi::PneumaticsModuleType::CTRE_PCM,
kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
: hatchSolenoid{0, wpi::PneumaticsModuleType::CTRE_PCM,
kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
wpi::cmd::CommandPtr HatchSubsystem::GrabHatchCommand() {
// implicitly require `this`
return this->RunOnce(
[this] { m_hatchSolenoid.Set(wpi::DoubleSolenoid::FORWARD); });
[this] { hatchSolenoid.Set(wpi::DoubleSolenoid::FORWARD); });
}
wpi::cmd::CommandPtr HatchSubsystem::ReleaseHatchCommand() {
// implicitly require `this`
return this->RunOnce(
[this] { m_hatchSolenoid.Set(wpi::DoubleSolenoid::REVERSE); });
[this] { hatchSolenoid.Set(wpi::DoubleSolenoid::REVERSE); });
}
void HatchSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
@@ -30,6 +30,6 @@ void HatchSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
// Publish the solenoid state to telemetry.
builder.AddBooleanProperty(
"extended",
[this] { return m_hatchSolenoid.Get() == wpi::DoubleSolenoid::FORWARD; },
[this] { return hatchSolenoid.Get() == wpi::DoubleSolenoid::FORWARD; },
nullptr);
}

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

@@ -23,7 +23,7 @@ class Robot : public wpi::TimedRobot {
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
wpi::cmd::Command* m_autonomousCommand = nullptr;
wpi::cmd::Command* autonomousCommand = nullptr;
RobotContainer m_container;
RobotContainer container;
};

13
wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.hpp
View File

@@ -30,23 +30,22 @@ class RobotContainer {
private:
// The driver's controller
wpi::cmd::CommandGamepad m_driverController{
OIConstants::kDriverControllerPort};
wpi::cmd::CommandGamepad driverController{OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
// The robot's subsystems
DriveSubsystem m_drive;
HatchSubsystem m_hatch;
DriveSubsystem drive;
HatchSubsystem hatch;
// Commands owned by RobotContainer
// The autonomous routines
wpi::cmd::CommandPtr m_simpleAuto = autos::SimpleAuto(&m_drive);
wpi::cmd::CommandPtr m_complexAuto = autos::ComplexAuto(&m_drive, &m_hatch);
wpi::cmd::CommandPtr simpleAuto = autos::SimpleAuto(&drive);
wpi::cmd::CommandPtr complexAuto = autos::ComplexAuto(&drive, &hatch);
// The chooser for the autonomous routines
wpi::SendableChooser<wpi::cmd::Command*> m_chooser;
wpi::SendableChooser<wpi::cmd::Command*> chooser;
void ConfigureButtonBindings();
};

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

@@ -56,19 +56,19 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
// declared private and exposed only through public methods.
// The motor controllers
wpi::PWMSparkMax m_left1;
wpi::PWMSparkMax m_left2;
wpi::PWMSparkMax m_right1;
wpi::PWMSparkMax m_right2;
wpi::PWMSparkMax left1;
wpi::PWMSparkMax left2;
wpi::PWMSparkMax right1;
wpi::PWMSparkMax right2;
// The robot's drive
wpi::DifferentialDrive m_drive{
[&](double output) { m_left1.SetThrottle(output); },
[&](double output) { m_right1.SetThrottle(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left1.SetThrottle(output); },
[&](double output) { right1.SetThrottle(output); }};
// The left-side drive encoder
wpi::Encoder m_leftEncoder;
wpi::Encoder leftEncoder;
// The right-side drive encoder
wpi::Encoder m_rightEncoder;
wpi::Encoder rightEncoder;
};

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

@@ -31,5 +31,5 @@ class HatchSubsystem : public wpi::cmd::SubsystemBase {
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
wpi::DoubleSolenoid m_hatchSolenoid;
wpi::DoubleSolenoid hatchSolenoid;
};

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

@@ -44,10 +44,10 @@ void Robot::DisabledPeriodic() {}
* RobotContainer} class.
*/
void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
autonomousCommand = container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
if (autonomousCommand != nullptr) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(autonomousCommand);
}
}
@@ -58,9 +58,9 @@ void Robot::TeleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != nullptr) {
m_autonomousCommand->Cancel();
m_autonomousCommand = nullptr;
if (autonomousCommand != nullptr) {
autonomousCommand->Cancel();
autonomousCommand = nullptr;
}
}

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

@@ -15,22 +15,22 @@ RobotContainer::RobotContainer() {
// Initialize all of your commands and subsystems here
// Add commands to the autonomous command chooser
m_chooser.SetDefaultOption("Simple Auto", &m_simpleAuto);
m_chooser.AddOption("Complex Auto", &m_complexAuto);
chooser.SetDefaultOption("Simple Auto", &simpleAuto);
chooser.AddOption("Complex Auto", &complexAuto);
// Put the chooser on the dashboard
wpi::SmartDashboard::PutData("Autonomous", &m_chooser);
wpi::SmartDashboard::PutData("Autonomous", &chooser);
// Put subsystems to dashboard.
wpi::SmartDashboard::PutData("Drivetrain", &m_drive);
wpi::SmartDashboard::PutData("HatchSubsystem", &m_hatch);
wpi::SmartDashboard::PutData("Drivetrain", &drive);
wpi::SmartDashboard::PutData("HatchSubsystem", &hatch);
// Configure the button bindings
ConfigureButtonBindings();
// Set up default drive command
m_drive.SetDefaultCommand(DefaultDrive(
&m_drive, [this] { return -m_driverController.GetLeftY(); },
[this] { return -m_driverController.GetRightX(); }));
drive.SetDefaultCommand(DefaultDrive(
&drive, [this] { return -driverController.GetLeftY(); },
[this] { return -driverController.GetRightX(); }));
}
void RobotContainer::ConfigureButtonBindings() {
@@ -40,18 +40,17 @@ void RobotContainer::ConfigureButtonBindings() {
// the scheduler thus, no memory leaks!
// Grab the hatch when the 'South Face' button is pressed.
wpi::cmd::GamepadButton(&m_driverController, wpi::Gamepad::Button::SOUTH_FACE)
.OnTrue(GrabHatch(&m_hatch).ToPtr());
wpi::cmd::GamepadButton(&driverController, wpi::Gamepad::Button::SOUTH_FACE)
.OnTrue(GrabHatch(&hatch).ToPtr());
// Release the hatch when the 'East Face' button is pressed.
wpi::cmd::GamepadButton(&m_driverController, wpi::Gamepad::Button::EAST_FACE)
.OnTrue(ReleaseHatch(&m_hatch).ToPtr());
wpi::cmd::GamepadButton(&driverController, wpi::Gamepad::Button::EAST_FACE)
.OnTrue(ReleaseHatch(&hatch).ToPtr());
// While holding the bumper button, drive at half velocity
wpi::cmd::GamepadButton(&m_driverController,
wpi::Gamepad::Button::RIGHT_BUMPER)
.WhileTrue(HalveDriveVelocity(&m_drive).ToPtr());
wpi::cmd::GamepadButton(&driverController, wpi::Gamepad::Button::RIGHT_BUMPER)
.WhileTrue(HalveDriveVelocity(&drive).ToPtr());
}
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {
// Runs the chosen command in autonomous
return m_chooser.GetSelected();
return chooser.GetSelected();
}

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

@@ -9,12 +9,12 @@
DefaultDrive::DefaultDrive(DriveSubsystem* subsystem,
std::function<double()> forward,
std::function<double()> rotation)
: m_drive{subsystem},
m_forward{std::move(forward)},
m_rotation{std::move(rotation)} {
: drive{subsystem},
forward{std::move(forward)},
rotation{std::move(rotation)} {
AddRequirements(subsystem);
}
void DefaultDrive::Execute() {
m_drive->ArcadeDrive(m_forward(), m_rotation());
drive->ArcadeDrive(forward(), rotation());
}

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

@@ -8,23 +8,23 @@
DriveDistance::DriveDistance(double inches, double velocity,
DriveSubsystem* subsystem)
: m_drive(subsystem), m_distance(inches), m_velocity(velocity) {
: drive(subsystem), distance(inches), velocity(velocity) {
AddRequirements(subsystem);
}
void DriveDistance::Initialize() {
m_drive->ResetEncoders();
m_drive->ArcadeDrive(m_velocity, 0);
drive->ResetEncoders();
drive->ArcadeDrive(velocity, 0);
}
void DriveDistance::Execute() {
m_drive->ArcadeDrive(m_velocity, 0);
drive->ArcadeDrive(velocity, 0);
}
void DriveDistance::End(bool interrupted) {
m_drive->ArcadeDrive(0, 0);
drive->ArcadeDrive(0, 0);
}
bool DriveDistance::IsFinished() {
return std::abs(m_drive->GetAverageEncoderDistance()) >= m_distance;
return std::abs(drive->GetAverageEncoderDistance()) >= distance;
}

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

@@ -4,12 +4,12 @@
#include "commands/GrabHatch.hpp"
GrabHatch::GrabHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) {
GrabHatch::GrabHatch(HatchSubsystem* subsystem) : hatch(subsystem) {
AddRequirements(subsystem);
}
void GrabHatch::Initialize() {
m_hatch->GrabHatch();
hatch->GrabHatch();
}
bool GrabHatch::IsFinished() {

6
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveVelocity.cpp
View File

@@ -5,12 +5,12 @@
#include "commands/HalveDriveVelocity.hpp"
HalveDriveVelocity::HalveDriveVelocity(DriveSubsystem* subsystem)
: m_drive(subsystem) {}
: drive(subsystem) {}
void HalveDriveVelocity::Initialize() {
m_drive->SetMaxOutput(0.5);
drive->SetMaxOutput(0.5);
}
void HalveDriveVelocity::End(bool interrupted) {
m_drive->SetMaxOutput(1);
drive->SetMaxOutput(1);
}

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

@@ -4,12 +4,12 @@
#include "commands/ReleaseHatch.hpp"
ReleaseHatch::ReleaseHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) {
ReleaseHatch::ReleaseHatch(HatchSubsystem* subsystem) : hatch(subsystem) {
AddRequirements(subsystem);
}
void ReleaseHatch::Initialize() {
m_hatch->ReleaseHatch();
hatch->ReleaseHatch();
}
bool ReleaseHatch::IsFinished() {

41
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -9,26 +9,26 @@
using namespace DriveConstants;
DriveSubsystem::DriveSubsystem()
: m_left1{kLeftMotor1Port},
m_left2{kLeftMotor2Port},
m_right1{kRightMotor1Port},
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
wpi::util::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_right1);
: left1{kLeftMotor1Port},
left2{kLeftMotor2Port},
right1{kRightMotor1Port},
right2{kRightMotor2Port},
leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
wpi::util::SendableRegistry::AddChild(&drive, &left1);
wpi::util::SendableRegistry::AddChild(&drive, &right1);
m_left1.AddFollower(m_left2);
m_right1.AddFollower(m_right2);
left1.AddFollower(left2);
right1.AddFollower(right2);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_right1.SetInverted(true);
right1.SetInverted(true);
// Set the distance per pulse for the encoders
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
}
void DriveSubsystem::Periodic() {
@@ -36,20 +36,20 @@ void DriveSubsystem::Periodic() {
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
drive.ArcadeDrive(fwd, rot);
}
void DriveSubsystem::ResetEncoders() {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
leftEncoder.Reset();
rightEncoder.Reset();
}
double DriveSubsystem::GetAverageEncoderDistance() {
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
return (leftEncoder.GetDistance() + rightEncoder.GetDistance()) / 2.0;
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
drive.SetMaxOutput(maxOutput);
}
void DriveSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
@@ -57,8 +57,7 @@ void DriveSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
// Publish encoder distances to telemetry.
builder.AddDoubleProperty(
"leftDistance", [this] { return m_leftEncoder.GetDistance(); }, nullptr);
"leftDistance", [this] { return leftEncoder.GetDistance(); }, nullptr);
builder.AddDoubleProperty(
"rightDistance", [this] { return m_rightEncoder.GetDistance(); },
nullptr);
"rightDistance", [this] { return rightEncoder.GetDistance(); }, nullptr);
}

10
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp
View File

@@ -9,15 +9,15 @@
using namespace HatchConstants;
HatchSubsystem::HatchSubsystem()
: m_hatchSolenoid{0, wpi::PneumaticsModuleType::CTRE_PCM,
kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
: hatchSolenoid{0, wpi::PneumaticsModuleType::CTRE_PCM,
kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
void HatchSubsystem::GrabHatch() {
m_hatchSolenoid.Set(wpi::DoubleSolenoid::FORWARD);
hatchSolenoid.Set(wpi::DoubleSolenoid::FORWARD);
}
void HatchSubsystem::ReleaseHatch() {
m_hatchSolenoid.Set(wpi::DoubleSolenoid::REVERSE);
hatchSolenoid.Set(wpi::DoubleSolenoid::REVERSE);
}
void HatchSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
@@ -26,6 +26,6 @@ void HatchSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
// Publish the solenoid state to telemetry.
builder.AddBooleanProperty(
"extended",
[this] { return m_hatchSolenoid.Get() == wpi::DoubleSolenoid::FORWARD; },
[this] { return hatchSolenoid.Get() == wpi::DoubleSolenoid::FORWARD; },
nullptr);
}

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

@@ -23,7 +23,7 @@ class Robot : public wpi::TimedRobot {
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
wpi::cmd::Command* m_autonomousCommand = nullptr;
wpi::cmd::Command* autonomousCommand = nullptr;
RobotContainer m_container;
RobotContainer container;
};

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

@@ -31,19 +31,19 @@ class RobotContainer {
// The robot's subsystems and commands are defined here...
// The robot's subsystems
DriveSubsystem m_drive;
HatchSubsystem m_hatch;
DriveSubsystem drive;
HatchSubsystem hatch;
// The autonomous routines
DriveDistance m_simpleAuto{AutoConstants::kAutoDriveDistanceInches,
AutoConstants::kAutoDriveVelocity, &m_drive};
ComplexAuto m_complexAuto{&m_drive, &m_hatch};
DriveDistance simpleAuto{AutoConstants::kAutoDriveDistanceInches,
AutoConstants::kAutoDriveVelocity, &drive};
ComplexAuto complexAuto{&drive, &hatch};
// The chooser for the autonomous routines
wpi::SendableChooser<wpi::cmd::Command*> m_chooser;
wpi::SendableChooser<wpi::cmd::Command*> chooser;
// The driver's controller
wpi::Gamepad m_driverController{OIConstants::kDriverControllerPort};
wpi::Gamepad driverController{OIConstants::kDriverControllerPort};
void ConfigureButtonBindings();
};

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

@@ -33,7 +33,7 @@ class DefaultDrive
void Execute() override;
private:
DriveSubsystem* m_drive;
std::function<double()> m_forward;
std::function<double()> m_rotation;
DriveSubsystem* drive;
std::function<double()> forward;
std::function<double()> rotation;
};

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

@@ -29,7 +29,7 @@ class DriveDistance
bool IsFinished() override;
private:
DriveSubsystem* m_drive;
double m_distance;
double m_velocity;
DriveSubsystem* drive;
double distance;
double velocity;
};

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

@@ -24,5 +24,5 @@ class GrabHatch : public wpi::cmd::CommandHelper<wpi::cmd::Command, GrabHatch> {
bool IsFinished() override;
private:
HatchSubsystem* m_hatch;
HatchSubsystem* hatch;
};

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

@@ -18,5 +18,5 @@ class HalveDriveVelocity
void End(bool interrupted) override;
private:
DriveSubsystem* m_drive;
DriveSubsystem* drive;
};

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

@@ -25,5 +25,5 @@ class ReleaseHatch
bool IsFinished() override;
private:
HatchSubsystem* m_hatch;
HatchSubsystem* hatch;
};

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

@@ -56,19 +56,19 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
// declared private and exposed only through public methods.
// The motor controllers
wpi::PWMSparkMax m_left1;
wpi::PWMSparkMax m_left2;
wpi::PWMSparkMax m_right1;
wpi::PWMSparkMax m_right2;
wpi::PWMSparkMax left1;
wpi::PWMSparkMax left2;
wpi::PWMSparkMax right1;
wpi::PWMSparkMax right2;
// The robot's drive
wpi::DifferentialDrive m_drive{
[&](double output) { m_left1.SetThrottle(output); },
[&](double output) { m_right1.SetThrottle(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left1.SetThrottle(output); },
[&](double output) { right1.SetThrottle(output); }};
// The left-side drive encoder
wpi::Encoder m_leftEncoder;
wpi::Encoder leftEncoder;
// The right-side drive encoder
wpi::Encoder m_rightEncoder;
wpi::Encoder rightEncoder;
};

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

@@ -30,5 +30,5 @@ class HatchSubsystem : public wpi::cmd::SubsystemBase {
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
wpi::DoubleSolenoid m_hatchSolenoid;
wpi::DoubleSolenoid hatchSolenoid;
};

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

@@ -8,48 +8,48 @@
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()}};
return {wpi::units::meter_t{frontLeftEncoder.GetDistance()},
wpi::units::meter_t{frontRightEncoder.GetDistance()},
wpi::units::meter_t{backLeftEncoder.GetDistance()},
wpi::units::meter_t{backRightEncoder.GetDistance()}};
}
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()}};
return {wpi::units::meters_per_second_t{frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{backRightEncoder.GetRate()}};
}
void Drivetrain::SetVelocities(
const wpi::math::MecanumDriveWheelVelocities& wheelVelocities) {
const auto frontLeftFeedforward =
m_feedforward.Calculate(wheelVelocities.frontLeft);
feedforward.Calculate(wheelVelocities.frontLeft);
const auto frontRightFeedforward =
m_feedforward.Calculate(wheelVelocities.frontRight);
feedforward.Calculate(wheelVelocities.frontRight);
const auto backLeftFeedforward =
m_feedforward.Calculate(wheelVelocities.rearLeft);
feedforward.Calculate(wheelVelocities.rearLeft);
const auto backRightFeedforward =
m_feedforward.Calculate(wheelVelocities.rearRight);
feedforward.Calculate(wheelVelocities.rearRight);
const double frontLeftOutput = m_frontLeftPIDController.Calculate(
m_frontLeftEncoder.GetRate(), wheelVelocities.frontLeft.value());
const double frontRightOutput = m_frontRightPIDController.Calculate(
m_frontRightEncoder.GetRate(), wheelVelocities.frontRight.value());
const double backLeftOutput = m_backLeftPIDController.Calculate(
m_backLeftEncoder.GetRate(), wheelVelocities.rearLeft.value());
const double backRightOutput = m_backRightPIDController.Calculate(
m_backRightEncoder.GetRate(), wheelVelocities.rearRight.value());
const double frontLeftOutput = frontLeftPIDController.Calculate(
frontLeftEncoder.GetRate(), wheelVelocities.frontLeft.value());
const double frontRightOutput = frontRightPIDController.Calculate(
frontRightEncoder.GetRate(), wheelVelocities.frontRight.value());
const double backLeftOutput = backLeftPIDController.Calculate(
backLeftEncoder.GetRate(), wheelVelocities.rearLeft.value());
const double backRightOutput = backRightPIDController.Calculate(
backRightEncoder.GetRate(), wheelVelocities.rearRight.value());
m_frontLeftMotor.SetVoltage(wpi::units::volt_t{frontLeftOutput} +
frontLeftFeedforward);
m_frontRightMotor.SetVoltage(wpi::units::volt_t{frontRightOutput} +
frontRightFeedforward);
m_backLeftMotor.SetVoltage(wpi::units::volt_t{backLeftOutput} +
backLeftFeedforward);
m_backRightMotor.SetVoltage(wpi::units::volt_t{backRightOutput} +
backRightFeedforward);
frontLeftMotor.SetVoltage(wpi::units::volt_t{frontLeftOutput} +
frontLeftFeedforward);
frontRightMotor.SetVoltage(wpi::units::volt_t{frontRightOutput} +
frontRightFeedforward);
backLeftMotor.SetVoltage(wpi::units::volt_t{backLeftOutput} +
backLeftFeedforward);
backRightMotor.SetVoltage(wpi::units::volt_t{backRightOutput} +
backRightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
@@ -58,14 +58,13 @@ void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::second_t period) {
wpi::math::ChassisVelocities chassisVelocities{xVelocity, yVelocity, rot};
if (fieldRelative) {
chassisVelocities =
chassisVelocities.ToRobotRelative(m_imu.GetRotation2d());
chassisVelocities = chassisVelocities.ToRobotRelative(imu.GetRotation2d());
}
SetVelocities(
m_kinematics.ToWheelVelocities(chassisVelocities.Discretize(period))
kinematics.ToWheelVelocities(chassisVelocities.Discretize(period))
.Desaturate(kMaxVelocity));
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_imu.GetRotation2d(), GetCurrentWheelDistances());
odometry.Update(imu.GetRotation2d(), GetCurrentWheelDistances());
}

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

@@ -11,43 +11,41 @@ class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
DriveWithJoystick(false);
m_mecanum.UpdateOdometry();
mecanum.UpdateOdometry();
}
void TeleopPeriodic() override { DriveWithJoystick(true); }
private:
wpi::Gamepad m_controller{0};
Drivetrain m_mecanum;
wpi::Gamepad controller{0};
Drivetrain mecanum;
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
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};
wpi::math::SlewRateLimiter<wpi::units::scalar> xVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> yVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xVelocity =
-m_xVelocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
const auto xVelocity = -xVelocityLimiter.Calculate(controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
// 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 yVelocity =
-m_yVelocityLimiter.Calculate(m_controller.GetLeftX()) *
Drivetrain::kMaxVelocity;
const auto yVelocity = -yVelocityLimiter.Calculate(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()) *
const auto rot = -rotLimiter.Calculate(controller.GetRightX()) *
Drivetrain::kMaxAngularVelocity;
m_mecanum.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
mecanum.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
}
};

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

@@ -22,12 +22,12 @@
class Drivetrain {
public:
Drivetrain() {
m_imu.ResetYaw();
imu.ResetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_frontRightMotor.SetInverted(true);
m_backRightMotor.SetInverted(true);
frontRightMotor.SetInverted(true);
backRightMotor.SetInverted(true);
}
wpi::math::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
@@ -46,37 +46,37 @@ class Drivetrain {
std::numbers::pi}; // 1/2 rotation per second
private:
wpi::PWMSparkMax m_frontLeftMotor{1};
wpi::PWMSparkMax m_frontRightMotor{2};
wpi::PWMSparkMax m_backLeftMotor{3};
wpi::PWMSparkMax m_backRightMotor{4};
wpi::PWMSparkMax frontLeftMotor{1};
wpi::PWMSparkMax frontRightMotor{2};
wpi::PWMSparkMax backLeftMotor{3};
wpi::PWMSparkMax backRightMotor{4};
wpi::Encoder m_frontLeftEncoder{0, 1};
wpi::Encoder m_frontRightEncoder{2, 3};
wpi::Encoder m_backLeftEncoder{4, 5};
wpi::Encoder m_backRightEncoder{6, 7};
wpi::Encoder frontLeftEncoder{0, 1};
wpi::Encoder frontRightEncoder{2, 3};
wpi::Encoder backLeftEncoder{4, 5};
wpi::Encoder backRightEncoder{6, 7};
wpi::math::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
wpi::math::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
wpi::math::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
wpi::math::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
wpi::math::Translation2d frontLeftLocation{0.381_m, 0.381_m};
wpi::math::Translation2d frontRightLocation{0.381_m, -0.381_m};
wpi::math::Translation2d backLeftLocation{-0.381_m, 0.381_m};
wpi::math::Translation2d backRightLocation{-0.381_m, -0.381_m};
wpi::math::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController frontLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController frontRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController backLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController backRightPIDController{1.0, 0.0, 0.0};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::FLAT};
wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
wpi::math::MecanumDriveKinematics m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
wpi::math::MecanumDriveKinematics kinematics{
frontLeftLocation, frontRightLocation, backLeftLocation,
backRightLocation};
wpi::math::MecanumDriveOdometry m_odometry{
m_kinematics, m_imu.GetRotation2d(), GetCurrentWheelDistances()};
wpi::math::MecanumDriveOdometry odometry{kinematics, imu.GetRotation2d(),
GetCurrentWheelDistances()};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward{
1_V, 3_V / 1_mps};
};

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

@@ -16,15 +16,15 @@
class Robot : public wpi::TimedRobot {
public:
Robot() {
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rearLeft);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rearRight);
wpi::util::SendableRegistry::AddChild(&robotDrive, &frontLeft);
wpi::util::SendableRegistry::AddChild(&robotDrive, &rearLeft);
wpi::util::SendableRegistry::AddChild(&robotDrive, &frontRight);
wpi::util::SendableRegistry::AddChild(&robotDrive, &rearRight);
// Invert the right side motors. You may need to change or remove this to
// match your robot.
m_frontRight.SetInverted(true);
m_rearRight.SetInverted(true);
frontRight.SetInverted(true);
rearRight.SetInverted(true);
}
/**
@@ -34,8 +34,8 @@ class Robot : public wpi::TimedRobot {
/* Use the joystick Y axis for forward movement, X axis for lateral
* movement, and Z axis for rotation.
*/
m_robotDrive.DriveCartesian(-m_joystick.GetY(), -m_joystick.GetX(),
-m_joystick.GetZ(), m_imu.GetRotation2d());
robotDrive.DriveCartesian(-joystick.GetY(), -joystick.GetX(),
-joystick.GetZ(), imu.GetRotation2d());
}
private:
@@ -47,18 +47,18 @@ class Robot : public wpi::TimedRobot {
wpi::OnboardIMU::FLAT;
static constexpr int kJoystickPort = 0;
wpi::PWMSparkMax m_frontLeft{kFrontLeftMotorPort};
wpi::PWMSparkMax m_rearLeft{kRearLeftMotorPort};
wpi::PWMSparkMax m_frontRight{kFrontRightMotorPort};
wpi::PWMSparkMax m_rearRight{kRearRightMotorPort};
wpi::MecanumDrive m_robotDrive{
[&](double output) { m_frontLeft.SetThrottle(output); },
[&](double output) { m_rearLeft.SetThrottle(output); },
[&](double output) { m_frontRight.SetThrottle(output); },
[&](double output) { m_rearRight.SetThrottle(output); }};
wpi::PWMSparkMax frontLeft{kFrontLeftMotorPort};
wpi::PWMSparkMax rearLeft{kRearLeftMotorPort};
wpi::PWMSparkMax frontRight{kFrontRightMotorPort};
wpi::PWMSparkMax rearRight{kRearRightMotorPort};
wpi::MecanumDrive robotDrive{
[&](double output) { frontLeft.SetThrottle(output); },
[&](double output) { rearLeft.SetThrottle(output); },
[&](double output) { frontRight.SetThrottle(output); },
[&](double output) { rearRight.SetThrottle(output); }};
wpi::OnboardIMU m_imu{kIMUMountOrientation};
wpi::Joystick m_joystick{kJoystickPort};
wpi::OnboardIMU imu{kIMUMountOrientation};
wpi::Joystick joystick{kJoystickPort};
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -9,42 +9,42 @@
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()}};
return {wpi::units::meter_t{frontLeftEncoder.GetDistance()},
wpi::units::meter_t{frontRightEncoder.GetDistance()},
wpi::units::meter_t{backLeftEncoder.GetDistance()},
wpi::units::meter_t{backRightEncoder.GetDistance()}};
}
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()}};
return {wpi::units::meters_per_second_t{frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{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&)>
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);
};
calcAndSetVelocities =
[&feedforward = feedforward](wpi::units::meters_per_second_t velocity,
const auto& encoder, auto& controller,
auto& motor) {
auto ff = feedforward.Calculate(velocity);
double output =
controller.Calculate(encoder.GetRate(), velocity.value());
motor.SetVoltage(wpi::units::volt_t{output} + ff);
};
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);
calcAndSetVelocities(wheelVelocities.frontLeft, frontLeftEncoder,
frontLeftPIDController, frontLeftMotor);
calcAndSetVelocities(wheelVelocities.frontRight, frontRightEncoder,
frontRightPIDController, frontRightMotor);
calcAndSetVelocities(wheelVelocities.rearLeft, backLeftEncoder,
backLeftPIDController, backLeftMotor);
calcAndSetVelocities(wheelVelocities.rearRight, backRightEncoder,
backRightPIDController, backRightMotor);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
@@ -54,23 +54,23 @@ void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::math::ChassisVelocities chassisVelocities{xVelocity, yVelocity, rot};
if (fieldRelative) {
chassisVelocities = chassisVelocities.ToRobotRelative(
m_poseEstimator.GetEstimatedPosition().Rotation());
poseEstimator.GetEstimatedPosition().Rotation());
}
SetVelocities(
m_kinematics.ToWheelVelocities(chassisVelocities.Discretize(period))
kinematics.ToWheelVelocities(chassisVelocities.Discretize(period))
.Desaturate(kMaxVelocity));
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(
m_imu.GetRotation2d(),
poseEstimator.Update(
imu.GetRotation2d(),
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
// or timestamps.
m_poseEstimator.AddVisionMeasurement(
poseEstimator.AddVisionMeasurement(
ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
m_poseEstimator.GetEstimatedPosition()),
poseEstimator.GetEstimatedPosition()),
wpi::Timer::GetTimestamp() - 0.3_s);
}

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

@@ -11,43 +11,41 @@ class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
DriveWithJoystick(false);
m_mecanum.UpdateOdometry();
mecanum.UpdateOdometry();
}
void TeleopPeriodic() override { DriveWithJoystick(true); }
private:
wpi::Gamepad m_controller{0};
Drivetrain m_mecanum;
wpi::Gamepad controller{0};
Drivetrain mecanum;
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
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};
wpi::math::SlewRateLimiter<wpi::units::scalar> xVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> yVelocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
const auto xVelocity =
-m_xVelocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
const auto xVelocity = -xVelocityLimiter.Calculate(controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
// 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 yVelocity =
-m_yVelocityLimiter.Calculate(m_controller.GetLeftX()) *
Drivetrain::kMaxVelocity;
const auto yVelocity = -yVelocityLimiter.Calculate(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()) *
const auto rot = -rotLimiter.Calculate(controller.GetRightX()) *
Drivetrain::kMaxAngularVelocity;
m_mecanum.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
mecanum.Drive(xVelocity, yVelocity, rot, fieldRelative, GetPeriod());
}
};

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

@@ -22,12 +22,12 @@
class Drivetrain {
public:
Drivetrain() {
m_imu.ResetYaw();
imu.ResetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_frontRightMotor.SetInverted(true);
m_backRightMotor.SetInverted(true);
frontRightMotor.SetInverted(true);
backRightMotor.SetInverted(true);
}
wpi::math::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
@@ -45,40 +45,40 @@ class Drivetrain {
std::numbers::pi}; // 1/2 rotation per second
private:
wpi::PWMSparkMax m_frontLeftMotor{1};
wpi::PWMSparkMax m_frontRightMotor{2};
wpi::PWMSparkMax m_backLeftMotor{3};
wpi::PWMSparkMax m_backRightMotor{4};
wpi::PWMSparkMax frontLeftMotor{1};
wpi::PWMSparkMax frontRightMotor{2};
wpi::PWMSparkMax backLeftMotor{3};
wpi::PWMSparkMax backRightMotor{4};
wpi::Encoder m_frontLeftEncoder{0, 1};
wpi::Encoder m_frontRightEncoder{2, 3};
wpi::Encoder m_backLeftEncoder{4, 5};
wpi::Encoder m_backRightEncoder{6, 7};
wpi::Encoder frontLeftEncoder{0, 1};
wpi::Encoder frontRightEncoder{2, 3};
wpi::Encoder backLeftEncoder{4, 5};
wpi::Encoder backRightEncoder{6, 7};
wpi::math::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
wpi::math::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
wpi::math::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
wpi::math::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
wpi::math::Translation2d frontLeftLocation{0.381_m, 0.381_m};
wpi::math::Translation2d frontRightLocation{0.381_m, -0.381_m};
wpi::math::Translation2d backLeftLocation{-0.381_m, 0.381_m};
wpi::math::Translation2d backRightLocation{-0.381_m, -0.381_m};
wpi::math::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController frontLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController frontRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController backLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController backRightPIDController{1.0, 0.0, 0.0};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::FLAT};
wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
wpi::math::MecanumDriveKinematics m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
wpi::math::MecanumDriveKinematics kinematics{
frontLeftLocation, frontRightLocation, backLeftLocation,
backRightLocation};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward{
1_V, 3_V / 1_mps};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::MecanumDrivePoseEstimator m_poseEstimator{
m_kinematics, m_imu.GetRotation2d(), GetCurrentWheelDistances(),
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
wpi::math::MecanumDrivePoseEstimator poseEstimator{
kinematics, imu.GetRotation2d(), GetCurrentWheelDistances(),
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
};

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

@@ -30,43 +30,40 @@ class Robot : public wpi::TimedRobot {
public:
Robot() {
m_elevatorEncoder.SetDistancePerPulse(kMetersPerPulse);
elevatorEncoder.SetDistancePerPulse(kMetersPerPulse);
// publish to dashboard
wpi::SmartDashboard::PutData("Mech2d", &m_mech);
wpi::SmartDashboard::PutData("Mech2d", &mech);
}
void RobotPeriodic() override {
// update the dashboard mechanism's state
m_elevator->SetLength(kElevatorMinimumLength +
m_elevatorEncoder.GetDistance());
m_wrist->SetAngle(wpi::units::degree_t{m_wristPotentiometer.Get()});
elevator->SetLength(kElevatorMinimumLength + elevatorEncoder.GetDistance());
wrist->SetAngle(wpi::units::degree_t{wristPotentiometer.Get()});
}
void TeleopPeriodic() override {
m_elevatorMotor.SetThrottle(m_joystick.GetRawAxis(0));
m_wristMotor.SetThrottle(m_joystick.GetRawAxis(1));
elevatorMotor.SetThrottle(joystick.GetRawAxis(0));
wristMotor.SetThrottle(joystick.GetRawAxis(1));
}
private:
wpi::PWMSparkMax m_elevatorMotor{0};
wpi::PWMSparkMax m_wristMotor{1};
wpi::Encoder m_elevatorEncoder{0, 1};
wpi::AnalogPotentiometer m_wristPotentiometer{1, 90};
wpi::Joystick m_joystick{0};
wpi::PWMSparkMax elevatorMotor{0};
wpi::PWMSparkMax wristMotor{1};
wpi::Encoder elevatorEncoder{0, 1};
wpi::AnalogPotentiometer wristPotentiometer{1, 90};
wpi::Joystick joystick{0};
// the main mechanism object
wpi::Mechanism2d m_mech{3, 3};
wpi::Mechanism2d mech{3, 3};
// the mechanism root node
wpi::MechanismRoot2d* m_root = m_mech.GetRoot("climber", 2, 0);
wpi::MechanismRoot2d* root = mech.GetRoot("climber", 2, 0);
// MechanismLigament2d objects represent each "section"/"stage" of the
// mechanism, and are based off the root node or another ligament object
wpi::MechanismLigament2d* m_elevator =
m_root->Append<wpi::MechanismLigament2d>("elevator", 1, 90_deg);
wpi::MechanismLigament2d* m_wrist =
m_elevator->Append<wpi::MechanismLigament2d>(
"wrist", 0.5, 90_deg, 6,
wpi::util::Color8Bit{wpi::util::Color::PURPLE});
wpi::MechanismLigament2d* elevator =
root->Append<wpi::MechanismLigament2d>("elevator", 1, 90_deg);
wpi::MechanismLigament2d* wrist = elevator->Append<wpi::MechanismLigament2d>(
"wrist", 0.5, 90_deg, 6, wpi::util::Color8Bit{wpi::util::Color::PURPLE});
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -13,37 +13,35 @@ void RapidReactCommandBot::ConfigureBindings() {
// Automatically run the storage motor whenever the ball storage is not full,
// and turn it off whenever it fills. Uses subsystem-hosted trigger to
// improve readability and make inter-subsystem communication easier.
m_storage.HasCargo.WhileFalse(m_storage.RunCommand());
storage.HasCargo.WhileFalse(storage.RunCommand());
// Automatically disable and retract the intake whenever the ball storage is
// full.
m_storage.HasCargo.OnTrue(m_intake.RetractCommand());
storage.HasCargo.OnTrue(intake.RetractCommand());
// Control the drive with split-stick arcade controls
m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand(
[this] { return -m_driverController.GetLeftY(); },
[this] { return -m_driverController.GetRightX(); }));
drive.SetDefaultCommand(drive.ArcadeDriveCommand(
[this] { return -driverController.GetLeftY(); },
[this] { return -driverController.GetRightX(); }));
// Deploy the intake with the West Face button
m_driverController.WestFace().OnTrue(m_intake.IntakeCommand());
driverController.WestFace().OnTrue(intake.IntakeCommand());
// Retract the intake with the North Face button
m_driverController.NorthFace().OnTrue(m_intake.RetractCommand());
driverController.NorthFace().OnTrue(intake.RetractCommand());
// Fire the shooter with the South Face button
m_driverController.SouthFace().OnTrue(
wpi::cmd::Parallel(
m_shooter.ShootCommand(ShooterConstants::kShooterTarget),
m_storage.RunCommand())
driverController.SouthFace().OnTrue(
wpi::cmd::Parallel(shooter.ShootCommand(ShooterConstants::kShooterTarget),
storage.RunCommand())
// Since we composed this inline we should give it a name
.WithName("Shoot"));
// Toggle compressor with the Start button
m_driverController.Start().ToggleOnTrue(
m_pneumatics.DisableCompressorCommand());
driverController.Start().ToggleOnTrue(pneumatics.DisableCompressorCommand());
}
wpi::cmd::CommandPtr RapidReactCommandBot::GetAutonomousCommand() {
return m_drive
return drive
.DriveDistanceCommand(AutoConstants::kDriveDistance,
AutoConstants::kDriveVelocity)
.WithTimeout(AutoConstants::kTimeout);

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

@@ -6,7 +6,7 @@
Robot::Robot() {
// Configure default commands and condition bindings on robot startup
m_robot.ConfigureBindings();
robot.ConfigureBindings();
}
void Robot::RobotPeriodic() {
@@ -23,11 +23,11 @@ void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit() {
m_autonomousCommand = m_robot.GetAutonomousCommand();
autonomousCommand = robot.GetAutonomousCommand();
if (m_autonomousCommand) {
if (autonomousCommand) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(
m_autonomousCommand.value());
autonomousCommand.value());
}
}
@@ -38,8 +38,8 @@ void Robot::TeleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand) {
m_autonomousCommand->Cancel();
if (autonomousCommand) {
autonomousCommand->Cancel();
}
}

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

@@ -10,34 +10,34 @@
#include "wpi/system/RobotController.hpp"
Drive::Drive() {
wpi::util::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
wpi::util::SendableRegistry::AddChild(&drive, &leftLeader);
wpi::util::SendableRegistry::AddChild(&drive, &rightLeader);
m_leftLeader.AddFollower(m_leftFollower);
m_rightLeader.AddFollower(m_rightFollower);
leftLeader.AddFollower(leftFollower);
rightLeader.AddFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.SetInverted(true);
rightLeader.SetInverted(true);
// Sets the distance per pulse for the encoders
m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
m_rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
// Set the controller to be continuous (because it is an angle controller)
m_controller.EnableContinuousInput(-180_deg, 180_deg);
controller.EnableContinuousInput(-180_deg, 180_deg);
// Set the controller tolerance - the delta tolerance ensures the robot is
// stationary at the setpoint before it is considered as having reached the
// reference
m_controller.SetTolerance(DriveConstants::kTurnTolerance,
DriveConstants::kTurnRateTolerance);
controller.SetTolerance(DriveConstants::kTurnTolerance,
DriveConstants::kTurnRateTolerance);
}
wpi::cmd::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
std::function<double()> rot) {
return Run([this, fwd = std::move(fwd), rot = std::move(rot)] {
m_drive.ArcadeDrive(fwd(), rot());
drive.ArcadeDrive(fwd(), rot());
})
.WithName("ArcadeDrive");
}
@@ -46,34 +46,32 @@ wpi::cmd::CommandPtr Drive::DriveDistanceCommand(wpi::units::meter_t distance,
double velocity) {
return RunOnce([this] {
// Reset encoders at the start of the command
m_leftEncoder.Reset();
m_rightEncoder.Reset();
leftEncoder.Reset();
rightEncoder.Reset();
})
// Drive forward at specified velocity
.AndThen(Run([this, velocity] { m_drive.ArcadeDrive(velocity, 0.0); }))
.AndThen(Run([this, velocity] { drive.ArcadeDrive(velocity, 0.0); }))
.Until([this, distance] {
return wpi::units::math::max(
wpi::units::meter_t(m_leftEncoder.GetDistance()),
wpi::units::meter_t(m_rightEncoder.GetDistance())) >=
distance;
wpi::units::meter_t(leftEncoder.GetDistance()),
wpi::units::meter_t(rightEncoder.GetDistance())) >= distance;
})
// Stop the drive when the command ends
.FinallyDo([this](bool interrupted) { m_drive.StopMotor(); });
.FinallyDo([this](bool interrupted) { drive.StopMotor(); });
}
wpi::cmd::CommandPtr Drive::TurnToAngleCommand(wpi::units::degree_t angle) {
return StartRun(
[this] { m_controller.Reset(m_imu.GetRotation2d().Degrees()); },
[this, angle] {
m_drive.ArcadeDrive(
0, m_controller.Calculate(m_imu.GetRotation2d().Degrees(),
angle) +
// Divide feedforward voltage by battery voltage to
// normalize it to [-1, 1]
m_feedforward.Calculate(
m_controller.GetSetpoint().velocity) /
wpi::RobotController::GetBatteryVoltage());
})
.Until([this] { return m_controller.AtGoal(); })
.FinallyDo([this] { m_drive.ArcadeDrive(0, 0); });
return StartRun([this] { controller.Reset(imu.GetRotation2d().Degrees()); },
[this, angle] {
drive.ArcadeDrive(
0, controller.Calculate(imu.GetRotation2d().Degrees(),
angle) +
// Divide feedforward voltage by battery voltage
// to normalize it to [-1, 1]
feedforward.Calculate(
controller.GetSetpoint().velocity) /
wpi::RobotController::GetBatteryVoltage());
})
.Until([this] { return controller.AtGoal(); })
.FinallyDo([this] { drive.ArcadeDrive(0, 0); });
}

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

@@ -5,15 +5,15 @@
#include "subsystems/Intake.hpp"
wpi::cmd::CommandPtr Intake::IntakeCommand() {
return RunOnce([this] { m_piston.Set(wpi::DoubleSolenoid::FORWARD); })
.AndThen(Run([this] { m_motor.SetThrottle(1.0); }))
return RunOnce([this] { piston.Set(wpi::DoubleSolenoid::FORWARD); })
.AndThen(Run([this] { motor.SetThrottle(1.0); }))
.WithName("Intake");
}
wpi::cmd::CommandPtr Intake::RetractCommand() {
return RunOnce([this] {
m_motor.Disable();
m_piston.Set(wpi::DoubleSolenoid::REVERSE);
motor.Disable();
piston.Set(wpi::DoubleSolenoid::REVERSE);
})
.WithName("Retract");
}

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

@@ -10,18 +10,18 @@ wpi::cmd::CommandPtr Pneumatics::DisableCompressorCommand() {
return StartEnd(
[&] {
// Disable closed-loop mode on the compressor.
m_compressor.Disable();
compressor.Disable();
},
[&] {
// Enable closed-loop mode based on the digital pressure switch
// connected to the PCM/PH. The switch is open when the pressure is over
// ~120 PSI.
m_compressor.EnableDigital();
compressor.EnableDigital();
});
}
wpi::units::pounds_per_square_inch_t Pneumatics::GetPressure() {
// Get the pressure (in PSI) from an analog pressure sensor connected to
// the RIO.
return wpi::units::pounds_per_square_inch_t{m_pressureTransducer.Get()};
return wpi::units::pounds_per_square_inch_t{pressureTransducer.Get()};
}

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

@@ -7,13 +7,13 @@
#include "wpi/commands2/Commands.hpp"
Shooter::Shooter() {
m_shooterFeedback.SetTolerance(ShooterConstants::kShooterTolerance.value());
m_shooterEncoder.SetDistancePerPulse(
shooterFeedback.SetTolerance(ShooterConstants::kShooterTolerance.value());
shooterEncoder.SetDistancePerPulse(
ShooterConstants::kEncoderDistancePerPulse);
SetDefaultCommand(RunOnce([this] {
m_shooterMotor.Disable();
m_feederMotor.Disable();
shooterMotor.Disable();
feederMotor.Disable();
})
.AndThen(Run([] {}))
.WithName("Idle"));
@@ -25,15 +25,15 @@ wpi::cmd::CommandPtr Shooter::ShootCommand(
// Run the shooter flywheel at the desired setpoint using
// feedforward and feedback
Run([this, setpoint] {
m_shooterMotor.SetVoltage(
m_shooterFeedforward.Calculate(setpoint) +
wpi::units::volt_t(m_shooterFeedback.Calculate(
m_shooterEncoder.GetRate(), setpoint.value())));
shooterMotor.SetVoltage(
shooterFeedforward.Calculate(setpoint) +
wpi::units::volt_t(shooterFeedback.Calculate(
shooterEncoder.GetRate(), setpoint.value())));
}),
// Wait until the shooter has reached the setpoint, and then
// run the feeder
wpi::cmd::WaitUntil([this] {
return m_shooterFeedback.AtSetpoint();
}).AndThen([this] { m_feederMotor.SetThrottle(1.0); }))
return shooterFeedback.AtSetpoint();
}).AndThen([this] { feederMotor.SetThrottle(1.0); }))
.WithName("Shoot");
}

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

@@ -6,9 +6,9 @@
Storage::Storage() {
SetDefaultCommand(
RunOnce([this] { m_motor.Disable(); }).AndThen([] {}).WithName("Idle"));
RunOnce([this] { motor.Disable(); }).AndThen([] {}).WithName("Idle"));
}
wpi::cmd::CommandPtr Storage::RunCommand() {
return Run([this] { m_motor.SetThrottle(1.0); }).WithName("Run");
return Run([this] { motor.SetThrottle(1.0); }).WithName("Run");
}

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

@@ -41,13 +41,12 @@ class RapidReactCommandBot {
private:
// The robot's subsystems
Drive m_drive;
Intake m_intake;
Shooter m_shooter;
Storage m_storage;
Pneumatics m_pneumatics;
Drive drive;
Intake intake;
Shooter shooter;
Storage storage;
Pneumatics pneumatics;
// The driver's controller
wpi::cmd::CommandGamepad m_driverController{
OIConstants::kDriverControllerPort};
wpi::cmd::CommandGamepad driverController{OIConstants::kDriverControllerPort};
};

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

@@ -24,6 +24,6 @@ class Robot : public wpi::TimedRobot {
void UtilityPeriodic() override;
private:
RapidReactCommandBot m_robot;
std::optional<wpi::cmd::CommandPtr> m_autonomousCommand;
RapidReactCommandBot robot;
std::optional<wpi::cmd::CommandPtr> autonomousCommand;
};

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

@@ -49,29 +49,29 @@ class Drive : public wpi::cmd::SubsystemBase {
wpi::cmd::CommandPtr TurnToAngleCommand(wpi::units::degree_t angle);
private:
wpi::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port};
wpi::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port};
wpi::PWMSparkMax m_rightLeader{DriveConstants::kRightMotor1Port};
wpi::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port};
wpi::PWMSparkMax leftLeader{DriveConstants::kLeftMotor1Port};
wpi::PWMSparkMax leftFollower{DriveConstants::kLeftMotor2Port};
wpi::PWMSparkMax rightLeader{DriveConstants::kRightMotor1Port};
wpi::PWMSparkMax rightFollower{DriveConstants::kRightMotor2Port};
wpi::DifferentialDrive m_drive{
[&](double output) { m_leftLeader.SetThrottle(output); },
[&](double output) { m_rightLeader.SetThrottle(output); }};
wpi::DifferentialDrive drive{
[&](double output) { leftLeader.SetThrottle(output); },
[&](double output) { rightLeader.SetThrottle(output); }};
wpi::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
DriveConstants::kLeftEncoderPorts[1],
DriveConstants::kLeftEncoderReversed};
wpi::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
DriveConstants::kRightEncoderPorts[1],
DriveConstants::kRightEncoderReversed};
wpi::Encoder leftEncoder{DriveConstants::kLeftEncoderPorts[0],
DriveConstants::kLeftEncoderPorts[1],
DriveConstants::kLeftEncoderReversed};
wpi::Encoder rightEncoder{DriveConstants::kRightEncoderPorts[0],
DriveConstants::kRightEncoderPorts[1],
DriveConstants::kRightEncoderReversed};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::FLAT};
wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
wpi::math::ProfiledPIDController<wpi::units::radians> m_controller{
wpi::math::ProfiledPIDController<wpi::units::radians> controller{
DriveConstants::kTurnP,
DriveConstants::kTurnI,
DriveConstants::kTurnD,
{DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::radians> feedforward{
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
};

8
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.hpp
View File

@@ -24,10 +24,10 @@ class Intake : public wpi::cmd::SubsystemBase {
wpi::cmd::CommandPtr RetractCommand();
private:
wpi::PWMSparkMax m_motor{IntakeConstants::kMotorPort};
wpi::PWMSparkMax motor{IntakeConstants::kMotorPort};
// Double solenoid connected to two channels of a PCM with the default CAN ID
wpi::DoubleSolenoid m_piston{0, wpi::PneumaticsModuleType::CTRE_PCM,
IntakeConstants::kSolenoidPorts[0],
IntakeConstants::kSolenoidPorts[1]};
wpi::DoubleSolenoid piston{0, wpi::PneumaticsModuleType::CTRE_PCM,
IntakeConstants::kSolenoidPorts[0],
IntakeConstants::kSolenoidPorts[1]};
};

6
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Pneumatics.hpp
View File

@@ -34,9 +34,9 @@ class Pneumatics : wpi::cmd::SubsystemBase {
// pressure is 250r-25
static constexpr double kScale = 250;
static constexpr double kOffset = -25;
wpi::AnalogPotentiometer m_pressureTransducer{/* the AnalogIn port*/ 2,
kScale, kOffset};
wpi::AnalogPotentiometer pressureTransducer{/* the AnalogIn port*/ 2, kScale,
kOffset};
// Compressor connected to a PH with a default CAN ID
wpi::Compressor m_compressor{0, wpi::PneumaticsModuleType::CTRE_PCM};
wpi::Compressor compressor{0, wpi::PneumaticsModuleType::CTRE_PCM};
};

14
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.hpp
View File

@@ -30,13 +30,13 @@ class Shooter : public wpi::cmd::SubsystemBase {
wpi::cmd::CommandPtr ShootCommand(wpi::units::turns_per_second_t setpoint);
private:
wpi::PWMSparkMax m_shooterMotor{ShooterConstants::kShooterMotorPort};
wpi::PWMSparkMax m_feederMotor{ShooterConstants::kFeederMotorPort};
wpi::PWMSparkMax shooterMotor{ShooterConstants::kShooterMotorPort};
wpi::PWMSparkMax feederMotor{ShooterConstants::kFeederMotorPort};
wpi::Encoder m_shooterEncoder{ShooterConstants::kEncoderPorts[0],
ShooterConstants::kEncoderPorts[1],
ShooterConstants::kEncoderReversed};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_shooterFeedforward{
wpi::Encoder shooterEncoder{ShooterConstants::kEncoderPorts[0],
ShooterConstants::kEncoderPorts[1],
ShooterConstants::kEncoderReversed};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> shooterFeedforward{
ShooterConstants::kS, ShooterConstants::kV};
wpi::math::PIDController m_shooterFeedback{ShooterConstants::kP, 0.0, 0.0};
wpi::math::PIDController shooterFeedback{ShooterConstants::kP, 0.0, 0.0};
};

6
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.hpp
View File

@@ -18,9 +18,9 @@ class Storage : wpi::cmd::SubsystemBase {
wpi::cmd::CommandPtr RunCommand();
/** Whether the ball storage is full. */
wpi::cmd::Trigger HasCargo{[this] { return m_ballSensor.Get(); }};
wpi::cmd::Trigger HasCargo{[this] { return ballSensor.Get(); }};
private:
wpi::PWMSparkMax m_motor{StorageConstants::kMotorPort};
wpi::DigitalInput m_ballSensor{StorageConstants::kBallSensorPort};
wpi::PWMSparkMax motor{StorageConstants::kMotorPort};
wpi::DigitalInput ballSensor{StorageConstants::kBallSensorPort};
};

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

@@ -34,10 +34,10 @@ void Robot::DisabledPeriodic() {}
* RobotContainer} class.
*/
void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
autonomousCommand = container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
if (autonomousCommand != nullptr) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(autonomousCommand);
}
}
@@ -48,9 +48,9 @@ void Robot::TeleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != nullptr) {
m_autonomousCommand->Cancel();
m_autonomousCommand = nullptr;
if (autonomousCommand != nullptr) {
autonomousCommand->Cancel();
autonomousCommand = nullptr;
}
}

16
wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/RobotContainer.cpp
View File

@@ -15,20 +15,20 @@ RobotContainer::RobotContainer() {
void RobotContainer::ConfigureButtonBindings() {
// Also set default commands here
m_drive.SetDefaultCommand(TeleopArcadeDrive(
&m_drive, [this] { return -m_controller.GetRawAxis(1); },
[this] { return -m_controller.GetRawAxis(2); }));
drive.SetDefaultCommand(TeleopArcadeDrive(
&drive, [this] { return -controller.GetRawAxis(1); },
[this] { return -controller.GetRawAxis(2); }));
// Example of how to use the onboard IO
m_onboardButtonA.OnTrue(wpi::cmd::Print("Button A Pressed"))
onboardButtonA.OnTrue(wpi::cmd::Print("Button A Pressed"))
.OnFalse(wpi::cmd::Print("Button A Released"));
// Setup SmartDashboard options.
m_chooser.SetDefaultOption("Auto Routine Distance", &m_autoDistance);
m_chooser.AddOption("Auto Routine Time", &m_autoTime);
wpi::SmartDashboard::PutData("Auto Selector", &m_chooser);
chooser.SetDefaultOption("Auto Routine Distance", &autoDistance);
chooser.AddOption("Auto Routine Time", &autoTime);
wpi::SmartDashboard::PutData("Auto Selector", &chooser);
}
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {
return m_chooser.GetSelected();
return chooser.GetSelected();
}

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

@@ -7,18 +7,18 @@
#include "wpi/units/math.hpp"
void DriveDistance::Initialize() {
m_drive->ArcadeDrive(0, 0);
m_drive->ResetEncoders();
drive->ArcadeDrive(0, 0);
drive->ResetEncoders();
}
void DriveDistance::Execute() {
m_drive->ArcadeDrive(m_velocity, 0);
drive->ArcadeDrive(velocity, 0);
}
void DriveDistance::End(bool interrupted) {
m_drive->ArcadeDrive(0, 0);
drive->ArcadeDrive(0, 0);
}
bool DriveDistance::IsFinished() {
return wpi::units::math::abs(m_drive->GetAverageDistance()) >= m_distance;
return wpi::units::math::abs(drive->GetAverageDistance()) >= distance;
}

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

@@ -5,20 +5,20 @@
#include "commands/DriveTime.hpp"
void DriveTime::Initialize() {
m_timer.Start();
m_drive->ArcadeDrive(0, 0);
timer.Start();
drive->ArcadeDrive(0, 0);
}
void DriveTime::Execute() {
m_drive->ArcadeDrive(m_velocity, 0);
drive->ArcadeDrive(velocity, 0);
}
void DriveTime::End(bool interrupted) {
m_drive->ArcadeDrive(0, 0);
m_timer.Stop();
m_timer.Reset();
drive->ArcadeDrive(0, 0);
timer.Stop();
timer.Reset();
}
bool DriveTime::IsFinished() {
return m_timer.HasElapsed(m_duration);
return timer.HasElapsed(duration);
}

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

@@ -11,12 +11,12 @@
TeleopArcadeDrive::TeleopArcadeDrive(
Drivetrain* subsystem, std::function<double()> xaxisVelocitySupplier,
std::function<double()> zaxisRotateSuppplier)
: m_drive{subsystem},
m_xaxisVelocitySupplier{std::move(xaxisVelocitySupplier)},
m_zaxisRotateSupplier{std::move(zaxisRotateSuppplier)} {
: drive{subsystem},
xaxisVelocitySupplier{std::move(xaxisVelocitySupplier)},
zaxisRotateSupplier{std::move(zaxisRotateSuppplier)} {
AddRequirements(subsystem);
}
void TeleopArcadeDrive::Execute() {
m_drive->ArcadeDrive(m_xaxisVelocitySupplier(), m_zaxisRotateSupplier());
drive->ArcadeDrive(xaxisVelocitySupplier(), zaxisRotateSupplier());
}

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

@@ -10,16 +10,16 @@
void TurnDegrees::Initialize() {
// Set motors to stop, read encoder values for starting point
m_drive->ArcadeDrive(0, 0);
m_drive->ResetEncoders();
drive->ArcadeDrive(0, 0);
drive->ResetEncoders();
}
void TurnDegrees::Execute() {
m_drive->ArcadeDrive(0, m_velocity);
drive->ArcadeDrive(0, velocity);
}
void TurnDegrees::End(bool interrupted) {
m_drive->ArcadeDrive(0, 0);
drive->ArcadeDrive(0, 0);
}
bool TurnDegrees::IsFinished() {
@@ -30,11 +30,11 @@ bool TurnDegrees::IsFinished() {
static auto inchPerDegree = (5.551_in * std::numbers::pi) / 360_deg;
// Compare distance traveled from start to distance based on degree turn.
return GetAverageTurningDistance() >= inchPerDegree * m_angle;
return GetAverageTurningDistance() >= inchPerDegree * angle;
}
wpi::units::meter_t TurnDegrees::GetAverageTurningDistance() {
auto l = wpi::units::math::abs(m_drive->GetLeftDistance());
auto r = wpi::units::math::abs(m_drive->GetRightDistance());
auto l = wpi::units::math::abs(drive->GetLeftDistance());
auto r = wpi::units::math::abs(drive->GetRightDistance());
return (l + r) / 2;
}

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

@@ -5,20 +5,20 @@
#include "commands/TurnTime.hpp"
void TurnTime::Initialize() {
m_timer.Start();
m_drive->ArcadeDrive(0, 0);
timer.Start();
drive->ArcadeDrive(0, 0);
}
void TurnTime::Execute() {
m_drive->ArcadeDrive(0, m_velocity);
drive->ArcadeDrive(0, velocity);
}
void TurnTime::End(bool interrupted) {
m_drive->ArcadeDrive(0, 0);
m_timer.Stop();
m_timer.Reset();
drive->ArcadeDrive(0, 0);
timer.Stop();
timer.Reset();
}
bool TurnTime::IsFinished() {
return m_timer.HasElapsed(m_duration);
return timer.HasElapsed(duration);
}

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

@@ -15,18 +15,18 @@ using namespace DriveConstants;
// The Romi has onboard encoders that are hardcoded
// to use DIO pins 4/5 and 6/7 for the left and right
Drivetrain::Drivetrain() {
wpi::util::SendableRegistry::AddChild(&m_drive, &m_leftMotor);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_rightMotor);
wpi::util::SendableRegistry::AddChild(&drive, &leftMotor);
wpi::util::SendableRegistry::AddChild(&drive, &rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.SetInverted(true);
rightMotor.SetInverted(true);
m_leftEncoder.SetDistancePerPulse(std::numbers::pi * kWheelDiameter.value() /
kCountsPerRevolution);
m_rightEncoder.SetDistancePerPulse(std::numbers::pi * kWheelDiameter.value() /
kCountsPerRevolution);
leftEncoder.SetDistancePerPulse(std::numbers::pi * kWheelDiameter.value() /
kCountsPerRevolution);
rightEncoder.SetDistancePerPulse(std::numbers::pi * kWheelDiameter.value() /
kCountsPerRevolution);
ResetEncoders();
}
@@ -35,28 +35,28 @@ void Drivetrain::Periodic() {
}
void Drivetrain::ArcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_drive.ArcadeDrive(xaxisVelocity, zaxisRotate);
drive.ArcadeDrive(xaxisVelocity, zaxisRotate);
}
void Drivetrain::ResetEncoders() {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
leftEncoder.Reset();
rightEncoder.Reset();
}
int Drivetrain::GetLeftEncoderCount() {
return m_leftEncoder.Get();
return leftEncoder.Get();
}
int Drivetrain::GetRightEncoderCount() {
return m_rightEncoder.Get();
return rightEncoder.Get();
}
wpi::units::meter_t Drivetrain::GetLeftDistance() {
return wpi::units::meter_t{m_leftEncoder.GetDistance()};
return wpi::units::meter_t{leftEncoder.GetDistance()};
}
wpi::units::meter_t Drivetrain::GetRightDistance() {
return wpi::units::meter_t{m_rightEncoder.GetDistance()};
return wpi::units::meter_t{rightEncoder.GetDistance()};
}
wpi::units::meter_t Drivetrain::GetAverageDistance() {
@@ -64,17 +64,17 @@ wpi::units::meter_t Drivetrain::GetAverageDistance() {
}
wpi::units::radian_t Drivetrain::GetGyroAngleX() {
return m_gyro.GetAngleX();
return gyro.GetAngleX();
}
wpi::units::radian_t Drivetrain::GetGyroAngleY() {
return m_gyro.GetAngleY();
return gyro.GetAngleY();
}
wpi::units::radian_t Drivetrain::GetGyroAngleZ() {
return m_gyro.GetAngleZ();
return gyro.GetAngleZ();
}
void Drivetrain::ResetGyro() {
m_gyro.Reset();
gyro.Reset();
}

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

@@ -23,6 +23,6 @@ class Robot : public wpi::TimedRobot {
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
wpi::cmd::Command* m_autonomousCommand = nullptr;
RobotContainer m_container;
wpi::cmd::Command* autonomousCommand = nullptr;
RobotContainer container;
};

18
wpilibcExamples/src/main/cpp/examples/RomiReference/include/RobotContainer.hpp
View File

@@ -41,21 +41,21 @@ class RobotContainer {
private:
// Assumes a gamepad plugged into channel 0
wpi::Joystick m_controller{0};
wpi::SendableChooser<wpi::cmd::Command*> m_chooser;
wpi::Joystick controller{0};
wpi::SendableChooser<wpi::cmd::Command*> chooser;
// The robot's subsystems
Drivetrain m_drive;
wpi::romi::OnBoardIO m_onboardIO{wpi::romi::OnBoardIO::ChannelMode::INPUT,
wpi::romi::OnBoardIO::ChannelMode::INPUT};
Drivetrain drive;
wpi::romi::OnBoardIO onboardIO{wpi::romi::OnBoardIO::ChannelMode::INPUT,
wpi::romi::OnBoardIO::ChannelMode::INPUT};
// Example button
wpi::cmd::Trigger m_onboardButtonA{
[this] { return m_onboardIO.GetButtonAPressed(); }};
wpi::cmd::Trigger onboardButtonA{
[this] { return onboardIO.GetButtonAPressed(); }};
// Autonomous commands.
AutonomousDistance m_autoDistance{&m_drive};
AutonomousTime m_autoTime{&m_drive};
AutonomousDistance autoDistance{&drive};
AutonomousTime autoTime{&drive};
void ConfigureButtonBindings();
};

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

@@ -22,8 +22,8 @@ class DriveDistance
*/
DriveDistance(double velocity, wpi::units::meter_t distance,
Drivetrain* drive)
: m_velocity(velocity), m_distance(distance), m_drive(drive) {
AddRequirements(m_drive);
: velocity(velocity), distance(distance), drive(drive) {
AddRequirements(drive);
}
void Initialize() override;
@@ -32,7 +32,7 @@ class DriveDistance
bool IsFinished() override;
private:
double m_velocity;
wpi::units::meter_t m_distance;
Drivetrain* m_drive;
double velocity;
wpi::units::meter_t distance;
Drivetrain* drive;
};

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

@@ -22,8 +22,8 @@ class DriveTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveTime> {
* @param drive The drivetrain subsystem on which this command will run
*/
DriveTime(double velocity, wpi::units::second_t time, Drivetrain* drive)
: m_velocity(velocity), m_duration(time), m_drive(drive) {
AddRequirements(m_drive);
: velocity(velocity), duration(time), drive(drive) {
AddRequirements(drive);
}
void Initialize() override;
@@ -32,8 +32,8 @@ class DriveTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveTime> {
bool IsFinished() override;
private:
double m_velocity;
wpi::units::second_t m_duration;
Drivetrain* m_drive;
wpi::Timer m_timer;
double velocity;
wpi::units::second_t duration;
Drivetrain* drive;
wpi::Timer timer;
};

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

@@ -27,7 +27,7 @@ class TeleopArcadeDrive
void Execute() override;
private:
Drivetrain* m_drive;
std::function<double()> m_xaxisVelocitySupplier;
std::function<double()> m_zaxisRotateSupplier;
Drivetrain* drive;
std::function<double()> xaxisVelocitySupplier;
std::function<double()> zaxisRotateSupplier;
};

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

@@ -23,8 +23,8 @@ class TurnDegrees
* @param drive The drive subsystem on which this command will run
*/
TurnDegrees(double velocity, wpi::units::degree_t angle, Drivetrain* drive)
: m_velocity(velocity), m_angle(angle), m_drive(drive) {
AddRequirements(m_drive);
: velocity(velocity), angle(angle), drive(drive) {
AddRequirements(drive);
}
void Initialize() override;
@@ -33,9 +33,9 @@ class TurnDegrees
bool IsFinished() override;
private:
double m_velocity;
wpi::units::degree_t m_angle;
Drivetrain* m_drive;
double velocity;
wpi::units::degree_t angle;
Drivetrain* drive;
wpi::units::meter_t GetAverageTurningDistance();
};

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

@@ -21,8 +21,8 @@ class TurnTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnTime> {
* @param drive The drive subsystem on which this command will run
*/
TurnTime(double velocity, wpi::units::second_t time, Drivetrain* drive)
: m_velocity(velocity), m_duration(time), m_drive(drive) {
AddRequirements(m_drive);
: velocity(velocity), duration(time), drive(drive) {
AddRequirements(drive);
}
void Initialize() override;
@@ -31,8 +31,8 @@ class TurnTime : public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnTime> {
bool IsFinished() override;
private:
double m_velocity;
wpi::units::second_t m_duration;
Drivetrain* m_drive;
wpi::Timer m_timer;
double velocity;
wpi::units::second_t duration;
Drivetrain* drive;
wpi::Timer timer;
};

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

@@ -99,15 +99,15 @@ class Drivetrain : public wpi::cmd::SubsystemBase {
void ResetGyro();
private:
wpi::Spark m_leftMotor{0};
wpi::Spark m_rightMotor{1};
wpi::Spark leftMotor{0};
wpi::Spark rightMotor{1};
wpi::Encoder m_leftEncoder{4, 5};
wpi::Encoder m_rightEncoder{6, 7};
wpi::Encoder leftEncoder{4, 5};
wpi::Encoder rightEncoder{6, 7};
wpi::DifferentialDrive m_drive{
[&](double output) { m_leftMotor.SetThrottle(output); },
[&](double output) { m_rightMotor.SetThrottle(output); }};
wpi::DifferentialDrive drive{
[&](double output) { leftMotor.SetThrottle(output); },
[&](double output) { rightMotor.SetThrottle(output); }};
wpi::romi::RomiGyro m_gyro;
wpi::romi::RomiGyro gyro;
};

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

@@ -8,33 +8,33 @@
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(),
velocities.left.value());
double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
velocities.right.value());
auto leftFeedforward = feedforward.Calculate(velocities.left);
auto rightFeedforward = feedforward.Calculate(velocities.right);
double leftOutput = leftPIDController.Calculate(leftEncoder.GetRate(),
velocities.left.value());
double rightOutput = rightPIDController.Calculate(rightEncoder.GetRate(),
velocities.right.value());
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
wpi::units::radians_per_second_t rot) {
SetVelocities(m_kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
SetVelocities(kinematics.ToWheelVelocities({xVelocity, 0_mps, rot}));
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_imu.GetRotation2d(),
wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()});
odometry.Update(imu.GetRotation2d(),
wpi::units::meter_t{leftEncoder.GetDistance()},
wpi::units::meter_t{rightEncoder.GetDistance()});
}
void Drivetrain::ResetOdometry(const wpi::math::Pose2d& pose) {
m_drivetrainSimulator.SetPose(pose);
m_odometry.ResetPosition(
m_imu.GetRotation2d(), wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()}, pose);
drivetrainSimulator.SetPose(pose);
odometry.ResetPosition(imu.GetRotation2d(),
wpi::units::meter_t{leftEncoder.GetDistance()},
wpi::units::meter_t{rightEncoder.GetDistance()}, pose);
}
void Drivetrain::SimulationPeriodic() {
@@ -42,22 +42,20 @@ 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.GetThrottle()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{m_rightLeader.GetThrottle()} *
wpi::RobotController::GetInputVoltage());
m_drivetrainSimulator.Update(20_ms);
drivetrainSimulator.SetInputs(wpi::units::volt_t{leftLeader.GetThrottle()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{rightLeader.GetThrottle()} *
wpi::RobotController::GetInputVoltage());
drivetrainSimulator.Update(20_ms);
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
m_rightEncoderSim.SetDistance(
m_drivetrainSimulator.GetRightPosition().value());
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
// m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
leftEncoderSim.SetDistance(drivetrainSimulator.GetLeftPosition().value());
leftEncoderSim.SetRate(drivetrainSimulator.GetLeftVelocity().value());
rightEncoderSim.SetDistance(drivetrainSimulator.GetRightPosition().value());
rightEncoderSim.SetRate(drivetrainSimulator.GetRightVelocity().value());
// gyroSim.SetAngle(-drivetrainSimulator.GetHeading().Degrees().value());
}
void Drivetrain::Periodic() {
UpdateOdometry();
m_fieldSim.SetRobotPose(m_odometry.GetPose());
fieldSim.SetRobotPose(odometry.GetPose());
}

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

@@ -13,57 +13,56 @@
class Robot : public wpi::TimedRobot {
public:
Robot() {
m_trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
wpi::math::Pose2d{2_m, 2_m, 0_rad}, {},
wpi::math::Pose2d{6_m, 4_m, 0_rad},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
}
void RobotPeriodic() override { m_drive.Periodic(); }
void RobotPeriodic() override { drive.Periodic(); }
void AutonomousInit() override {
m_timer.Restart();
m_drive.ResetOdometry(m_trajectory.InitialPose());
timer.Restart();
drive.ResetOdometry(trajectory.InitialPose());
}
void AutonomousPeriodic() override {
auto elapsed = m_timer.Get();
auto reference = m_trajectory.Sample(elapsed);
auto velocities = m_feedback.Calculate(m_drive.GetPose(), reference);
m_drive.Drive(velocities.vx, velocities.omega);
auto elapsed = timer.Get();
auto reference = trajectory.Sample(elapsed);
auto velocities = feedback.Calculate(drive.GetPose(), reference);
drive.Drive(velocities.vx, velocities.omega);
}
void TeleopPeriodic() override {
// Get the x velocity. We are inverting this because Xbox controllers return
// negative values when we push forward.
const auto xVelocity =
-m_velocityLimiter.Calculate(m_controller.GetLeftY()) *
Drivetrain::kMaxVelocity;
const auto xVelocity = -velocityLimiter.Calculate(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()) *
auto rot = -rotLimiter.Calculate(controller.GetRightX()) *
Drivetrain::kMaxAngularVelocity;
m_drive.Drive(xVelocity, rot);
drive.Drive(xVelocity, rot);
}
void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
void SimulationPeriodic() override { drive.SimulationPeriodic(); }
private:
wpi::Gamepad m_controller{0};
wpi::Gamepad controller{0};
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
wpi::math::SlewRateLimiter<wpi::units::scalar> m_velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> velocityLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> rotLimiter{3 / 1_s};
Drivetrain m_drive;
wpi::math::Trajectory m_trajectory;
wpi::math::LTVUnicycleController m_feedback{20_ms};
wpi::Timer m_timer;
Drivetrain drive;
wpi::math::Trajectory trajectory;
wpi::math::LTVUnicycleController feedback{20_ms};
wpi::Timer timer;
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -28,30 +28,30 @@
class Drivetrain {
public:
Drivetrain() {
m_imu.ResetYaw();
imu.ResetYaw();
m_leftLeader.AddFollower(m_leftFollower);
m_rightLeader.AddFollower(m_rightFollower);
leftLeader.AddFollower(leftFollower);
rightLeader.AddFollower(rightFollower);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.SetInverted(true);
rightLeader.SetInverted(true);
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
m_leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
kEncoderResolution);
m_leftEncoder.Reset();
m_rightEncoder.Reset();
leftEncoder.Reset();
rightEncoder.Reset();
m_rightLeader.SetInverted(true);
rightLeader.SetInverted(true);
wpi::SmartDashboard::PutData("Field", &m_fieldSim);
wpi::SmartDashboard::PutData("Field", &fieldSim);
}
static constexpr wpi::units::meters_per_second_t kMaxVelocity =
@@ -66,7 +66,7 @@ class Drivetrain {
void UpdateOdometry();
void ResetOdometry(const wpi::math::Pose2d& pose);
wpi::math::Pose2d GetPose() const { return m_odometry.GetPose(); }
wpi::math::Pose2d GetPose() const { return odometry.GetPose(); }
void SimulationPeriodic();
void Periodic();
@@ -76,36 +76,36 @@ class Drivetrain {
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
wpi::PWMSparkMax m_leftLeader{1};
wpi::PWMSparkMax m_leftFollower{2};
wpi::PWMSparkMax m_rightLeader{3};
wpi::PWMSparkMax m_rightFollower{4};
wpi::PWMSparkMax leftLeader{1};
wpi::PWMSparkMax leftFollower{2};
wpi::PWMSparkMax rightLeader{3};
wpi::PWMSparkMax rightFollower{4};
wpi::Encoder m_leftEncoder{0, 1};
wpi::Encoder m_rightEncoder{2, 3};
wpi::Encoder leftEncoder{0, 1};
wpi::Encoder rightEncoder{2, 3};
wpi::math::PIDController m_leftPIDController{8.5, 0.0, 0.0};
wpi::math::PIDController m_rightPIDController{8.5, 0.0, 0.0};
wpi::math::PIDController leftPIDController{8.5, 0.0, 0.0};
wpi::math::PIDController rightPIDController{8.5, 0.0, 0.0};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::FLAT};
wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
wpi::math::DifferentialDriveKinematics m_kinematics{kTrackwidth};
wpi::math::DifferentialDriveOdometry m_odometry{
m_imu.GetRotation2d(), wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()}};
wpi::math::DifferentialDriveKinematics kinematics{kTrackwidth};
wpi::math::DifferentialDriveOdometry odometry{
imu.GetRotation2d(), wpi::units::meter_t{leftEncoder.GetDistance()},
wpi::units::meter_t{rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward{
1_V, 3_V / 1_mps};
// Simulation classes help us simulate our robot
wpi::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
wpi::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
wpi::Field2d m_fieldSim;
wpi::math::LinearSystem<2, 2, 2> m_drivetrainSystem =
wpi::sim::EncoderSim leftEncoderSim{leftEncoder};
wpi::sim::EncoderSim rightEncoderSim{rightEncoder};
wpi::Field2d fieldSim;
wpi::math::LinearSystem<2, 2, 2> drivetrainSystem =
wpi::math::Models::DifferentialDriveFromSysId(
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
wpi::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
m_drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in};
wpi::sim::DifferentialDrivetrainSim drivetrainSimulator{
drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in};
};

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