Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Clean up unit UDL usage #6961

Merged
merged 1 commit into from
Aug 14, 2024
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Clean up unit UDL usage
calcmogul committed Aug 14, 2024
commit 517b6e968e402f1b8afbca98f2d16f26e27a9692
2 changes: 1 addition & 1 deletion sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
Original file line number Diff line number Diff line change
@@ -172,7 +172,7 @@ AnalysisManager::AnalysisManager(TestData data, Settings& settings,
wpi::Logger& logger)
: m_data{std::move(data)}, m_logger{logger}, m_settings{settings} {
// Reset settings for Dynamic Test Limits
m_settings.stepTestDuration = units::second_t{0.0};
m_settings.stepTestDuration = 0_s;
m_settings.velocityThreshold = std::numeric_limits<double>::infinity();
}

Original file line number Diff line number Diff line change
@@ -56,15 +56,12 @@ class TrapezoidProfileCommand
void Initialize() override {}

void Execute() override {
m_output(
m_profile.Calculate(units::second_t{0.02}, m_currentState(), m_goal()));
m_output(m_profile.Calculate(20_ms, m_currentState(), m_goal()));
}

void End(bool interrupted) override {}

bool IsFinished() override {
return m_profile.IsFinished(units::second_t{0});
}
bool IsFinished() override { return m_profile.IsFinished(0_s); }

private:
frc::TrapezoidProfile<Distance> m_profile;
Original file line number Diff line number Diff line change
@@ -18,11 +18,11 @@ TEST_F(NotifierCommandTest, NotifierCommandSchedule) {
frc::sim::PauseTiming();

int counter = 0;
NotifierCommand command([&] { counter++; }, 0.01_s, {});
NotifierCommand command([&] { counter++; }, 10_ms, {});

scheduler.Schedule(&command);
for (int i = 0; i < 5; ++i) {
frc::sim::StepTiming(0.005_s);
frc::sim::StepTiming(5_ms);
}
scheduler.Cancel(&command);

2 changes: 1 addition & 1 deletion wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
Original file line number Diff line number Diff line change
@@ -818,7 +818,7 @@ units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const {
break;
}

return units::degree_t{0.0};
return 0_deg;
}

units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const {
2 changes: 1 addition & 1 deletion wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
Original file line number Diff line number Diff line change
@@ -264,7 +264,7 @@ void PneumaticsControlModule::UnreserveCompressor() {
}

units::volt_t PneumaticsControlModule::GetAnalogVoltage(int channel) const {
return units::volt_t{0};
return 0_V;
}

units::pounds_per_square_inch_t PneumaticsControlModule::GetPressure(
6 changes: 3 additions & 3 deletions wpilibc/src/main/native/cpp/counter/Tachometer.cpp
Original file line number Diff line number Diff line change
@@ -45,7 +45,7 @@ Tachometer::~Tachometer() {
units::hertz_t Tachometer::GetFrequency() const {
auto period = GetPeriod();
if (period.value() == 0) {
return units::hertz_t{0.0};
return 0_Hz;
}
return 1 / period;
}
@@ -67,11 +67,11 @@ void Tachometer::SetEdgesPerRevolution(int edges) {
units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const {
auto period = GetPeriod();
if (period.value() == 0) {
return units::turns_per_second_t{0.0};
return 0_tps;
}
int edgesPerRevolution = GetEdgesPerRevolution();
if (edgesPerRevolution == 0) {
return units::turns_per_second_t{0.0};
return 0_tps;
}
auto rotationHz = ((1.0 / edgesPerRevolution) / period);
return units::turns_per_second_t{rotationHz.value()};
2 changes: 1 addition & 1 deletion wpilibc/src/main/native/include/frc/LEDPattern.h
Original file line number Diff line number Diff line change
@@ -112,7 +112,7 @@ class LEDPattern {
*
* frc::LEDPattern rainbow = frc::LEDPattern::Rainbow();
* frc::LEDPattern scrollingRainbow =
* rainbow.ScrollAtAbsoluteSpeed(units::inches_per_second_t{4},
* rainbow.ScrollAtAbsoluteSpeed(units::feet_per_second_t{1 / 3.0},
* LED_SPACING);
* </pre>
*
Original file line number Diff line number Diff line change
@@ -114,7 +114,7 @@ TEST(EncoderSimTest, SetMaxPeriod) {
auto cb = sim.RegisterMaxPeriodCallback(callback.GetCallback(), false);

WPI_IGNORE_DEPRECATED
encoder.SetMaxPeriod(units::second_t{123.456});
encoder.SetMaxPeriod(123.456_s);
WPI_UNIGNORE_DEPRECATED
EXPECT_EQ(123.456, sim.GetMaxPeriod());

Original file line number Diff line number Diff line change
@@ -54,7 +54,7 @@ class Robot : public frc::TimedRobot {
m_flywheelSim.SetInputVoltage(
m_flywheelMotor.Get() *
units::volt_t{frc::RobotController::GetInputVoltage()});
m_flywheelSim.Update(0.02_s);
m_flywheelSim.Update(20_ms);
m_encoderSim.SetRate(m_flywheelSim.GetAngularVelocity().value());
}

Original file line number Diff line number Diff line change
@@ -24,7 +24,7 @@ inline constexpr bool kLeftEncoderReversed = false;
inline constexpr bool kRightEncoderReversed = true;

inline constexpr double kEncoderCPR = 1024;
inline constexpr units::meter_t kWheelDiameter = 6.0_in;
inline constexpr units::meter_t kWheelDiameter = 6_in;
inline constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value();
@@ -45,9 +45,8 @@ namespace ShooterConstants {
inline constexpr int kEncoderPorts[]{4, 5};
inline constexpr bool kEncoderReversed = false;
inline constexpr double kEncoderCPR = 1024;
inline constexpr double kEncoderDistancePerPulse =
// Distance units will be rotations
1.0 / kEncoderCPR;
// Distance units will be rotations
inline constexpr double kEncoderDistancePerPulse = 1.0 / kEncoderCPR;

inline constexpr int kShooterMotorPort = 4;
inline constexpr int kFeederMotorPort = 5;
@@ -61,9 +60,8 @@ inline constexpr auto kShooterTolerance = 50_tps;
inline constexpr double kP = 1;

inline constexpr units::volt_t kS = 0.05_V;
inline constexpr auto kV =
// Should have value 12V at free speed...
12.0_V / kShooterFree;
// Should have value 12V at free speed
inline constexpr auto kV = 12_V / kShooterFree;

inline constexpr double kFeederSpeed = 0.5;
} // namespace ShooterConstants
@@ -73,7 +71,7 @@ inline constexpr int kDriverControllerPort = 0;
} // namespace OIConstants

namespace AutoConstants {
constexpr units::second_t kTimeout = 3.0_s;
constexpr units::meter_t kDriveDistance = 2.0_m;
constexpr units::second_t kTimeout = 3_s;
constexpr units::meter_t kDriveDistance = 2_m;
constexpr double kDriveSpeed = 0.5;
} // namespace AutoConstants
Original file line number Diff line number Diff line change
@@ -37,9 +37,8 @@ public static final class ShooterConstants {
public static final int[] kEncoderPorts = {4, 5};
public static final boolean kEncoderReversed = false;
public static final int kEncoderCPR = 1024;
public static final double kEncoderDistancePerPulse =
// Distance units will be rotations
1.0 / kEncoderCPR;
// Distance units will be rotations
public static final double kEncoderDistancePerPulse = 1.0 / kEncoderCPR;

public static final int kShooterMotorPort = 4;
public static final int kFeederMotorPort = 5;
@@ -54,9 +53,8 @@ public static final class ShooterConstants {
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
public static final double kSVolts = 0.05;
public static final double kVVoltSecondsPerRotation =
// Should have value 12V at free speed...
12.0 / kShooterFreeRPS;
// Should have value 12V at free speed
public static final double kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS;

public static final double kFeederSpeed = 0.5;
}
Original file line number Diff line number Diff line change
@@ -25,8 +25,8 @@ TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
std::function<Vectord<2>(const Vectord<2>&, const Vectord<1>&)>
modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };

frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, units::second_t{0.02}};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
20_ms};

Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};
Original file line number Diff line number Diff line change
@@ -36,7 +36,7 @@ TEST(HolonomicDriveControllerTest, ReachesReference) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.0_mps, 4.0_mps_sq});

constexpr auto kDt = 0.02_s;
constexpr units::second_t kDt = 20_ms;
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
Original file line number Diff line number Diff line change
@@ -62,7 +62,7 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
}

TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
constexpr auto kDt = 0.02_s;
constexpr units::second_t kDt = 20_ms;

frc::LTVDifferentialDriveController controller{
plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
Original file line number Diff line number Diff line change
@@ -17,7 +17,7 @@ static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};

TEST(LTVUnicycleControllerTest, ReachesReference) {
constexpr auto kDt = 0.02_s;
constexpr units::second_t kDt = 20_ms;

frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
Original file line number Diff line number Diff line change
@@ -180,9 +180,9 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {

return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s};
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 20_ms};

controller.LatencyCompensate(plant, 0.02_s, 0.01_s);
controller.LatencyCompensate(plant, 20_ms, 10_ms);

EXPECT_NEAR(8.97115941, controller.K(0, 0), 1e-3);
EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3);
Original file line number Diff line number Diff line change
@@ -29,7 +29,7 @@ TEST(RamseteControllerTest, ReachesReference) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});

constexpr auto kDt = 0.02_s;
constexpr units::second_t kDt = 20_ms;
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
Original file line number Diff line number Diff line change
@@ -167,8 +167,8 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
0.1_s, 0.25_s, true, false);
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}

TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
@@ -205,8 +205,8 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
0.25_s, false, false);
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
}
@@ -275,7 +275,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

// Add enough measurements to fill up the buffer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m);
}

Original file line number Diff line number Diff line change
@@ -162,8 +162,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
0.1_s, 0.25_s, true, false);
trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms,
100_ms, 250_ms, true, false);
}

TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
@@ -204,8 +204,8 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
0.25_s, false, false);
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
}
@@ -276,7 +276,7 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

// Add enough measurements to fill up the buffer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation2d{},
frc::MecanumDriveWheelPositions{});
}
Original file line number Diff line number Diff line change
@@ -169,7 +169,7 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
},
[&](frc::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}},
0.02_s, 0.1_s, 0.25_s, true, false);
20_ms, 100_ms, 250_ms, true, false);
}

TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
@@ -213,8 +213,8 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
state.velocity * state.curvature};
},
[&](frc::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
0.25_s, false, false);
initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms,
250_ms, false, false);
}
}
}
@@ -293,7 +293,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

// Add enough measurements to fill up the buffer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
for (auto time = 0_s; time < 4_s; time += 20_ms) {
estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
}

Original file line number Diff line number Diff line change
@@ -97,7 +97,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);

units::second_t dt = 0.02_s;
units::second_t dt = 20_ms;
units::second_t t = 0_s;

double maxError = -std::numeric_limits<double>::max();
@@ -160,7 +160,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);

units::second_t dt = 0.02_s;
units::second_t dt = 20_ms;
units::second_t t = 0_s;

double maxError = -std::numeric_limits<double>::max();
Original file line number Diff line number Diff line change
@@ -93,7 +93,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);

units::second_t dt = 0.02_s;
units::second_t dt = 20_ms;
units::second_t t = 0_s;

double maxError = -std::numeric_limits<double>::max();
@@ -157,7 +157,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);

units::second_t dt = 0.02_s;
units::second_t dt = 20_ms;
units::second_t t = 0_s;

double maxError = -std::numeric_limits<double>::max();
Original file line number Diff line number Diff line change
@@ -9,17 +9,10 @@

using namespace frc;

namespace {

using ProtoType = wpi::Protobuf<frc::DCMotor>;

const DCMotor kExpectedData = DCMotor{units::volt_t{1.91},
units::newton_meter_t{19.1},
units::ampere_t{1.74},
units::ampere_t{2.29},
units::radians_per_second_t{2.2},
2};
} // namespace
inline constexpr DCMotor kExpectedData =
DCMotor{1.91_V, 19.1_Nm, 1.74_A, 2.29_A, 2.2_rad_per_s, 2};

TEST(DCMotorProtoTest, Roundtrip) {
google::protobuf::Arena arena;
Loading