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

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp
View File

@@ -118,14 +118,17 @@ class Robot : public wpi::TimedRobot {
// draw lines around the tag
for (int i = 0; i <= 3; i++) {
int j = (i + 1) % 4;
const wpi::apriltag::AprilTagDetection::Point pti = detection->GetCorner(i);
const wpi::apriltag::AprilTagDetection::Point ptj = detection->GetCorner(j);
const wpi::apriltag::AprilTagDetection::Point pti =
detection->GetCorner(i);
const wpi::apriltag::AprilTagDetection::Point ptj =
detection->GetCorner(j);
line(mat, cv::Point(pti.x, pti.y), cv::Point(ptj.x, ptj.y),
outlineColor, 2);
}
// mark the center of the tag
const wpi::apriltag::AprilTagDetection::Point c = detection->GetCenter();
const wpi::apriltag::AprilTagDetection::Point c =
detection->GetCenter();
int ll = 10;
line(mat, cv::Point(c.x - ll, c.y), cv::Point(c.x + ll, c.y),
crossColor, 2);

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

@@ -24,8 +24,8 @@ Arm::Arm() {
void Arm::SimulationPeriodic() {
// In this method, we update our simulation of what our arm is doing
// First, we set our "inputs" (voltages)
m_armSim.SetInput(
wpi::math::Vectord<1>{m_motor.Get() * wpi::RobotController::GetInputVoltage()});
m_armSim.SetInput(wpi::math::Vectord<1>{
m_motor.Get() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_armSim.Update(20_ms);

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

@@ -4,7 +4,8 @@
#include "Drivetrain.hpp"
void Drivetrain::SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
void Drivetrain::SetSpeeds(
const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
const double leftOutput = m_leftPIDController.Calculate(

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

@@ -80,5 +80,6 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
1_V, 3_V / 1_mps};
};

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

@@ -32,7 +32,8 @@ Drivetrain::Drivetrain() {
wpi::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
}
void Drivetrain::SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
void Drivetrain::SetSpeeds(
const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
const double leftOutput = m_leftPIDController.Calculate(
@@ -75,12 +76,15 @@ wpi::math::Pose3d Drivetrain::ObjectToRobotPose(
std::vector<double> val{cameraToObjectEntry.Get()};
// Reconstruct cameraToObject Transform3D from networktables.
wpi::math::Translation3d translation{wpi::units::meter_t{val[0]}, wpi::units::meter_t{val[1]},
wpi::units::meter_t{val[2]}};
wpi::math::Rotation3d rotation{wpi::math::Quaternion{val[3], val[4], val[5], val[6]}};
wpi::math::Translation3d translation{wpi::units::meter_t{val[0]},
wpi::units::meter_t{val[1]},
wpi::units::meter_t{val[2]}};
wpi::math::Rotation3d rotation{
wpi::math::Quaternion{val[3], val[4], val[5], val[6]}};
wpi::math::Transform3d cameraToObject{translation, rotation};
return wpi::math::ObjectToRobotPose(objectInField, cameraToObject, robotToCamera);
return wpi::math::ObjectToRobotPose(objectInField, cameraToObject,
robotToCamera);
}
void Drivetrain::UpdateOdometry() {
@@ -98,8 +102,8 @@ void Drivetrain::UpdateOdometry() {
wpi::math::Pose3d visionMeasurement3d = ObjectToRobotPose(
m_objectInField, m_robotToCamera, m_cameraToObjectEntryRef);
// Convert robot's pose from wpi::math::Pose3d to wpi::math::Pose2d needed to apply vision
// measurements.
// Convert robot's pose from wpi::math::Pose3d to wpi::math::Pose2d needed to
// apply vision measurements.
wpi::math::Pose2d visionMeasurement2d = visionMeasurement3d.ToPose2d();
// Apply vision measurements. For simulation purposes only, we don't input a

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

@@ -106,9 +106,9 @@ class Drivetrain {
* @param cameraToObjectEntry The networktables entry publishing and querying
* example computer vision measurements.
*/
wpi::math::Pose3d ObjectToRobotPose(wpi::math::Pose3d objectInField,
wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry);
wpi::math::Pose3d ObjectToRobotPose(
wpi::math::Pose3d objectInField, wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry);
private:
static constexpr wpi::units::meter_t kTrackwidth = 0.381_m * 2;
@@ -120,9 +120,11 @@ class Drivetrain {
wpi::math::Transform3d m_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::math::Rotation3d{0_rad, 0_rad,
wpi::units::radian_t{std::numbers::pi / 2}}};
wpi::nt::NetworkTableInstance m_inst{wpi::nt::NetworkTableInstance::GetDefault()};
wpi::nt::NetworkTableInstance m_inst{
wpi::nt::NetworkTableInstance::GetDefault()};
wpi::nt::DoubleArrayTopic m_cameraToObjectTopic{
m_inst.GetDoubleArrayTopic("m_cameraToObjectTopic")};
wpi::nt::DoubleArrayEntry m_cameraToObjectEntry =
@@ -130,8 +132,10 @@ class Drivetrain {
wpi::nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
wpi::apriltag::AprilTagFieldLayout m_aprilTagFieldLayout{
wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo)};
wpi::math::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo)};
wpi::math::Pose3d m_objectInField{
m_aprilTagFieldLayout.GetTagPose(0).value()};
wpi::PWMSparkMax m_leftLeader{1};
wpi::PWMSparkMax m_leftFollower{2};
@@ -161,7 +165,8 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
1_V, 3_V / 1_mps};
// Simulation classes
wpi::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};

9
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp
View File

@@ -16,9 +16,10 @@ class ExampleGlobalMeasurementSensor {
static wpi::math::Pose2d GetEstimatedGlobalPose(
const wpi::math::Pose2d& estimatedRobotPose) {
auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return wpi::math::Pose2d{estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
return wpi::math::Pose2d{
estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
}
};

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

@@ -38,7 +38,8 @@ void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
wpi::cmd::CommandScheduler::GetInstance().Schedule(
m_autonomousCommand.value());
}
}

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

@@ -35,10 +35,11 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
* @param nextLeft The next left wheel state.
* @param nextRight The next right wheel state.
*/
void SetDriveStates(wpi::math::TrapezoidProfile<wpi::units::meters>::State currentLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State currentRight,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextRight);
void SetDriveStates(
wpi::math::TrapezoidProfile<wpi::units::meters>::State currentLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State currentRight,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextRight);
/**
* Drives the robot using arcade controls.
@@ -91,7 +92,8 @@ class DriveSubsystem : public wpi::cmd::SubsystemBase {
* @param distance The distance to drive forward.
* @return A command.
*/
wpi::cmd::CommandPtr DynamicProfiledDriveDistance(wpi::units::meter_t distance);
wpi::cmd::CommandPtr DynamicProfiledDriveDistance(
wpi::units::meter_t distance);
private:
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{

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

@@ -57,7 +57,7 @@ class Robot : public wpi::TimedRobot {
// to be changed.
double percentOfRange = fullRange * 0.1;
double shiftedOutput = wpi::math::InputModulus(output, 0 - percentOfRange,
fullRange - percentOfRange);
fullRange - percentOfRange);
wpi::SmartDashboard::PutBoolean("Connected", connected);
wpi::SmartDashboard::PutNumber("Frequency", frequency.value());

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

@@ -54,10 +54,12 @@ class Robot : public wpi::TimedRobot {
// 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>::State m_goal;
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State m_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>::State
m_goal;
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State
m_setpoint;
};
#ifndef RUNNING_WPILIB_TESTS

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

@@ -40,8 +40,8 @@ void Elevator::UpdateTelemetry() {
}
void Elevator::ReachGoal(wpi::units::meter_t goal) {
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State goalState{goal,
0_mps};
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State
goalState{goal, 0_mps};
auto next = m_profile.Calculate(20_ms, m_setpoint, goalState);

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

@@ -35,11 +35,13 @@ class Elevator {
wpi::math::DCMotor m_elevatorGearbox = wpi::math::DCMotor::NEO(2);
// Standard classes for controlling our elevator
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::Constraints
m_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>::State m_setpoint;
wpi::math::ExponentialProfile<wpi::units::meters,
wpi::units::volts>::Constraints m_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>::State
m_setpoint;
wpi::math::PIDController m_controller{
Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd};

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

@@ -34,8 +34,8 @@ class Robot : public wpi::TimedRobot {
// Run controller and update motor output
m_motor.SetVoltage(
wpi::units::volt_t{
m_controller.Calculate(wpi::units::meter_t{m_encoder.GetDistance()})} +
wpi::units::volt_t{m_controller.Calculate(
wpi::units::meter_t{m_encoder.GetDistance()})} +
m_feedforward.Calculate(m_controller.GetSetpoint().velocity));
}
@@ -58,8 +58,8 @@ class Robot : public wpi::TimedRobot {
// velocity and acceleration constraints.
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints m_constraints{
kMaxVelocity, kMaxAcceleration};
wpi::math::ProfiledPIDController<wpi::units::meters> m_controller{kP, kI, kD,
m_constraints, kDt};
wpi::math::ProfiledPIDController<wpi::units::meters> m_controller{
kP, kI, kD, m_constraints, kDt};
wpi::math::ElevatorFeedforward m_feedforward{kS, kG, kV};
};

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

@@ -34,8 +34,8 @@ class Elevator {
wpi::math::DCMotor m_elevatorGearbox = wpi::math::DCMotor::Vex775Pro(4);
// Standard classes for controlling our elevator
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints m_constraints{2.45_mps,
2.45_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints m_constraints{
2.45_mps, 2.45_mps_sq};
wpi::math::ProfiledPIDController<wpi::units::meters> m_controller{
Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd,
m_constraints};

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

@@ -51,7 +51,8 @@ class Robot : public wpi::TimedRobot {
// 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{{1.75_mps, 0.75_mps_sq}};
wpi::math::TrapezoidProfile<wpi::units::meters> m_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;
};

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

@@ -55,7 +55,7 @@ class Robot : public wpi::TimedRobot {
&encoder = m_shooterEncoder] {
shooter.SetVoltage(
wpi::units::volt_t{controller.Calculate(encoder.GetRate(),
SHOT_VELOCITY.value())} +
SHOT_VELOCITY.value())} +
ff.Calculate(wpi::units::radians_per_second_t{SHOT_VELOCITY}));
});
// if not, stop
@@ -84,7 +84,8 @@ class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_shooter{0};
wpi::Encoder m_shooterEncoder{0, 1};
wpi::math::PIDController m_controller{0.3, 0, 0};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_ff{0.1_V, 0.065_V / 1_rpm};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_ff{0.1_V,
0.065_V / 1_rpm};
wpi::PWMSparkMax m_kicker{1};

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

@@ -15,8 +15,8 @@
#include <wpi/units/moment_of_inertia.hpp>
/**
* This is a sample program to demonstrate the use of a wpi::math::BangBangController with
* a flywheel to control speed.
* This is a sample program to demonstrate the use of a
* wpi::math::BangBangController with a flywheel to control speed.
*/
class Robot : public wpi::TimedRobot {
public:
@@ -25,8 +25,8 @@ class Robot : public wpi::TimedRobot {
*/
void TeleopPeriodic() override {
// Scale setpoint value between 0 and maxSetpointValue
wpi::units::radians_per_second_t setpoint =
wpi::units::math::max(0_rpm, m_joystick.GetRawAxis(0) * kMaxSetpointValue);
wpi::units::radians_per_second_t setpoint = wpi::units::math::max(
0_rpm, m_joystick.GetRawAxis(0) * kMaxSetpointValue);
// Set setpoint and measurement of the bang-bang controller
wpi::units::volt_t bangOutput =
@@ -64,7 +64,8 @@ class Robot : public wpi::TimedRobot {
static constexpr int kEncoderBChannel = 1;
// Max setpoint for joystick control
static constexpr wpi::units::radians_per_second_t kMaxSetpointValue = 6000_rpm;
static constexpr wpi::units::radians_per_second_t kMaxSetpointValue =
6000_rpm;
// Joystick to control setpoint
wpi::Joystick m_joystick{0};
@@ -90,12 +91,13 @@ class Robot : public wpi::TimedRobot {
static constexpr double kFlywheelGearing = 1.0;
// 1/2 MR²
static constexpr wpi::units::kilogram_square_meter_t kFlywheelMomentOfInertia =
0.5 * 1.5_lb * 4_in * 4_in;
static constexpr wpi::units::kilogram_square_meter_t
kFlywheelMomentOfInertia = 0.5 * 1.5_lb * 4_in * 4_in;
wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::NEO(1);
wpi::math::LinearSystem<1, 1, 1> m_plant{wpi::math::LinearSystemId::FlywheelSystem(
m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)};
wpi::math::LinearSystem<1, 1, 1> m_plant{
wpi::math::LinearSystemId::FlywheelSystem(
m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)};
wpi::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox};
wpi::sim::EncoderSim m_encoderSim{m_encoder};

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

@@ -42,7 +42,8 @@ void RobotContainer::ConfigureButtonBindings() {
// While holding R1, drive at half speed
m_driverController.R1()
.OnTrue(wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}))
.OnFalse(wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); }, {}));
.OnFalse(
wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); }, {}));
}
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {

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

@@ -31,7 +31,7 @@ wpi::cmd::CommandPtr autos::SimpleAuto(DriveSubsystem* drive) {
}
wpi::cmd::CommandPtr autos::ComplexAuto(DriveSubsystem* drive,
HatchSubsystem* hatch) {
HatchSubsystem* hatch) {
return wpi::cmd::cmd::Sequence(
// Drive forward the specified distance
wpi::cmd::FunctionalCommand(

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

@@ -48,7 +48,7 @@ void RobotContainer::ConfigureButtonBindings() {
.OnTrue(ReleaseHatch(&m_hatch).ToPtr());
// While holding the shoulder button, drive at half speed
wpi::cmd::JoystickButton(&m_driverController,
wpi::XboxController::Button::kRightBumper)
wpi::XboxController::Button::kRightBumper)
.WhileTrue(HalveDriveSpeed(&m_drive).ToPtr());
}

3
wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.hpp
View File

@@ -16,7 +16,8 @@
* backward.
*/
class ComplexAuto
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup, ComplexAuto> {
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup,
ComplexAuto> {
public:
/**
* Creates a new ComplexAuto.

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

@@ -18,7 +18,8 @@
*
* @see RunCommand
*/
class DefaultDrive : public wpi::cmd::CommandHelper<wpi::cmd::Command, DefaultDrive> {
class DefaultDrive
: public wpi::cmd::CommandHelper<wpi::cmd::Command, DefaultDrive> {
public:
/**
* Creates a new DefaultDrive.

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

@@ -9,7 +9,8 @@
#include "subsystems/DriveSubsystem.hpp"
class DriveDistance : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveDistance> {
class DriveDistance
: public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveDistance> {
public:
/**
* Creates a new DriveDistance.

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

@@ -16,7 +16,8 @@
*
* @see InstantCommand
*/
class ReleaseHatch : public wpi::cmd::CommandHelper<wpi::cmd::Command, ReleaseHatch> {
class ReleaseHatch
: public wpi::cmd::CommandHelper<wpi::cmd::Command, ReleaseHatch> {
public:
explicit ReleaseHatch(HatchSubsystem* subsystem);

3
wpilibcExamples/src/main/cpp/examples/HttpCamera/cpp/Robot.cpp
View File

@@ -27,7 +27,8 @@ class Robot : public wpi::TimedRobot {
static void VisionThread() {
// Create an HTTP camera. The address will need to be modified to have the
// correct team number. The exact path will depend on the source.
wpi::cs::HttpCamera camera{"My Camera", "http://10.x.y.11/video/stream.mjpg"};
wpi::cs::HttpCamera camera{"My Camera",
"http://10.x.y.11/video/stream.mjpg"};
// Start capturing images
wpi::CameraServer::StartAutomaticCapture(camera);
// Set the resolution

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

@@ -13,14 +13,16 @@ wpi::math::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
wpi::units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances() const {
wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances()
const {
return {wpi::units::meter_t{m_frontLeftEncoder.GetDistance()},
wpi::units::meter_t{m_frontRightEncoder.GetDistance()},
wpi::units::meter_t{m_backLeftEncoder.GetDistance()},
wpi::units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
void Drivetrain::SetSpeeds(
const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
const auto frontLeftFeedforward =
m_feedforward.Calculate(wheelSpeeds.frontLeft);
const auto frontRightFeedforward =

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

@@ -34,8 +34,9 @@ class Drivetrain {
wpi::math::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
void SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed, wpi::units::radians_per_second_t rot,
bool fieldRelative, wpi::units::second_t period);
wpi::units::meters_per_second_t ySpeed,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
@@ -70,10 +71,11 @@ class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
wpi::math::MecanumDriveOdometry m_odometry{m_kinematics, m_imu.GetRotation2d(),
GetCurrentWheelDistances()};
wpi::math::MecanumDriveOdometry m_odometry{
m_kinematics, m_imu.GetRotation2d(), GetCurrentWheelDistances()};
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
1_V, 3_V / 1_mps};
};

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

@@ -22,7 +22,8 @@ wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentDistances() const {
wpi::units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
void Drivetrain::SetSpeeds(
const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
std::function<void(wpi::units::meters_per_second_t, const wpi::Encoder&,
wpi::math::PIDController&, wpi::PWMSparkMax&)>
calcAndSetSpeeds = [&m_feedforward = m_feedforward](

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

@@ -35,8 +35,9 @@ class Drivetrain {
wpi::math::MecanumDriveWheelPositions GetCurrentDistances() const;
void SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed, wpi::units::radians_per_second_t rot,
bool fieldRelative, wpi::units::second_t period);
wpi::units::meters_per_second_t ySpeed,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second
@@ -72,11 +73,12 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_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(), GetCurrentDistances(),
m_kinematics, m_imu.GetRotation2d(), GetCurrentDistances(),
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
};

9
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp
View File

@@ -16,9 +16,10 @@ class ExampleGlobalMeasurementSensor {
static wpi::math::Pose2d GetEstimatedGlobalPose(
const wpi::math::Pose2d& estimatedRobotPose) {
auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return wpi::math::Pose2d{estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
return wpi::math::Pose2d{
estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
}
};

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

@@ -26,7 +26,8 @@ void Robot::AutonomousInit() {
m_autonomousCommand = m_robot.GetAutonomousCommand();
if (m_autonomousCommand) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
wpi::cmd::CommandScheduler::GetInstance().Schedule(
m_autonomousCommand.value());
}
}

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

@@ -35,7 +35,7 @@ Drive::Drive() {
}
wpi::cmd::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
std::function<double()> rot) {
std::function<double()> rot) {
return Run([this, fwd = std::move(fwd), rot = std::move(rot)] {
m_drive.ArcadeDrive(fwd(), rot());
})
@@ -43,7 +43,7 @@ wpi::cmd::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
}
wpi::cmd::CommandPtr Drive::DriveDistanceCommand(wpi::units::meter_t distance,
double speed) {
double speed) {
return RunOnce([this] {
// Reset encoders at the start of the command
m_leftEncoder.Reset();
@@ -52,8 +52,9 @@ wpi::cmd::CommandPtr Drive::DriveDistanceCommand(wpi::units::meter_t distance,
// Drive forward at specified speed
.AndThen(Run([this, speed] { m_drive.ArcadeDrive(speed, 0.0); }))
.Until([this, distance] {
return wpi::units::math::max(wpi::units::meter_t(m_leftEncoder.GetDistance()),
wpi::units::meter_t(m_rightEncoder.GetDistance())) >=
return wpi::units::math::max(
wpi::units::meter_t(m_leftEncoder.GetDistance()),
wpi::units::meter_t(m_rightEncoder.GetDistance())) >=
distance;
})
// Stop the drive when the command ends

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

@@ -19,7 +19,8 @@ Shooter::Shooter() {
.WithName("Idle"));
}
wpi::cmd::CommandPtr Shooter::ShootCommand(wpi::units::turns_per_second_t setpoint) {
wpi::cmd::CommandPtr Shooter::ShootCommand(
wpi::units::turns_per_second_t setpoint) {
return wpi::cmd::cmd::Parallel(
// Run the shooter flywheel at the desired setpoint using
// feedforward and feedback

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

@@ -29,7 +29,7 @@ class Drive : public wpi::cmd::SubsystemBase {
* @param rot the commanded rotation
*/
wpi::cmd::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
std::function<double()> rot);
std::function<double()> rot);
/**
* Returns a command that drives the robot forward a specified distance at a
@@ -38,7 +38,8 @@ class Drive : public wpi::cmd::SubsystemBase {
* @param distance The distance to drive forward in meters
* @param speed The fraction of max speed at which to drive
*/
wpi::cmd::CommandPtr DriveDistanceCommand(wpi::units::meter_t distance, double speed);
wpi::cmd::CommandPtr DriveDistanceCommand(wpi::units::meter_t distance,
double speed);
/**
* Returns a command that turns to robot to the specified angle using a motion

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

@@ -48,7 +48,7 @@ class RobotContainer {
// The robot's subsystems
Drivetrain m_drive;
wpi::romi::OnBoardIO m_onboardIO{wpi::romi::OnBoardIO::ChannelMode::INPUT,
wpi::romi::OnBoardIO::ChannelMode::INPUT};
wpi::romi::OnBoardIO::ChannelMode::INPUT};
// Example button
wpi::cmd::Trigger m_onboardButtonA{

2
wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousDistance.hpp
View File

@@ -13,7 +13,7 @@
class AutonomousDistance
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup,
AutonomousDistance> {
AutonomousDistance> {
public:
/**
* Creates a new Autonomous Drive based on distance. This will drive out for a

3
wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousTime.hpp
View File

@@ -12,7 +12,8 @@
#include "subsystems/Drivetrain.hpp"
class AutonomousTime
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup, AutonomousTime> {
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup,
AutonomousTime> {
public:
/**
* Creates a new Autonomous Drive based on time. This will drive out for a

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

@@ -10,7 +10,8 @@
#include "subsystems/Drivetrain.hpp"
class DriveDistance : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveDistance> {
class DriveDistance
: public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveDistance> {
public:
/**
* Creates a new DriveDistance. This command will drive your your robot for a

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

@@ -11,7 +11,8 @@
#include "subsystems/Drivetrain.hpp"
class TurnDegrees : public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnDegrees> {
class TurnDegrees
: public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnDegrees> {
public:
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired

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

@@ -35,12 +35,14 @@ class RobotContainer {
// value returned by the selector method at runtime. Note that selectcommand
// takes a generic type, so the selector does not have to be an enum; it could
// be any desired type (string, integer, boolean, double...)
wpi::cmd::CommandPtr m_exampleSelectCommand = wpi::cmd::cmd::Select<CommandSelector>(
[this] { return m_chooser.GetSelected(); },
// Maps selector values to commands
std::pair{ONE, wpi::cmd::cmd::Print("Command one was selected!")},
std::pair{TWO, wpi::cmd::cmd::Print("Command two was selected!")},
std::pair{THREE, wpi::cmd::cmd::Print("Command three was selected!")});
wpi::cmd::CommandPtr m_exampleSelectCommand =
wpi::cmd::cmd::Select<CommandSelector>(
[this] { return m_chooser.GetSelected(); },
// Maps selector values to commands
std::pair{ONE, wpi::cmd::cmd::Print("Command one was selected!")},
std::pair{TWO, wpi::cmd::cmd::Print("Command two was selected!")},
std::pair{THREE,
wpi::cmd::cmd::Print("Command three was selected!")});
void ConfigureButtonBindings();
};

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

@@ -6,7 +6,8 @@
#include <wpi/system/RobotController.hpp>
void Drivetrain::SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
void Drivetrain::SetSpeeds(
const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
auto leftFeedforward = m_feedforward.Calculate(speeds.left);
auto rightFeedforward = m_feedforward.Calculate(speeds.right);
double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
@@ -31,9 +32,9 @@ void Drivetrain::UpdateOdometry() {
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);
m_odometry.ResetPosition(
m_imu.GetRotation2d(), wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
void Drivetrain::SimulationPeriodic() {

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

@@ -15,7 +15,8 @@ class Robot : public wpi::TimedRobot {
public:
Robot() {
m_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::Pose2d{2_m, 2_m, 0_rad}, {},
wpi::math::Pose2d{6_m, 4_m, 0_rad},
wpi::math::TrajectoryConfig(2_mps, 2_mps_sq));
}

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

@@ -96,7 +96,8 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
1_V, 3_V / 1_mps};
// Simulation classes help us simulate our robot
wpi::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};

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

@@ -32,7 +32,8 @@ class Robot : public wpi::TimedRobot {
static constexpr wpi::units::radian_t kLoweredPosition = 0_deg;
// Moment of inertia of the arm. Can be estimated with CAD. If finding this
// constant is difficult, wpi::math::LinearSystem.identifyPositionSystem may be better.
// constant is difficult, wpi::math::LinearSystem.identifyPositionSystem may
// be better.
static constexpr wpi::units::kilogram_square_meter_t kArmMOI = 1.2_kg_sq_m;
// Reduction between motors and encoder, as output over input. If the arm
@@ -46,8 +47,8 @@ class Robot : public wpi::TimedRobot {
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [position], in radians.
wpi::math::LinearSystem<2, 1, 1> m_armPlant =
wpi::math::LinearSystemId::SingleJointedArmSystem(wpi::math::DCMotor::NEO(2), kArmMOI,
kArmGearing)
wpi::math::LinearSystemId::SingleJointedArmSystem(
wpi::math::DCMotor::NEO(2), kArmMOI, kArmGearing)
.Slice(0);
// The observer fuses our encoder data and voltage inputs to reject noise.
@@ -78,8 +79,8 @@ class Robot : public wpi::TimedRobot {
// The state-space loop combines a controller, observer, feedforward and plant
// for easy control.
wpi::math::LinearSystemLoop<2, 1, 1> m_loop{m_armPlant, m_controller, m_observer,
12_V, 20_ms};
wpi::math::LinearSystemLoop<2, 1, 1> m_loop{m_armPlant, m_controller,
m_observer, 12_V, 20_ms};
// An encoder set up to measure arm position in radians per second.
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
@@ -90,7 +91,8 @@ class Robot : public wpi::TimedRobot {
wpi::math::TrapezoidProfile<wpi::units::radians> m_profile{
{45_deg_per_s, 90_deg_per_s / 1_s}};
wpi::math::TrapezoidProfile<wpi::units::radians>::State m_lastProfiledReference;
wpi::math::TrapezoidProfile<wpi::units::radians>::State
m_lastProfiledReference;
public:
Robot() {
@@ -99,7 +101,8 @@ class Robot : public wpi::TimedRobot {
}
void TeleopInit() override {
m_loop.Reset(wpi::math::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_loop.Reset(
wpi::math::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_lastProfiledReference = {
wpi::units::radian_t{m_encoder.GetDistance()},
@@ -120,8 +123,9 @@ class Robot : public wpi::TimedRobot {
m_lastProfiledReference =
m_profile.Calculate(20_ms, m_lastProfiledReference, goal);
m_loop.SetNextR(wpi::math::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
m_loop.SetNextR(
wpi::math::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.Correct(wpi::math::Vectord<1>{m_encoder.GetDistance()});

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

@@ -44,8 +44,8 @@ class Robot : public wpi::TimedRobot {
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [position], in meters.
wpi::math::LinearSystem<2, 1, 1> m_elevatorPlant =
wpi::math::LinearSystemId::ElevatorSystem(wpi::math::DCMotor::NEO(2), kCarriageMass,
kDrumRadius, kGearRatio)
wpi::math::LinearSystemId::ElevatorSystem(
wpi::math::DCMotor::NEO(2), kCarriageMass, kDrumRadius, kGearRatio)
.Slice(0);
// The observer fuses our encoder data and voltage inputs to reject noise.
@@ -79,7 +79,7 @@ class Robot : public wpi::TimedRobot {
// The state-space loop combines a controller, observer, feedforward and plant
// for easy control.
wpi::math::LinearSystemLoop<2, 1, 1> m_loop{m_elevatorPlant, m_controller,
m_observer, 12_V, 20_ms};
m_observer, 12_V, 20_ms};
// An encoder set up to measure elevator height in meters.
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
@@ -89,7 +89,8 @@ class Robot : public wpi::TimedRobot {
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{{3_fps, 6_fps_sq}};
wpi::math::TrapezoidProfile<wpi::units::meters>::State m_lastProfiledReference;
wpi::math::TrapezoidProfile<wpi::units::meters>::State
m_lastProfiledReference;
public:
Robot() {
@@ -100,10 +101,12 @@ class Robot : public wpi::TimedRobot {
void TeleopInit() override {
// Reset our loop to make sure it's in a known state.
m_loop.Reset(wpi::math::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_loop.Reset(
wpi::math::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_lastProfiledReference = {wpi::units::meter_t{m_encoder.GetDistance()},
wpi::units::meters_per_second_t{m_encoder.GetRate()}};
m_lastProfiledReference = {
wpi::units::meter_t{m_encoder.GetDistance()},
wpi::units::meters_per_second_t{m_encoder.GetRate()}};
}
void TeleopPeriodic() override {
@@ -120,8 +123,9 @@ class Robot : public wpi::TimedRobot {
m_lastProfiledReference =
m_profile.Calculate(20_ms, m_lastProfiledReference, goal);
m_loop.SetNextR(wpi::math::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
m_loop.SetNextR(
wpi::math::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.Correct(wpi::math::Vectord<1>{m_encoder.GetDistance()});

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

@@ -28,8 +28,8 @@ class Robot : public wpi::TimedRobot {
static constexpr int kJoystickPort = 0;
static constexpr wpi::units::radians_per_second_t kSpinup = 500_rpm;
static constexpr wpi::units::kilogram_square_meter_t kFlywheelMomentOfInertia =
0.00032_kg_sq_m;
static constexpr wpi::units::kilogram_square_meter_t
kFlywheelMomentOfInertia = 0.00032_kg_sq_m;
// Reduction between motors and encoder, as output over input. If the flywheel
// spins slower than the motors, this number should be greater than one.
@@ -42,8 +42,9 @@ class Robot : public wpi::TimedRobot {
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [velocity], in radians per second.
wpi::math::LinearSystem<1, 1, 1> m_flywheelPlant =
wpi::math::LinearSystemId::FlywheelSystem(
wpi::math::DCMotor::NEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
wpi::math::LinearSystemId::FlywheelSystem(wpi::math::DCMotor::NEO(2),
kFlywheelMomentOfInertia,
kFlywheelGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
wpi::math::KalmanFilter<1, 1, 1> m_observer{
@@ -71,7 +72,7 @@ class Robot : public wpi::TimedRobot {
// The state-space loop combines a controller, observer, feedforward and plant
// for easy control.
wpi::math::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller,
m_observer, 12_V, 20_ms};
m_observer, 12_V, 20_ms};
// An encoder set up to measure flywheel velocity in radians per second.
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};

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

@@ -42,8 +42,8 @@ class Robot : public wpi::TimedRobot {
//
// The Kv and Ka constants are found using the FRC Characterization toolsuite.
wpi::math::LinearSystem<1, 1, 1> m_flywheelPlant =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::radian>(kFlywheelKv,
kFlywheelKa);
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::radian>(
kFlywheelKv, kFlywheelKa);
// The observer fuses our encoder data and voltage inputs to reject noise.
wpi::math::KalmanFilter<1, 1, 1> m_observer{
@@ -71,7 +71,7 @@ class Robot : public wpi::TimedRobot {
// The state-space loop combines a controller, observer, feedforward and plant
// for easy control.
wpi::math::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller,
m_observer, 12_V, 20_ms};
m_observer, 12_V, 20_ms};
// An encoder set up to measure flywheel velocity in radians per second.
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};

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

@@ -31,23 +31,23 @@ class Robot : public wpi::TimedRobot {
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
const auto xSpeed = -m_xspeedLimiter.Calculate(
wpi::math::ApplyDeadband(m_controller.GetLeftY(), 0.02)) *
const auto xSpeed = -m_xspeedLimiter.Calculate(wpi::math::ApplyDeadband(
m_controller.GetLeftY(), 0.02)) *
Drivetrain::kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
const auto ySpeed = -m_yspeedLimiter.Calculate(
wpi::math::ApplyDeadband(m_controller.GetLeftX(), 0.02)) *
const auto ySpeed = -m_yspeedLimiter.Calculate(wpi::math::ApplyDeadband(
m_controller.GetLeftX(), 0.02)) *
Drivetrain::kMaxSpeed;
// 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.
const auto rot = -m_rotLimiter.Calculate(
wpi::math::ApplyDeadband(m_controller.GetRightX(), 0.02)) *
const auto rot = -m_rotLimiter.Calculate(wpi::math::ApplyDeadband(
m_controller.GetRightX(), 0.02)) *
Drivetrain::kMaxAngularSpeed;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());

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

@@ -33,7 +33,8 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
m_turningPIDController.EnableContinuousInput(
-wpi::units::radian_t{std::numbers::pi}, wpi::units::radian_t{std::numbers::pi});
-wpi::units::radian_t{std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
}
wpi::math::SwerveModuleState SwerveModule::GetState() const {
@@ -46,7 +47,8 @@ wpi::math::SwerveModulePosition SwerveModule::GetPosition() const {
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState& referenceState) {
void SwerveModule::SetDesiredState(
wpi::math::SwerveModuleState& referenceState) {
wpi::math::Rotation2d encoderRotation{
wpi::units::radian_t{m_turningEncoder.GetDistance()}};

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

@@ -21,8 +21,9 @@ class Drivetrain {
Drivetrain() { m_imu.ResetYaw(); }
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed, wpi::units::radians_per_second_t rot,
bool fieldRelative, wpi::units::second_t period);
wpi::units::meters_per_second_t ySpeed,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr wpi::units::meters_per_second_t kMaxSpeed =

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

@@ -49,8 +49,8 @@ class SwerveModule {
0.0,
{kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_driveFeedforward{1_V,
3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_driveFeedforward{
1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_turnFeedforward{
1_V, 0.5_V / 1_rad_per_s};
};

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

@@ -33,7 +33,8 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
m_turningPIDController.EnableContinuousInput(
-wpi::units::radian_t{std::numbers::pi}, wpi::units::radian_t{std::numbers::pi});
-wpi::units::radian_t{std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
}
wpi::math::SwerveModuleState SwerveModule::GetState() const {
@@ -46,7 +47,8 @@ wpi::math::SwerveModulePosition SwerveModule::GetPosition() const {
wpi::units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(wpi::math::SwerveModuleState& referenceState) {
void SwerveModule::SetDesiredState(
wpi::math::SwerveModuleState& referenceState) {
wpi::math::Rotation2d encoderRotation{
wpi::units::radian_t{m_turningEncoder.GetDistance()}};

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

@@ -22,8 +22,9 @@ class Drivetrain {
Drivetrain() { m_imu.ResetYaw(); }
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed, wpi::units::radians_per_second_t rot,
bool fieldRelative, wpi::units::second_t period);
wpi::units::meters_per_second_t ySpeed,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second

9
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp
View File

@@ -16,9 +16,10 @@ class ExampleGlobalMeasurementSensor {
static wpi::math::Pose2d GetEstimatedGlobalPose(
const wpi::math::Pose2d& estimatedRobotPose) {
auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return wpi::math::Pose2d{estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
return wpi::math::Pose2d{
estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
}
};

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

@@ -49,8 +49,8 @@ class SwerveModule {
0.0,
{kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_driveFeedforward{1_V,
3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_driveFeedforward{
1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_turnFeedforward{
1_V, 0.5_V / 1_rad_per_s};
};

3
wpilibcExamples/src/main/cpp/examples/SysIdRoutine/cpp/Robot.cpp
View File

@@ -22,7 +22,8 @@ void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
wpi::cmd::CommandScheduler::GetInstance().Schedule(
m_autonomousCommand.value());
}
}

12
wpilibcExamples/src/main/cpp/examples/SysIdRoutine/cpp/SysIdRoutineBot.cpp
View File

@@ -18,9 +18,11 @@ void SysIdRoutineBot::ConfigureBindings() {
// Using bumpers as a modifier and combining it with the buttons so that we
// can have both sets of bindings at once
(m_driverController.A() && m_driverController.RightBumper())
.WhileTrue(m_drive.SysIdQuasistatic(wpi::cmd::sysid::Direction::kForward));
.WhileTrue(
m_drive.SysIdQuasistatic(wpi::cmd::sysid::Direction::kForward));
(m_driverController.B() && m_driverController.RightBumper())
.WhileTrue(m_drive.SysIdQuasistatic(wpi::cmd::sysid::Direction::kReverse));
.WhileTrue(
m_drive.SysIdQuasistatic(wpi::cmd::sysid::Direction::kReverse));
(m_driverController.X() && m_driverController.RightBumper())
.WhileTrue(m_drive.SysIdDynamic(wpi::cmd::sysid::Direction::kForward));
(m_driverController.Y() && m_driverController.RightBumper())
@@ -30,9 +32,11 @@ void SysIdRoutineBot::ConfigureBindings() {
[this] { return m_driverController.GetLeftTriggerAxis(); }));
(m_driverController.A() && m_driverController.LeftBumper())
.WhileTrue(m_shooter.SysIdQuasistatic(wpi::cmd::sysid::Direction::kForward));
.WhileTrue(
m_shooter.SysIdQuasistatic(wpi::cmd::sysid::Direction::kForward));
(m_driverController.B() && m_driverController.LeftBumper())
.WhileTrue(m_shooter.SysIdQuasistatic(wpi::cmd::sysid::Direction::kReverse));
.WhileTrue(
m_shooter.SysIdQuasistatic(wpi::cmd::sysid::Direction::kReverse));
(m_driverController.X() && m_driverController.LeftBumper())
.WhileTrue(m_shooter.SysIdDynamic(wpi::cmd::sysid::Direction::kForward));
(m_driverController.Y() && m_driverController.LeftBumper())

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

@@ -22,13 +22,14 @@ Drive::Drive() {
}
wpi::cmd::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
std::function<double()> rot) {
return wpi::cmd::cmd::Run([this, fwd, rot] { m_drive.ArcadeDrive(fwd(), rot()); },
{this})
std::function<double()> rot) {
return wpi::cmd::cmd::Run(
[this, fwd, rot] { m_drive.ArcadeDrive(fwd(), rot()); }, {this})
.WithName("Arcade Drive");
}
wpi::cmd::CommandPtr Drive::SysIdQuasistatic(wpi::cmd::sysid::Direction direction) {
wpi::cmd::CommandPtr Drive::SysIdQuasistatic(
wpi::cmd::sysid::Direction direction) {
return m_sysIdRoutine.Quasistatic(direction);
}

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

@@ -28,10 +28,12 @@ wpi::cmd::CommandPtr Shooter::RunShooterCommand(
.WithName("Set Shooter Speed");
}
wpi::cmd::CommandPtr Shooter::SysIdQuasistatic(wpi::cmd::sysid::Direction direction) {
wpi::cmd::CommandPtr Shooter::SysIdQuasistatic(
wpi::cmd::sysid::Direction direction) {
return m_sysIdRoutine.Quasistatic(direction);
}
wpi::cmd::CommandPtr Shooter::SysIdDynamic(wpi::cmd::sysid::Direction direction) {
wpi::cmd::CommandPtr Shooter::SysIdDynamic(
wpi::cmd::sysid::Direction direction) {
return m_sysIdRoutine.Dynamic(direction);
}

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

@@ -34,14 +34,14 @@ inline constexpr wpi::units::meter_t kEncoderDistancePerPulse =
namespace shooter {
using kv_unit =
wpi::units::compound_unit<wpi::units::compound_unit<wpi::units::volts, wpi::units::seconds>,
wpi::units::inverse<wpi::units::turns>>;
using kv_unit = wpi::units::compound_unit<
wpi::units::compound_unit<wpi::units::volts, wpi::units::seconds>,
wpi::units::inverse<wpi::units::turns>>;
using kv_unit_t = wpi::units::unit_t<kv_unit>;
using ka_unit =
wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<wpi::units::turns_per_second_squared>>;
using ka_unit = wpi::units::compound_unit<
wpi::units::volts,
wpi::units::inverse<wpi::units::turns_per_second_squared>>;
using ka_unit_t = wpi::units::unit_t<ka_unit>;
inline constexpr std::array<int, 2> kEncoderPorts = {4, 5};

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

@@ -20,7 +20,7 @@ class Drive : public wpi::cmd::SubsystemBase {
Drive();
wpi::cmd::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
std::function<double()> rot);
std::function<double()> rot);
wpi::cmd::CommandPtr SysIdQuasistatic(wpi::cmd::sysid::Direction direction);
wpi::cmd::CommandPtr SysIdDynamic(wpi::cmd::sysid::Direction direction);
@@ -39,7 +39,8 @@ class Drive : public wpi::cmd::SubsystemBase {
constants::drive::kRightEncoderReversed};
wpi::cmd::sysid::SysIdRoutine m_sysIdRoutine{
wpi::cmd::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr},
wpi::cmd::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
nullptr},
wpi::cmd::sysid::Mechanism{
[this](wpi::units::volt_t driveVoltage) {
m_leftMotor.SetVoltage(driveVoltage);
@@ -50,12 +51,14 @@ class Drive : public wpi::cmd::SubsystemBase {
.voltage(m_leftMotor.Get() *
wpi::RobotController::GetBatteryVoltage())
.position(wpi::units::meter_t{m_leftEncoder.GetDistance()})
.velocity(wpi::units::meters_per_second_t{m_leftEncoder.GetRate()});
.velocity(
wpi::units::meters_per_second_t{m_leftEncoder.GetRate()});
log->Motor("drive-right")
.voltage(m_rightMotor.Get() *
wpi::RobotController::GetBatteryVoltage())
.position(wpi::units::meter_t{m_rightEncoder.GetDistance()})
.velocity(wpi::units::meters_per_second_t{m_rightEncoder.GetRate()});
.velocity(
wpi::units::meters_per_second_t{m_rightEncoder.GetRate()});
},
this}};
};

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

@@ -33,7 +33,8 @@ class Shooter : public wpi::cmd::SubsystemBase {
constants::shooter::kEncoderReversed};
wpi::cmd::sysid::SysIdRoutine m_sysIdRoutine{
wpi::cmd::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr},
wpi::cmd::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
nullptr},
wpi::cmd::sysid::Mechanism{
[this](wpi::units::volt_t driveVoltage) {
m_shooterMotor.SetVoltage(driveVoltage);

2
wpilibcExamples/src/main/cpp/examples/XRPReference/include/commands/AutonomousDistance.hpp
View File

@@ -13,7 +13,7 @@
class AutonomousDistance
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup,
AutonomousDistance> {
AutonomousDistance> {
public:
/**
* Creates a new Autonomous Drive based on distance. This will drive out for a

3
wpilibcExamples/src/main/cpp/examples/XRPReference/include/commands/AutonomousTime.hpp
View File

@@ -12,7 +12,8 @@
#include "subsystems/Drivetrain.hpp"
class AutonomousTime
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup, AutonomousTime> {
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup,
AutonomousTime> {
public:
/**
* Creates a new Autonomous Drive based on time. This will drive out for a

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

@@ -10,7 +10,8 @@
#include "subsystems/Drivetrain.hpp"
class DriveDistance : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveDistance> {
class DriveDistance
: public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveDistance> {
public:
/**
* Creates a new DriveDistance. This command will drive your your robot for a

3
wpilibcExamples/src/main/cpp/examples/XRPReference/include/commands/TurnDegrees.hpp
View File

@@ -11,7 +11,8 @@
#include "subsystems/Drivetrain.hpp"
class TurnDegrees : public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnDegrees> {
class TurnDegrees
: public wpi::cmd::CommandHelper<wpi::cmd::Command, TurnDegrees> {
public:
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired