diff --git a/glass/src/lib/native/cpp/other/Field2D.cpp b/glass/src/lib/native/cpp/other/Field2D.cpp index 786d3de45b..ec0210e375 100644 --- a/glass/src/lib/native/cpp/other/Field2D.cpp +++ b/glass/src/lib/native/cpp/other/Field2D.cpp @@ -261,17 +261,17 @@ static DisplayUnits gDisplayUnits = kDisplayMeters; static double ConvertDisplayLength(units::meter_t v) { switch (gDisplayUnits) { case kDisplayFeet: - return v.convert().to(); + return v.convert().value(); case kDisplayInches: - return v.convert().to(); + return v.convert().value(); case kDisplayMeters: default: - return v.to(); + return v.value(); } } static double ConvertDisplayAngle(units::degree_t v) { - return v.to(); + return v.value(); } static bool InputLength(const char* label, units::meter_t* v, double step = 0.0, diff --git a/glass/src/libnt/native/cpp/NTField2D.cpp b/glass/src/libnt/native/cpp/NTField2D.cpp index 3ce4580184..47fa9a7c82 100644 --- a/glass/src/libnt/native/cpp/NTField2D.cpp +++ b/glass/src/libnt/native/cpp/NTField2D.cpp @@ -94,9 +94,9 @@ void NTField2DModel::ObjectModel::UpdateNT() { wpi::SmallVector arr; for (auto&& pose : m_poses) { auto& translation = pose.Translation(); - arr.push_back(translation.X().to()); - arr.push_back(translation.Y().to()); - arr.push_back(pose.Rotation().Degrees().to()); + arr.push_back(translation.X().value()); + arr.push_back(translation.Y().value()); + arr.push_back(pose.Rotation().Degrees().value()); } nt::SetEntryTypeValue(m_entry, nt::Value::MakeDoubleArray(arr)); } else { @@ -107,13 +107,13 @@ void NTField2DModel::ObjectModel::UpdateNT() { for (auto&& pose : m_poses) { auto& translation = pose.Translation(); wpi::support::endian::write64be( - p, wpi::DoubleToBits(translation.X().to())); + p, wpi::DoubleToBits(translation.X().value())); p += 8; wpi::support::endian::write64be( - p, wpi::DoubleToBits(translation.Y().to())); + p, wpi::DoubleToBits(translation.Y().value())); p += 8; wpi::support::endian::write64be( - p, wpi::DoubleToBits(pose.Rotation().Degrees().to())); + p, wpi::DoubleToBits(pose.Rotation().Degrees().value())); p += 8; } nt::SetEntryTypeValue(m_entry, diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index b7a5dcbd59..c521f8b8ff 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -310,20 +310,20 @@ void MecanumControllerCommand::Execute() { (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt); auto frontLeftOutput = volt_t(m_frontLeftController->Calculate( - m_currentWheelSpeeds().frontLeft.to(), - frontLeftSpeedSetpoint.to())) + + m_currentWheelSpeeds().frontLeft.value(), + frontLeftSpeedSetpoint.value())) + frontLeftFeedforward; auto rearLeftOutput = volt_t(m_rearLeftController->Calculate( - m_currentWheelSpeeds().rearLeft.to(), - rearLeftSpeedSetpoint.to())) + + m_currentWheelSpeeds().rearLeft.value(), + rearLeftSpeedSetpoint.value())) + rearLeftFeedforward; auto frontRightOutput = volt_t(m_frontRightController->Calculate( - m_currentWheelSpeeds().frontRight.to(), - frontRightSpeedSetpoint.to())) + + m_currentWheelSpeeds().frontRight.value(), + frontRightSpeedSetpoint.value())) + frontRightFeedforward; auto rearRightOutput = volt_t(m_rearRightController->Calculate( - m_currentWheelSpeeds().rearRight.to(), - rearRightSpeedSetpoint.to())) + + m_currentWheelSpeeds().rearRight.value(), + rearRightSpeedSetpoint.value())) + rearRightFeedforward; m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput, diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index 0c0b852419..4674031b9d 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -126,15 +126,15 @@ void RamseteCommand::Execute() { targetWheelSpeeds.right, (targetWheelSpeeds.right - m_prevSpeeds.right) / dt); - auto leftOutput = volt_t(m_leftController->Calculate( - m_speeds().left.to(), - targetWheelSpeeds.left.to())) + - leftFeedforward; + auto leftOutput = + volt_t(m_leftController->Calculate(m_speeds().left.value(), + targetWheelSpeeds.left.value())) + + leftFeedforward; - auto rightOutput = volt_t(m_rightController->Calculate( - m_speeds().right.to(), - targetWheelSpeeds.right.to())) + - rightFeedforward; + auto rightOutput = + volt_t(m_rightController->Calculate(m_speeds().right.value(), + targetWheelSpeeds.right.value())) + + rightFeedforward; m_outputVolts(leftOutput, rightOutput); } else { diff --git a/wpilibOldCommands/src/main/native/cpp/PIDBase.cpp b/wpilibOldCommands/src/main/native/cpp/PIDBase.cpp index 2dc6fed13a..825d4ebf83 100644 --- a/wpilibOldCommands/src/main/native/cpp/PIDBase.cpp +++ b/wpilibOldCommands/src/main/native/cpp/PIDBase.cpp @@ -153,7 +153,7 @@ double PIDBase::GetSetpoint() const { double PIDBase::GetDeltaSetpoint() const { std::scoped_lock lock(m_thisMutex); - return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get().to(); + return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get().value(); } double PIDBase::GetError() const { diff --git a/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp index 66b88f2d88..778f9565de 100644 --- a/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp +++ b/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp @@ -34,7 +34,7 @@ Command::Command(std::string_view name, units::second_t timeout) { // We use -1.0 to indicate no timeout. if (timeout < 0_s && timeout != -1_s) { throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s", - timeout.to()); + timeout.value()); } m_timeout = timeout; @@ -177,7 +177,7 @@ int Command::GetID() const { void Command::SetTimeout(units::second_t timeout) { if (timeout < 0_s) { throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s", - timeout.to()); + timeout.value()); } else { m_timeout = timeout; } diff --git a/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp index c5ddab32dd..977795d93f 100644 --- a/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp +++ b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp @@ -38,7 +38,7 @@ void CommandGroup::AddSequential(Command* command, units::second_t timeout) { } if (timeout < 0_s) { throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s", - timeout.to()); + timeout.value()); } m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence, @@ -82,7 +82,7 @@ void CommandGroup::AddParallel(Command* command, units::second_t timeout) { } if (timeout < 0_s) { throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s", - timeout.to()); + timeout.value()); } m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild, diff --git a/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp index 237c00baa2..b4d44e05ae 100644 --- a/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp +++ b/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp @@ -9,7 +9,7 @@ using namespace frc; WaitCommand::WaitCommand(units::second_t timeout) - : TimedCommand(fmt::format("Wait({})", timeout.to()), timeout) {} + : TimedCommand(fmt::format("Wait({})", timeout.value()), timeout) {} WaitCommand::WaitCommand(std::string_view name, units::second_t timeout) : TimedCommand(name, timeout) {} diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index 889a1d312b..5f26e87fb9 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -93,7 +93,7 @@ double AnalogEncoder::GetDistancePerRotation() const { } double AnalogEncoder::GetDistance() const { - return Get().to() * GetDistancePerRotation(); + return Get().value() * GetDistancePerRotation(); } void AnalogEncoder::Reset() { diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp index 3516d2a418..a1ad9e7a38 100644 --- a/wpilibc/src/main/native/cpp/Counter.cpp +++ b/wpilibc/src/main/native/cpp/Counter.cpp @@ -282,7 +282,7 @@ units::second_t Counter::GetPeriod() const { void Counter::SetMaxPeriod(units::second_t maxPeriod) { int32_t status = 0; - HAL_SetCounterMaxPeriod(m_counter, maxPeriod.to(), &status); + HAL_SetCounterMaxPeriod(m_counter, maxPeriod.value(), &status); FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod"); } diff --git a/wpilibc/src/main/native/cpp/DMA.cpp b/wpilibc/src/main/native/cpp/DMA.cpp index 5eeaa60d02..80c0adba20 100644 --- a/wpilibc/src/main/native/cpp/DMA.cpp +++ b/wpilibc/src/main/native/cpp/DMA.cpp @@ -42,7 +42,7 @@ void DMA::SetPause(bool pause) { void DMA::SetTimedTrigger(units::second_t seconds) { int32_t status = 0; - HAL_SetDMATimedTrigger(dmaHandle, seconds.to(), &status); + HAL_SetDMATimedTrigger(dmaHandle, seconds.value(), &status); FRC_CheckErrorStatus(status, "{}", "SetTimedTrigger"); } diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp index d3b7a309bb..8e994e9baa 100644 --- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp +++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp @@ -112,7 +112,7 @@ double DutyCycleEncoder::GetDistancePerRotation() const { } double DutyCycleEncoder::GetDistance() const { - return Get().to() * GetDistancePerRotation(); + return Get().value() * GetDistancePerRotation(); } int DutyCycleEncoder::GetFrequency() const { diff --git a/wpilibc/src/main/native/cpp/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp index 5538c9b71b..bef6d76aea 100644 --- a/wpilibc/src/main/native/cpp/Encoder.cpp +++ b/wpilibc/src/main/native/cpp/Encoder.cpp @@ -87,7 +87,7 @@ units::second_t Encoder::GetPeriod() const { void Encoder::SetMaxPeriod(units::second_t maxPeriod) { int32_t status = 0; - HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod.to(), &status); + HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod.value(), &status); FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod"); } diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index 9474936c80..47eb29914d 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -200,6 +200,5 @@ void IterativeRobotBase::LoopFunc() { } void IterativeRobotBase::PrintLoopOverrunMessage() { - FRC_ReportError(err::Error, "Loop time of {:.6f}s overrun", - m_period.to()); + FRC_ReportError(err::Error, "Loop time of {:.6f}s overrun", m_period.value()); } diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp index 6d85b2daab..8bc2d6b2c9 100644 --- a/wpilibc/src/main/native/cpp/RobotController.cpp +++ b/wpilibc/src/main/native/cpp/RobotController.cpp @@ -168,7 +168,7 @@ units::volt_t RobotController::GetBrownoutVoltage() { void RobotController::SetBrownoutVoltage(units::volt_t brownoutVoltage) { int32_t status = 0; - HAL_SetBrownoutVoltage(brownoutVoltage.to(), &status); + HAL_SetBrownoutVoltage(brownoutVoltage.value(), &status); FRC_CheckErrorStatus(status, "{}", "SetBrownoutVoltage"); } diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp index 1c37db34b4..bdcee45eae 100644 --- a/wpilibc/src/main/native/cpp/SPI.cpp +++ b/wpilibc/src/main/native/cpp/SPI.cpp @@ -274,7 +274,7 @@ void SPI::SetAutoTransmitData(wpi::span dataToSend, void SPI::StartAutoRate(units::second_t period) { int32_t status = 0; - HAL_StartSPIAutoRate(m_port, period.to(), &status); + HAL_StartSPIAutoRate(m_port, period.value(), &status); FRC_CheckErrorStatus(status, "Port {}", m_port); } @@ -307,7 +307,7 @@ int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, units::second_t timeout) { int32_t status = 0; int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead, - timeout.to(), &status); + timeout.value(), &status); FRC_CheckErrorStatus(status, "Port {}", m_port); return val; } diff --git a/wpilibc/src/main/native/cpp/SerialPort.cpp b/wpilibc/src/main/native/cpp/SerialPort.cpp index 73af1f1e3d..fb984f19dc 100644 --- a/wpilibc/src/main/native/cpp/SerialPort.cpp +++ b/wpilibc/src/main/native/cpp/SerialPort.cpp @@ -124,7 +124,7 @@ int SerialPort::Write(std::string_view buffer) { void SerialPort::SetTimeout(units::second_t timeout) { int32_t status = 0; - HAL_SetSerialTimeout(m_portHandle, timeout.to(), &status); + HAL_SetSerialTimeout(m_portHandle, timeout.value(), &status); FRC_CheckErrorStatus(status, "{}", "SetTimeout"); } diff --git a/wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp b/wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp index 54b740a708..49acd84f6a 100644 --- a/wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp +++ b/wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp @@ -64,8 +64,8 @@ inline SynchronousInterrupt::WaitResult operator|( SynchronousInterrupt::WaitResult SynchronousInterrupt::WaitForInterrupt( units::second_t timeout, bool ignorePrevious) { int32_t status = 0; - auto result = HAL_WaitForInterrupt(m_handle, timeout.to(), - ignorePrevious, &status); + auto result = + HAL_WaitForInterrupt(m_handle, timeout.value(), ignorePrevious, &status); auto rising = ((result & 0xFF) != 0) ? WaitResult::kRisingEdge : WaitResult::kTimeout; diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp index f5bdb93125..bbd2262f65 100644 --- a/wpilibc/src/main/native/cpp/Timer.cpp +++ b/wpilibc/src/main/native/cpp/Timer.cpp @@ -13,8 +13,7 @@ namespace frc { void Wait(units::second_t seconds) { - std::this_thread::sleep_for( - std::chrono::duration(seconds.to())); + std::this_thread::sleep_for(std::chrono::duration(seconds.value())); } units::second_t GetTime() { diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp index aad11bedf9..02035dd14a 100644 --- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp +++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp @@ -162,7 +162,7 @@ void Ultrasonic::SetEnabled(bool enable) { void Ultrasonic::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("Ultrasonic"); builder.AddDoubleProperty( - "Value", [=] { return units::inch_t{GetRange()}.to(); }, nullptr); + "Value", [=] { return units::inch_t{GetRange()}.value(); }, nullptr); } void Ultrasonic::Initialize() { diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp index 8fbfe81f71..854f9e9d39 100644 --- a/wpilibc/src/main/native/cpp/Watchdog.cpp +++ b/wpilibc/src/main/native/cpp/Watchdog.cpp @@ -80,7 +80,7 @@ void Watchdog::Impl::UpdateAlarm() { } else { HAL_UpdateNotifierAlarm( notifier, - static_cast(m_watchdogs.top()->m_expirationTime.to() * + static_cast(m_watchdogs.top()->m_expirationTime.value() * 1e6), &status); } @@ -114,7 +114,7 @@ void Watchdog::Impl::Main() { watchdog->m_lastTimeoutPrintTime = now; if (!watchdog->m_suppressTimeoutMessage) { FRC_ReportError(warn::Warning, "Watchdog not fed within {:.6f}s", - watchdog->m_timeout.to()); + watchdog->m_timeout.value()); } } diff --git a/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp index 4fdf288412..a21d1b78ed 100644 --- a/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp @@ -16,9 +16,9 @@ ADXRS450_GyroSim::ADXRS450_GyroSim(const frc::ADXRS450_Gyro& gyro) { } void ADXRS450_GyroSim::SetAngle(units::degree_t angle) { - m_simAngle.Set(angle.to()); + m_simAngle.Set(angle.value()); } void ADXRS450_GyroSim::SetRate(units::degrees_per_second_t rate) { - m_simRate.Set(rate.to()); + m_simRate.Set(rate.value()); } diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp index e331078efb..049241e0b3 100644 --- a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp @@ -19,7 +19,7 @@ void AnalogEncoderSim::SetPosition(frc::Rotation2d angle) { } void AnalogEncoderSim::SetTurns(units::turn_t turns) { - m_positionSim.Set(turns.to()); + m_positionSim.Set(turns.value()); } units::turn_t AnalogEncoderSim::GetTurns() { diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 5f53575df8..0bbf9c381c 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -49,7 +49,7 @@ Eigen::Vector DifferentialDrivetrainSim::ClampInput( void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage) { - m_u << leftVoltage.to(), rightVoltage.to(); + m_u << leftVoltage.value(), rightVoltage.value(); m_u = ClampInput(m_u); } @@ -93,10 +93,10 @@ Pose2d DifferentialDrivetrainSim::GetPose() const { units::ampere_t DifferentialDrivetrainSim::GetLeftCurrentDraw() const { auto loadIleft = - m_motor.Current(units::radians_per_second_t(m_x(State::kLeftVelocity) * - m_currentGearing / - m_wheelRadius.to()), - units::volt_t(m_u(0))) * + m_motor.Current( + units::radians_per_second_t(m_x(State::kLeftVelocity) * + m_currentGearing / m_wheelRadius.value()), + units::volt_t(m_u(0))) * wpi::sgn(m_u(0)); return loadIleft; @@ -104,10 +104,10 @@ units::ampere_t DifferentialDrivetrainSim::GetLeftCurrentDraw() const { units::ampere_t DifferentialDrivetrainSim::GetRightCurrentDraw() const { auto loadIRight = - m_motor.Current(units::radians_per_second_t(m_x(State::kRightVelocity) * - m_currentGearing / - m_wheelRadius.to()), - units::volt_t(m_u(1))) * + m_motor.Current( + units::radians_per_second_t(m_x(State::kRightVelocity) * + m_currentGearing / m_wheelRadius.value()), + units::volt_t(m_u(1))) * wpi::sgn(m_u(1)); return loadIRight; @@ -122,9 +122,9 @@ void DifferentialDrivetrainSim::SetState( } void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) { - m_x(State::kX) = pose.X().to(); - m_x(State::kY) = pose.Y().to(); - m_x(State::kHeading) = pose.Rotation().Radians().to(); + m_x(State::kX) = pose.X().value(); + m_x(State::kY) = pose.Y().value(); + m_x(State::kHeading) = pose.Rotation().Radians().value(); m_x(State::kLeftPosition) = 0; m_x(State::kRightPosition) = 0; } @@ -154,7 +154,7 @@ Eigen::Vector DifferentialDrivetrainSim::Dynamics( xdot(1) = v * std::sin(x(State::kHeading)); xdot(2) = ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / (2.0 * m_rb)) - .to(); + .value(); xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u; return xdot; } diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp index 9e9bcb3d2a..cb83ccbe1d 100644 --- a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp @@ -17,7 +17,7 @@ DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder) { } void DutyCycleEncoderSim::Set(units::turn_t turns) { - m_simPosition.Set(turns.to()); + m_simPosition.Set(turns.value()); } void DutyCycleEncoderSim::SetDistance(double distance) { diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 31d2f3a89a..a2d5c663df 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -78,7 +78,7 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { } void ElevatorSim::SetInputVoltage(units::volt_t voltage) { - SetInput(Eigen::Vector{voltage.to()}); + SetInput(Eigen::Vector{voltage.value()}); } Eigen::Vector ElevatorSim::UpdateX( @@ -93,10 +93,10 @@ Eigen::Vector ElevatorSim::UpdateX( currentXhat, u, dt); // Check for collision after updating x-hat. if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) { - return Eigen::Vector{m_minHeight.to(), 0.0}; + return Eigen::Vector{m_minHeight.value(), 0.0}; } if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) { - return Eigen::Vector{m_maxHeight.to(), 0.0}; + return Eigen::Vector{m_maxHeight.value(), 0.0}; } return updatedXhat; } diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index 442183a163..f4371b12bb 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -38,5 +38,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const { } void FlywheelSim::SetInputVoltage(units::volt_t voltage) { - SetInput(Eigen::Vector{voltage.to()}); + SetInput(Eigen::Vector{voltage.value()}); } diff --git a/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp index f857e0404b..dc2b55754a 100644 --- a/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp @@ -43,7 +43,7 @@ units::volt_t RoboRioSim::GetVInVoltage() { } void RoboRioSim::SetVInVoltage(units::volt_t vInVoltage) { - HALSIM_SetRoboRioVInVoltage(vInVoltage.to()); + HALSIM_SetRoboRioVInVoltage(vInVoltage.value()); } std::unique_ptr RoboRioSim::RegisterVInCurrentCallback( @@ -60,7 +60,7 @@ units::ampere_t RoboRioSim::GetVInCurrent() { } void RoboRioSim::SetVInCurrent(units::ampere_t vInCurrent) { - HALSIM_SetRoboRioVInCurrent(vInCurrent.to()); + HALSIM_SetRoboRioVInCurrent(vInCurrent.value()); } std::unique_ptr RoboRioSim::RegisterUserVoltage6VCallback( @@ -77,7 +77,7 @@ units::volt_t RoboRioSim::GetUserVoltage6V() { } void RoboRioSim::SetUserVoltage6V(units::volt_t userVoltage6V) { - HALSIM_SetRoboRioUserVoltage6V(userVoltage6V.to()); + HALSIM_SetRoboRioUserVoltage6V(userVoltage6V.value()); } std::unique_ptr RoboRioSim::RegisterUserCurrent6VCallback( @@ -94,7 +94,7 @@ units::ampere_t RoboRioSim::GetUserCurrent6V() { } void RoboRioSim::SetUserCurrent6V(units::ampere_t userCurrent6V) { - HALSIM_SetRoboRioUserCurrent6V(userCurrent6V.to()); + HALSIM_SetRoboRioUserCurrent6V(userCurrent6V.value()); } std::unique_ptr RoboRioSim::RegisterUserActive6VCallback( @@ -128,7 +128,7 @@ units::volt_t RoboRioSim::GetUserVoltage5V() { } void RoboRioSim::SetUserVoltage5V(units::volt_t userVoltage5V) { - HALSIM_SetRoboRioUserVoltage5V(userVoltage5V.to()); + HALSIM_SetRoboRioUserVoltage5V(userVoltage5V.value()); } std::unique_ptr RoboRioSim::RegisterUserCurrent5VCallback( @@ -145,7 +145,7 @@ units::ampere_t RoboRioSim::GetUserCurrent5V() { } void RoboRioSim::SetUserCurrent5V(units::ampere_t userCurrent5V) { - HALSIM_SetRoboRioUserCurrent5V(userCurrent5V.to()); + HALSIM_SetRoboRioUserCurrent5V(userCurrent5V.value()); } std::unique_ptr RoboRioSim::RegisterUserActive5VCallback( @@ -179,7 +179,7 @@ units::volt_t RoboRioSim::GetUserVoltage3V3() { } void RoboRioSim::SetUserVoltage3V3(units::volt_t userVoltage3V3) { - HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3.to()); + HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3.value()); } std::unique_ptr RoboRioSim::RegisterUserCurrent3V3Callback( @@ -196,7 +196,7 @@ units::ampere_t RoboRioSim::GetUserCurrent3V3() { } void RoboRioSim::SetUserCurrent3V3(units::ampere_t userCurrent3V3) { - HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3.to()); + HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3.value()); } std::unique_ptr RoboRioSim::RegisterUserActive3V3Callback( @@ -281,7 +281,7 @@ units::volt_t RoboRioSim::GetBrownoutVoltage() { } void RoboRioSim::SetBrownoutVoltage(units::volt_t vInVoltage) { - HALSIM_SetRoboRioBrownoutVoltage(vInVoltage.to()); + HALSIM_SetRoboRioBrownoutVoltage(vInVoltage.value()); } void RoboRioSim::ResetData() { diff --git a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp index 8a7889b58e..fe1cc65380 100644 --- a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp @@ -41,11 +41,11 @@ bool IsTimingPaused() { } void StepTiming(units::second_t delta) { - HALSIM_StepTiming(static_cast(delta.to() * 1e6)); + HALSIM_StepTiming(static_cast(delta.value() * 1e6)); } void StepTimingAsync(units::second_t delta) { - HALSIM_StepTimingAsync(static_cast(delta.to() * 1e6)); + HALSIM_StepTimingAsync(static_cast(delta.value() * 1e6)); } } // namespace frc::sim diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index eeb982d1d6..db56434ffe 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -72,7 +72,7 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const { } void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) { - SetInput(Eigen::Vector{voltage.to()}); + SetInput(Eigen::Vector{voltage.value()}); } Eigen::Vector SingleJointedArmSim::UpdateX( @@ -96,7 +96,7 @@ Eigen::Vector SingleJointedArmSim::UpdateX( xdot += Eigen::Vector{ 0.0, (m_mass * m_r * -9.8 * 3.0 / (m_mass * m_r * m_r) * std::cos(x(0))) - .template to()}; + .value()}; } return xdot; }, @@ -104,9 +104,9 @@ Eigen::Vector SingleJointedArmSim::UpdateX( // Check for collisions. if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) { - return Eigen::Vector{m_minAngle.to(), 0.0}; + return Eigen::Vector{m_minAngle.value(), 0.0}; } else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) { - return Eigen::Vector{m_maxAngle.to(), 0.0}; + return Eigen::Vector{m_maxAngle.value(), 0.0}; } return updatedXhat; } diff --git a/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp b/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp index d5040f6e45..d2d1687faa 100644 --- a/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp @@ -20,5 +20,5 @@ void UltrasonicSim::SetRangeValid(bool isValid) { } void UltrasonicSim::SetRange(units::meter_t range) { - m_simRange.Set(range.to()); + m_simRange.Set(range.value()); } diff --git a/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp index bad7288aee..93d6d7ea76 100644 --- a/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp +++ b/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp @@ -87,9 +87,9 @@ void FieldObject2d::UpdateEntry(bool setDefault) { wpi::SmallVector arr; for (auto&& pose : m_poses) { auto& translation = pose.Translation(); - arr.push_back(translation.X().to()); - arr.push_back(translation.Y().to()); - arr.push_back(pose.Rotation().Degrees().to()); + arr.push_back(translation.X().value()); + arr.push_back(translation.Y().value()); + arr.push_back(pose.Rotation().Degrees().value()); } if (setDefault) { m_entry.SetDefaultDoubleArray(arr); @@ -104,13 +104,13 @@ void FieldObject2d::UpdateEntry(bool setDefault) { for (auto&& pose : m_poses) { auto& translation = pose.Translation(); wpi::support::endian::write64be( - p, wpi::DoubleToBits(translation.X().to())); + p, wpi::DoubleToBits(translation.X().value())); p += 8; wpi::support::endian::write64be( - p, wpi::DoubleToBits(translation.Y().to())); + p, wpi::DoubleToBits(translation.Y().value())); p += 8; wpi::support::endian::write64be( - p, wpi::DoubleToBits(pose.Rotation().Degrees().to())); + p, wpi::DoubleToBits(pose.Rotation().Degrees().value())); p += 8; } if (setDefault) { diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp index c46b8e12d3..175db0da8d 100644 --- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp +++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp @@ -14,7 +14,7 @@ MechanismLigament2d::MechanismLigament2d(std::string_view name, double length, const frc::Color8Bit& color) : MechanismObject2d(name), m_length{length}, - m_angle{angle.to()}, + m_angle{angle.value()}, m_weight{lineWeight} { SetColor(color); } @@ -40,7 +40,7 @@ void MechanismLigament2d::SetColor(const Color8Bit& color) { void MechanismLigament2d::SetAngle(units::degree_t angle) { std::scoped_lock lock(m_mutex); - m_angle = angle.to(); + m_angle = angle.value(); Flush(); } diff --git a/wpilibc/src/main/native/include/frc/DMASample.h b/wpilibc/src/main/native/include/frc/DMASample.h index 9d78cca6d5..48a0a9e9ca 100644 --- a/wpilibc/src/main/native/include/frc/DMASample.h +++ b/wpilibc/src/main/native/include/frc/DMASample.h @@ -27,8 +27,8 @@ class DMASample : public HAL_DMASample { DMAReadStatus Update(const DMA* dma, units::second_t timeout, int32_t* remaining, int32_t* status) { - return static_cast(HAL_ReadDMA( - dma->dmaHandle, this, timeout.to(), remaining, status)); + return static_cast( + HAL_ReadDMA(dma->dmaHandle, this, timeout.value(), remaining, status)); } uint64_t GetTime() const { return timeStamp; } diff --git a/wpilibc/src/test/native/cpp/UltrasonicTest.cpp b/wpilibc/src/test/native/cpp/UltrasonicTest.cpp index 5932efc35a..72d72d2b0d 100644 --- a/wpilibc/src/test/native/cpp/UltrasonicTest.cpp +++ b/wpilibc/src/test/native/cpp/UltrasonicTest.cpp @@ -12,15 +12,15 @@ TEST(UltrasonicTest, SimDevices) { Ultrasonic ultrasonic{0, 1}; sim::UltrasonicSim sim{ultrasonic}; - EXPECT_EQ(0, ultrasonic.GetRange().to()); + EXPECT_EQ(0, ultrasonic.GetRange().value()); EXPECT_TRUE(ultrasonic.IsRangeValid()); sim.SetRange(units::meter_t{35.04}); - EXPECT_EQ(35.04, ultrasonic.GetRange().to()); + EXPECT_EQ(35.04, ultrasonic.GetRange().value()); sim.SetRangeValid(false); EXPECT_FALSE(ultrasonic.IsRangeValid()); - EXPECT_EQ(0, ultrasonic.GetRange().to()); + EXPECT_EQ(0, ultrasonic.GetRange().value()); } } // namespace frc diff --git a/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp index 0ca83e276e..57c4fbe248 100644 --- a/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp @@ -25,8 +25,8 @@ TEST(ADXRS450GyroSimTest, SetAttributes) { sim.SetAngle(TEST_ANGLE); sim.SetRate(TEST_RATE); - EXPECT_EQ(TEST_ANGLE.to(), gyro.GetAngle()); - EXPECT_EQ(TEST_RATE.to(), gyro.GetRate()); + EXPECT_EQ(TEST_ANGLE.value(), gyro.GetAngle()); + EXPECT_EQ(TEST_RATE.value(), gyro.GetRate()); gyro.Reset(); EXPECT_EQ(0, gyro.GetAngle()); diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp index 461950e0a6..375e21a43c 100644 --- a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp @@ -20,8 +20,8 @@ TEST(AnalogEncoderSimTest, Basic) { frc::sim::AnalogEncoderSim encoderSim{encoder}; encoderSim.SetPosition(180_deg); - EXPECT_NEAR(encoder.Get().to(), 0.5, 1E-8); - EXPECT_NEAR(encoderSim.GetTurns().to(), 0.5, 1E-8); - EXPECT_NEAR(encoderSim.GetPosition().Radians().to(), wpi::numbers::pi, + EXPECT_NEAR(encoder.Get().value(), 0.5, 1E-8); + EXPECT_NEAR(encoderSim.GetTurns().value(), 0.5, 1E-8); + EXPECT_NEAR(encoderSim.GetPosition().Radians().value(), wpi::numbers::pi, 1E-8); } diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogGyroSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogGyroSimTest.cpp index 07eddee0a9..f1baaca769 100644 --- a/wpilibc/src/test/native/cpp/simulation/AnalogGyroSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/AnalogGyroSimTest.cpp @@ -43,7 +43,7 @@ TEST(AnalogGyroSimTest, SetAngle) { sim.SetAngle(kTestAngle); EXPECT_EQ(kTestAngle, sim.GetAngle()); EXPECT_EQ(kTestAngle, gyro.GetAngle()); - EXPECT_EQ(-kTestAngle, gyro.GetRotation2d().Degrees().to()); + EXPECT_EQ(-kTestAngle, gyro.GetRotation2d().Degrees().value()); EXPECT_TRUE(callback.WasTriggered()); EXPECT_EQ(kTestAngle, callback.GetLastValue()); } diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp index d67747bf3d..eabbecb367 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp @@ -43,13 +43,13 @@ TEST(DifferentialDriveSimTest, Convergence) { frc::Pose2d(), {}, frc::Pose2d(2_m, 2_m, 0_rad), config); // NOLINTNEXTLINE - for (double t = 0; t < trajectory.TotalTime().to(); t += 0.02) { + for (double t = 0; t < trajectory.TotalTime().value(); t += 0.02) { auto state = trajectory.Sample(20_ms); auto ramseteOut = ramsete.Calculate(sim.GetPose(), state); auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut); - auto voltages = feedforward.Calculate( - Eigen::Vector{l.to(), r.to()}); + auto voltages = + feedforward.Calculate(Eigen::Vector{l.value(), r.value()}); // Sim periodic code. sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0))); @@ -65,10 +65,9 @@ TEST(DifferentialDriveSimTest, Convergence) { // 2 inch tolerance is OK since our ground truth is an approximation of the // ODE solution using RK4 anyway - EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().to(), 0.05); - EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().to(), 0.05); - EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().to(), - 0.01); + EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05); + EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05); + EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01); } TEST(DifferentialDriveSimTest, Current) { diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index ce98e4ccb6..bfc50e15bc 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -41,7 +41,7 @@ TEST(ElevatorSimTest, StateSpaceSim) { encoderSim.SetDistance(y(0)); } - EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().to(), 0.2); + EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().value(), 0.2); } TEST(ElevatorSimTest, MinMax) { diff --git a/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp index 58a147dfa0..be13ca5fce 100644 --- a/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp @@ -91,7 +91,7 @@ TEST(EncoderSimTest, Period) { auto cb = sim.RegisterPeriodCallback(callback.GetCallback(), false); sim.SetPeriod(123.456); EXPECT_EQ(123.456, sim.GetPeriod()); - EXPECT_EQ(123.456, encoder.GetPeriod().to()); + EXPECT_EQ(123.456, encoder.GetPeriod().value()); EXPECT_EQ(kDefaultDistancePerPulse / 123.456, encoder.GetRate()); EXPECT_TRUE(callback.WasTriggered()); diff --git a/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp index 90d3f2e635..c6e93d4eec 100644 --- a/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp @@ -50,13 +50,13 @@ TEST(RoboRioSimTest, SetVin) { RoboRioSim::SetVInVoltage(units::volt_t{kTestVoltage}); EXPECT_TRUE(voltageCallback.WasTriggered()); EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue()); - EXPECT_EQ(kTestVoltage, RoboRioSim::GetVInVoltage().to()); + EXPECT_EQ(kTestVoltage, RoboRioSim::GetVInVoltage().value()); EXPECT_EQ(kTestVoltage, RobotController::GetInputVoltage()); RoboRioSim::SetVInCurrent(units::ampere_t{kTestCurrent}); EXPECT_TRUE(currentCallback.WasTriggered()); EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue()); - EXPECT_EQ(kTestCurrent, RoboRioSim::GetVInCurrent().to()); + EXPECT_EQ(kTestCurrent, RoboRioSim::GetVInCurrent().value()); EXPECT_EQ(kTestCurrent, RobotController::GetInputCurrent()); } @@ -71,8 +71,8 @@ TEST(RoboRioSimTest, SetBrownout) { RoboRioSim::SetBrownoutVoltage(units::volt_t{kTestVoltage}); EXPECT_TRUE(voltageCallback.WasTriggered()); EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue()); - EXPECT_EQ(kTestVoltage, RoboRioSim::GetBrownoutVoltage().to()); - EXPECT_EQ(kTestVoltage, RobotController::GetBrownoutVoltage().to()); + EXPECT_EQ(kTestVoltage, RoboRioSim::GetBrownoutVoltage().value()); + EXPECT_EQ(kTestVoltage, RobotController::GetBrownoutVoltage().value()); } TEST(RoboRioSimTest, Set6V) { @@ -97,13 +97,13 @@ TEST(RoboRioSimTest, Set6V) { RoboRioSim::SetUserVoltage6V(units::volt_t{kTestVoltage}); EXPECT_TRUE(voltageCallback.WasTriggered()); EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue()); - EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage6V().to()); + EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage6V().value()); EXPECT_EQ(kTestVoltage, RobotController::GetVoltage6V()); RoboRioSim::SetUserCurrent6V(units::ampere_t{kTestCurrent}); EXPECT_TRUE(currentCallback.WasTriggered()); EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue()); - EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent6V().to()); + EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent6V().value()); EXPECT_EQ(kTestCurrent, RobotController::GetCurrent6V()); RoboRioSim::SetUserActive6V(false); @@ -141,13 +141,13 @@ TEST(RoboRioSimTest, Set5V) { RoboRioSim::SetUserVoltage5V(units::volt_t{kTestVoltage}); EXPECT_TRUE(voltageCallback.WasTriggered()); EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue()); - EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage5V().to()); + EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage5V().value()); EXPECT_EQ(kTestVoltage, RobotController::GetVoltage5V()); RoboRioSim::SetUserCurrent5V(units::ampere_t{kTestCurrent}); EXPECT_TRUE(currentCallback.WasTriggered()); EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue()); - EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent5V().to()); + EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent5V().value()); EXPECT_EQ(kTestCurrent, RobotController::GetCurrent5V()); RoboRioSim::SetUserActive5V(false); @@ -185,13 +185,13 @@ TEST(RoboRioSimTest, Set3V3) { RoboRioSim::SetUserVoltage3V3(units::volt_t{kTestVoltage}); EXPECT_TRUE(voltageCallback.WasTriggered()); EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue()); - EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage3V3().to()); + EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage3V3().value()); EXPECT_EQ(kTestVoltage, RobotController::GetVoltage3V3()); RoboRioSim::SetUserCurrent3V3(units::ampere_t{kTestCurrent}); EXPECT_TRUE(currentCallback.WasTriggered()); EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue()); - EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent3V3().to()); + EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent3V3().value()); EXPECT_EQ(kTestCurrent, RobotController::GetCurrent3V3()); RoboRioSim::SetUserActive3V3(false); diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 7046a5a785..03df12b8fa 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -18,5 +18,5 @@ TEST(SingleJointedArmTest, Disabled) { } // The arm should swing down. - EXPECT_NEAR(sim.GetAngle().to(), -wpi::numbers::pi / 2, 0.01); + EXPECT_NEAR(sim.GetAngle().value(), -wpi::numbers::pi / 2, 0.01); } diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp index 869d445e39..e7e04055ff 100644 --- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp @@ -51,7 +51,7 @@ TEST(StateSpaceSimTest, FlywheelSim) { sim.SetInput(Eigen::Vector{ motor.Get() * frc::RobotController::GetInputVoltage()}); sim.Update(20_ms); - encoderSim.SetRate(sim.GetAngularVelocity().to()); + encoderSim.SetRate(sim.GetAngularVelocity().value()); } ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1); diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp index c2ccdb121f..8618aaaa0f 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp @@ -16,7 +16,7 @@ ArmSubsystem::ArmSubsystem() m_motor(kMotorPort), m_encoder(kEncoderPorts[0], kEncoderPorts[1]), m_feedforward(kS, kCos, kV, kA) { - m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to()); + m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value()); // Start arm in neutral position SetGoal(State{kArmOffset, 0_rad_per_s}); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp index f4b62d0695..f80970aa16 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp @@ -23,5 +23,5 @@ void ArmSubsystem::UseState(State setpoint) { m_feedforward.Calculate(setpoint.position, setpoint.velocity); // Add the feedforward to the PID output to get the motor output m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, - setpoint.position.to(), feedforward / 12_V); + setpoint.position.value(), feedforward / 12_V); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index b97e051939..b8b8368274 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -94,7 +94,7 @@ class Robot : public frc::TimedRobot { // Finally, we set our simulated encoder's readings and simulated battery // voltage - m_encoderSim.SetDistance(m_armSim.GetAngle().to()); + m_encoderSim.SetDistance(m_armSim.GetAngle().value()); // SimBattery estimates loaded battery voltages frc::sim::RoboRioSim::SetVInVoltage( frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()})); @@ -108,7 +108,7 @@ class Robot : public frc::TimedRobot { // Here, we run PID control like normal, with a constant setpoint of 75 // degrees. double pidOutput = m_controller.Calculate( - m_encoder.GetDistance(), (units::radian_t(75_deg)).to()); + m_encoder.GetDistance(), (units::radian_t(75_deg)).value()); m_motor.SetVoltage(units::volt_t(pidOutput)); } else { // Otherwise, we disable the motor. diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp index a213ab5db5..da638d5b1d 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp @@ -8,9 +8,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { const auto leftFeedforward = m_feedforward.Calculate(speeds.left); const auto rightFeedforward = m_feedforward.Calculate(speeds.right); const double leftOutput = m_leftPIDController.Calculate( - m_leftEncoder.GetRate(), speeds.left.to()); + m_leftEncoder.GetRate(), speeds.left.value()); const double rightOutput = m_rightPIDController.Calculate( - m_rightEncoder.GetRate(), speeds.right.to()); + m_rightEncoder.GetRate(), speeds.right.value()); m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp index f9197f0c68..1ed0e46768 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp @@ -12,9 +12,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { const auto leftFeedforward = m_feedforward.Calculate(speeds.left); const auto rightFeedforward = m_feedforward.Calculate(speeds.right); const double leftOutput = m_leftPIDController.Calculate( - m_leftEncoder.GetRate(), speeds.left.to()); + m_leftEncoder.GetRate(), speeds.left.value()); const double rightOutput = m_rightPIDController.Calculate( - m_rightEncoder.GetRate(), speeds.right.to()); + m_rightEncoder.GetRate(), speeds.right.value()); m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index 2691bc530f..dfb57a6155 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -34,9 +34,9 @@ class Drivetrain { // distance traveled for one rotation of the wheel divided by the encoder // resolution. m_leftEncoder.SetDistancePerPulse( - 2 * wpi::numbers::pi * kWheelRadius.to() / kEncoderResolution); + 2 * wpi::numbers::pi * kWheelRadius.value() / kEncoderResolution); m_rightEncoder.SetDistancePerPulse( - 2 * wpi::numbers::pi * kWheelRadius.to() / kEncoderResolution); + 2 * wpi::numbers::pi * kWheelRadius.value() / kEncoderResolution); m_leftEncoder.Reset(); m_rightEncoder.Reset(); diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp index 55ebc9ab9b..270124bb22 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp @@ -37,10 +37,10 @@ void DriveSubsystem::SetDriveStates( frc::TrapezoidProfile::State left, frc::TrapezoidProfile::State right) { m_leftLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, - left.position.to(), + left.position.value(), m_feedforward.Calculate(left.velocity) / 12_V); m_rightLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, - right.position.to(), + right.position.value(), m_feedforward.Calculate(right.velocity) / 12_V); } diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp index 294f019910..a9343d8aea 100644 --- a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp @@ -31,7 +31,7 @@ class Robot : public frc::TimedRobot { frc::SmartDashboard::PutBoolean("Connected", connected); frc::SmartDashboard::PutNumber("Frequency", frequency); - frc::SmartDashboard::PutNumber("Output", output.to()); + frc::SmartDashboard::PutNumber("Output", output.value()); frc::SmartDashboard::PutNumber("Distance", distance); } }; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index 856bd38f7e..4b2891defa 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -46,7 +46,7 @@ class Robot : public frc::TimedRobot { // distance per pulse = (distance per revolution) / (pulses per revolution) // = (Pi * D) / ppr static constexpr double kArmEncoderDistPerPulse = - 2.0 * wpi::numbers::pi * kElevatorDrumRadius.to() / 4096.0; + 2.0 * wpi::numbers::pi * kElevatorDrumRadius.value() / 4096.0; // This gearbox represents a gearbox containing 4 Vex 775pro motors. frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4); @@ -73,7 +73,7 @@ class Robot : public frc::TimedRobot { m_mech2d.GetRoot("Elevator Root", 10, 0); frc::MechanismLigament2d* m_elevatorMech2d = m_elevatorRoot->Append( - "Elevator", units::inch_t(m_elevatorSim.GetPosition()).to(), + "Elevator", units::inch_t(m_elevatorSim.GetPosition()).value(), 90_deg); public: @@ -95,21 +95,21 @@ class Robot : public frc::TimedRobot { // Finally, we set our simulated encoder's readings and simulated battery // voltage - m_encoderSim.SetDistance(m_elevatorSim.GetPosition().to()); + m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value()); // SimBattery estimates loaded battery voltages frc::sim::RoboRioSim::SetVInVoltage( frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()})); // Update the Elevator length based on the simulated elevator height m_elevatorMech2d->SetLength( - units::inch_t(m_elevatorSim.GetPosition()).to()); + units::inch_t(m_elevatorSim.GetPosition()).value()); } void TeleopPeriodic() override { if (m_joystick.GetTrigger()) { // Here, we run PID control like normal, with a constant setpoint of 30in. - double pidOutput = m_controller.Calculate( - m_encoder.GetDistance(), units::meter_t(30_in).to()); + double pidOutput = m_controller.Calculate(m_encoder.GetDistance(), + units::meter_t(30_in).value()); m_motor.SetVoltage(units::volt_t(pidOutput)); } else { // Otherwise, we disable the motor. diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp index d63d84e48d..d62918578f 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp @@ -43,7 +43,7 @@ class Robot : public frc::TimedRobot { // Send setpoint to offboard controller PID m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, - m_setpoint.position.to(), + m_setpoint.position.value(), m_feedforward.Calculate(m_setpoint.velocity) / 12_V); } diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp index d225e52898..c386ee7eb8 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp @@ -16,9 +16,9 @@ ShooterSubsystem::ShooterSubsystem() m_feederMotor(kFeederMotorPort), m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]), m_shooterFeedforward(kS, kV) { - m_controller.SetTolerance(kShooterToleranceRPS.to()); + m_controller.SetTolerance(kShooterToleranceRPS.value()); m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); - SetSetpoint(kShooterTargetRPS.to()); + SetSetpoint(kShooterTargetRPS.value()); } void ShooterSubsystem::UseOutput(double output, double setpoint) { diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp index fa0a6659e3..f3d41a27af 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp @@ -25,9 +25,9 @@ Drivetrain::Drivetrain() { m_rightEncoder.SetDistancePerPulse(0.042); #else // Circumference = diameter * pi. 360 tick simulated encoders. - m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.to() * + m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() * wpi::numbers::pi / 360.0); - m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.to() * + m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() * wpi::numbers::pi / 360.0); #endif SetName("Drivetrain"); diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp index 7d11e04bd3..681bb3933b 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp @@ -12,9 +12,9 @@ TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive) : CommandHelper( frc2::PIDController(kTurnP, kTurnI, kTurnD), // Close loop on heading - [drive] { return drive->GetHeading().to(); }, + [drive] { return drive->GetHeading().value(); }, // Set reference to target - target.to(), + target.value(), // Pipe output to turn robot [drive](double output) { drive->ArcadeDrive(0, output); }, // Require the drive @@ -24,8 +24,7 @@ TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive) // 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(kTurnTolerance.to(), - kTurnRateTolerance.to()); + m_controller.SetTolerance(kTurnTolerance.value(), kTurnRateTolerance.value()); AddRequirements({drive}); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index 81bae47738..c33450d0cd 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -22,13 +22,13 @@ void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) { m_feedforward.Calculate(wheelSpeeds.rearRight); const double frontLeftOutput = m_frontLeftPIDController.Calculate( - m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to()); + m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.value()); const double frontRightOutput = m_frontRightPIDController.Calculate( - m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to()); + m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.value()); const double backLeftOutput = m_backLeftPIDController.Calculate( - m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to()); + m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.value()); const double backRightOutput = m_backRightPIDController.Calculate( - m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to()); + m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.value()); m_frontLeftMotor.SetVoltage(units::volt_t{frontLeftOutput} + frontLeftFeedforward); diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp index 7d2fa3486a..149f12e3fa 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -18,15 +18,14 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const { void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) { std::function - calcAndSetSpeeds = - [&m_feedforward = m_feedforward](units::meters_per_second_t speed, - const auto& encoder, - auto& controller, auto& motor) { - auto feedforward = m_feedforward.Calculate(speed); - double output = - controller.Calculate(encoder.GetRate(), speed.to()); - motor.SetVoltage(units::volt_t{output} + feedforward); - }; + calcAndSetSpeeds = [&m_feedforward = m_feedforward]( + units::meters_per_second_t speed, + const auto& encoder, auto& controller, + auto& motor) { + auto feedforward = m_feedforward.Calculate(speed); + double output = controller.Calculate(encoder.GetRate(), speed.value()); + motor.SetVoltage(units::volt_t{output} + feedforward); + }; calcAndSetSpeeds(wheelSpeeds.frontLeft, m_frontLeftEncoder, m_frontLeftPIDController, m_frontLeftMotor); diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp index 7a6b2bd457..1871688255 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp @@ -8,9 +8,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { const auto leftFeedforward = m_feedforward.Calculate(speeds.left); const auto rightFeedforward = m_feedforward.Calculate(speeds.right); const double leftOutput = m_leftPIDController.Calculate( - m_leftEncoder.GetRate(), speeds.left.to()); + m_leftEncoder.GetRate(), speeds.left.value()); const double rightOutput = m_rightPIDController.Calculate( - m_rightEncoder.GetRate(), speeds.right.to()); + m_rightEncoder.GetRate(), speeds.right.value()); m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp index 822b52c4e5..13ec2d847b 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp @@ -20,10 +20,10 @@ Drivetrain::Drivetrain() { // gearbox is constructed, you might have to invert the left side instead. m_rightMotor.SetInverted(true); - m_leftEncoder.SetDistancePerPulse( - wpi::numbers::pi * kWheelDiameter.to() / kCountsPerRevolution); - m_rightEncoder.SetDistancePerPulse( - wpi::numbers::pi * kWheelDiameter.to() / kCountsPerRevolution); + m_leftEncoder.SetDistancePerPulse(wpi::numbers::pi * kWheelDiameter.value() / + kCountsPerRevolution); + m_rightEncoder.SetDistancePerPulse(wpi::numbers::pi * kWheelDiameter.value() / + kCountsPerRevolution); ResetEncoders(); } diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp index c896ba0b12..05d99f37f7 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp @@ -10,9 +10,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { auto leftFeedforward = m_feedforward.Calculate(speeds.left); auto rightFeedforward = m_feedforward.Calculate(speeds.right); double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(), - speeds.left.to()); - double rightOutput = m_rightPIDController.Calculate( - m_rightEncoder.GetRate(), speeds.right.to()); + speeds.left.value()); + double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(), + speeds.right.value()); m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); @@ -47,16 +47,12 @@ void Drivetrain::SimulationPeriodic() { frc::RobotController::GetInputVoltage()); m_drivetrainSimulator.Update(20_ms); - m_leftEncoderSim.SetDistance( - m_drivetrainSimulator.GetLeftPosition().to()); - m_leftEncoderSim.SetRate( - m_drivetrainSimulator.GetLeftVelocity().to()); + m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value()); + m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value()); m_rightEncoderSim.SetDistance( - m_drivetrainSimulator.GetRightPosition().to()); - m_rightEncoderSim.SetRate( - m_drivetrainSimulator.GetRightVelocity().to()); - m_gyroSim.SetAngle( - -m_drivetrainSimulator.GetHeading().Degrees().to()); + m_drivetrainSimulator.GetRightPosition().value()); + m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value()); + m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value()); } void Drivetrain::Periodic() { diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 71233a30a8..0f03d37b4e 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -122,9 +122,9 @@ class Robot : public frc::TimedRobot { m_lastProfiledReference)) .Calculate(20_ms); - m_loop.SetNextR(Eigen::Vector{ - m_lastProfiledReference.position.to(), - m_lastProfiledReference.velocity.to()}); + m_loop.SetNextR( + Eigen::Vector{m_lastProfiledReference.position.value(), + m_lastProfiledReference.velocity.value()}); // Correct our Kalman filter's state vector estimate with encoder data. m_loop.Correct(Eigen::Vector{m_encoder.GetDistance()}); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp index f529a4c428..a94b860d71 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp @@ -46,14 +46,11 @@ void DriveSubsystem::SimulationPeriodic() { frc::RobotController::GetInputVoltage()); m_drivetrainSimulator.Update(20_ms); - m_leftEncoderSim.SetDistance( - m_drivetrainSimulator.GetLeftPosition().to()); - m_leftEncoderSim.SetRate( - m_drivetrainSimulator.GetLeftVelocity().to()); + m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value()); + m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value()); m_rightEncoderSim.SetDistance( - m_drivetrainSimulator.GetRightPosition().to()); - m_rightEncoderSim.SetRate( - m_drivetrainSimulator.GetRightVelocity().to()); + m_drivetrainSimulator.GetRightPosition().value()); + m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value()); m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees()); } diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h index 9285812997..307d40846d 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h @@ -43,7 +43,7 @@ constexpr int kEncoderCPR = 1024; constexpr auto kWheelDiameter = 6_in; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameter.to() * wpi::numbers::pi) / + (kWheelDiameter.value() * wpi::numbers::pi) / static_cast(kEncoderCPR); // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index 70a3ac6bb8..4c9d65e2f7 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -90,8 +90,8 @@ class Robot : public frc::TimedRobot { public: void RobotInit() override { // Circumference = pi * d, so distance per click = pi * d / counts - m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi * - kDrumRadius.to() / 4096.0); + m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi * kDrumRadius.value() / + 4096.0); } void TeleopInit() override { @@ -119,9 +119,9 @@ class Robot : public frc::TimedRobot { m_lastProfiledReference)) .Calculate(20_ms); - m_loop.SetNextR(Eigen::Vector{ - m_lastProfiledReference.position.to(), - m_lastProfiledReference.velocity.to()}); + m_loop.SetNextR( + Eigen::Vector{m_lastProfiledReference.position.value(), + m_lastProfiledReference.velocity.value()}); // Correct our Kalman filter's state vector estimate with encoder data. m_loop.Correct(Eigen::Vector{m_encoder.GetDistance()}); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp index 1e9e6851a1..636916f41f 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp @@ -93,7 +93,7 @@ class Robot : public frc::TimedRobot { // setpoint of a PID controller. if (m_joystick.GetRightBumper()) { // We pressed the bumper, so let's set our next reference - m_loop.SetNextR(Eigen::Vector{kSpinup.to()}); + m_loop.SetNextR(Eigen::Vector{kSpinup.value()}); } else { // We released the bumper, so let's spin down m_loop.SetNextR(Eigen::Vector{0.0}); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index 32f6ceca28..69d1953d4f 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -94,7 +94,7 @@ class Robot : public frc::TimedRobot { // setpoint of a PID controller. if (m_joystick.GetRightBumper()) { // We pressed the bumper, so let's set our next reference - m_loop.SetNextR(Eigen::Vector{kSpinup.to()}); + m_loop.SetNextR(Eigen::Vector{kSpinup.value()}); } else { // We released the bumper, so let's spin down m_loop.SetNextR(Eigen::Vector{0.0}); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp index 33397ebd77..24a6527363 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp @@ -48,7 +48,7 @@ void SwerveModule::SetDesiredState( // Calculate the drive output from the drive PID controller. const auto driveOutput = m_drivePIDController.Calculate( - m_driveEncoder.GetRate(), state.speed.to()); + m_driveEncoder.GetRate(), state.speed.value()); const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp index f75e475b9f..4d20ec86bc 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp @@ -51,7 +51,7 @@ void SwerveModule::SetDesiredState( // Calculate the drive output from the drive PID controller. const auto driveOutput = m_drivePIDController.Calculate( - m_driveEncoder.GetRate(), state.speed.to()); + m_driveEncoder.GetRate(), state.speed.value()); // Calculate the turning motor output from the turning PID controller. auto turnOutput = m_turningPIDController.Calculate( diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp index 3091da8c9e..968ccad079 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp @@ -20,8 +20,8 @@ SwerveModule::SwerveModule(const int driveMotorChannel, // Set the distance per pulse for the drive encoder. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution. - m_driveEncoder.SetDistancePerPulse( - 2 * wpi::numbers::pi * kWheelRadius.to() / kEncoderResolution); + m_driveEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * + kWheelRadius.value() / kEncoderResolution); // Set the distance (in this case, angle) per pulse for the turning encoder. // This is the the angle through an entire rotation (2 * wpi::numbers::pi) @@ -48,7 +48,7 @@ void SwerveModule::SetDesiredState( // Calculate the drive output from the drive PID controller. const auto driveOutput = m_drivePIDController.Calculate( - m_driveEncoder.GetRate(), state.speed.to()); + m_driveEncoder.GetRate(), state.speed.value()); const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed); diff --git a/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp index d1872e9ee0..e230310a5b 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp @@ -170,9 +170,9 @@ TEST_F(DIOLoopTest, SynchronousInterruptWorks) { frc::Timer timer; timer.Start(); interrupt.WaitForInterrupt(kSynchronousInterruptTime + 1_s); - auto time = timer.Get().to(); + auto time = timer.Get().value(); if (thr.joinable()) thr.join(); - EXPECT_NEAR(kSynchronousInterruptTime.to(), time, - kSynchronousInterruptTimeTolerance.to()); + EXPECT_NEAR(kSynchronousInterruptTime.value(), time, + kSynchronousInterruptTimeTolerance.value()); } diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index 869a2bb71a..8f1145d96d 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -7,14 +7,14 @@ namespace frc { Eigen::Vector PoseTo3dVector(const Pose2d& pose) { - return Eigen::Vector{pose.Translation().X().to(), - pose.Translation().Y().to(), - pose.Rotation().Radians().to()}; + return Eigen::Vector{pose.Translation().X().value(), + pose.Translation().Y().value(), + pose.Rotation().Radians().value()}; } Eigen::Vector PoseTo4dVector(const Pose2d& pose) { - return Eigen::Vector{pose.Translation().X().to(), - pose.Translation().Y().to(), + return Eigen::Vector{pose.Translation().X().value(), + pose.Translation().Y().value(), pose.Rotation().Cos(), pose.Rotation().Sin()}; } @@ -31,8 +31,8 @@ bool IsStabilizable<2, 1>(const Eigen::Matrix& A, } Eigen::Vector PoseToVector(const Pose2d& pose) { - return Eigen::Vector{pose.X().to(), pose.Y().to(), - pose.Rotation().Radians().to()}; + return Eigen::Vector{pose.X().value(), pose.Y().value(), + pose.Rotation().Radians().value()}; } } // namespace frc diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp index 71e5b42ed9..23b22cd1a5 100644 --- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp @@ -56,10 +56,10 @@ ChassisSpeeds HolonomicDriveController::Calculate( } // Calculate feedback velocities (based on position error). - auto xFeedback = units::meters_per_second_t(m_xController.Calculate( - currentPose.X().to(), poseRef.X().to())); - auto yFeedback = units::meters_per_second_t(m_yController.Calculate( - currentPose.Y().to(), poseRef.Y().to())); + auto xFeedback = units::meters_per_second_t( + m_xController.Calculate(currentPose.X().value(), poseRef.X().value())); + auto yFeedback = units::meters_per_second_t( + m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value())); // Return next output. return ChassisSpeeds::FromFieldRelativeSpeeds( diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp index bfcea7d7b5..34af2aa97a 100644 --- a/wpimath/src/main/native/cpp/controller/PIDController.cpp +++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp @@ -21,7 +21,7 @@ PIDController::PIDController(double Kp, double Ki, double Kd, if (period <= 0_s) { wpi::math::MathSharedStore::ReportError( "Controller period must be a non-zero positive number, got {}!", - period.to()); + period.value()); m_period = 20_ms; wpi::math::MathSharedStore::ReportWarning( "{}", "Controller period defaulted to 20ms."); @@ -86,7 +86,7 @@ bool PIDController::AtSetpoint() const { positionError = m_setpoint - m_measurement; } - double velocityError = (positionError - m_prevError) / m_period.to(); + double velocityError = (positionError - m_prevError) / m_period.value(); return std::abs(positionError) < m_positionTolerance && std::abs(velocityError) < m_velocityTolerance; @@ -139,11 +139,11 @@ double PIDController::Calculate(double measurement) { m_positionError = m_setpoint - measurement; } - m_velocityError = (m_positionError - m_prevError) / m_period.to(); + m_velocityError = (m_positionError - m_prevError) / m_period.value(); if (m_Ki != 0) { m_totalError = - std::clamp(m_totalError + m_positionError * m_period.to(), + std::clamp(m_totalError + m_positionError * m_period.value(), m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki); } diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp index 22469a577e..8aa16d8311 100644 --- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp +++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp @@ -51,11 +51,11 @@ ChassisSpeeds RamseteController::Calculate( m_poseError = poseRef.RelativeTo(currentPose); // Aliases for equation readability - double eX = m_poseError.X().to(); - double eY = m_poseError.Y().to(); - double eTheta = m_poseError.Rotation().Radians().to(); - double vRef = linearVelocityRef.to(); - double omegaRef = angularVelocityRef.to(); + double eX = m_poseError.X().value(); + double eY = m_poseError.Y().value(); + double eTheta = m_poseError.Rotation().Radians().value(); + double vRef = linearVelocityRef.value(); + double omegaRef = angularVelocityRef.value(); double k = 2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2)); diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index a82384b13d..c5ed7a13f7 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -96,14 +96,12 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime( auto omega = (gyroAngle - m_previousAngle).Radians() / dt; auto u = Eigen::Vector{ - (wheelSpeeds.left + wheelSpeeds.right).to() / 2.0, 0.0, - omega.to()}; + (wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()}; m_previousAngle = angle; - auto localY = Eigen::Vector{leftDistance.to(), - rightDistance.to(), - angle.Radians().to()}; + auto localY = Eigen::Vector{ + leftDistance.value(), rightDistance.value(), angle.Radians().value()}; m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); m_observer.Predict(u, dt); @@ -140,8 +138,8 @@ wpi::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( Eigen::Vector DifferentialDrivePoseEstimator::FillStateVector( const Pose2d& pose, units::meter_t leftDistance, units::meter_t rightDistance) { - return Eigen::Vector{ - pose.Translation().X().to(), pose.Translation().Y().to(), - pose.Rotation().Radians().to(), leftDistance.to(), - rightDistance.to()}; + return Eigen::Vector{pose.Translation().X().value(), + pose.Translation().Y().value(), + pose.Rotation().Radians().value(), + leftDistance.value(), rightDistance.value()}; } diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 0d1413d9e0..9d936476a6 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -100,11 +100,11 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s) .RotateBy(angle); - Eigen::Vector u{fieldRelativeVelocities.X().to(), - fieldRelativeVelocities.Y().to(), - omega.to()}; + Eigen::Vector u{fieldRelativeVelocities.X().value(), + fieldRelativeVelocities.Y().value(), + omega.value()}; - Eigen::Vector localY{angle.Radians().template to()}; + Eigen::Vector localY{angle.Radians().value()}; m_previousAngle = angle; m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index d7e57c039d..b7176cd4a4 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -46,7 +46,7 @@ Pose2d Pose2d::RelativeTo(const Pose2d& other) const { Pose2d Pose2d::Exp(const Twist2d& twist) const { const auto dx = twist.dx; const auto dy = twist.dy; - const auto dtheta = twist.dtheta.to(); + const auto dtheta = twist.dtheta.value(); const auto sinTheta = std::sin(dtheta); const auto cosTheta = std::cos(dtheta); @@ -68,7 +68,7 @@ Pose2d Pose2d::Exp(const Twist2d& twist) const { Twist2d Pose2d::Log(const Pose2d& end) const { const auto transform = end.RelativeTo(*this); - const auto dtheta = transform.Rotation().Radians().to(); + const auto dtheta = transform.Rotation().Radians().value(); const auto halfDtheta = dtheta / 2.0; const auto cosMinusOne = transform.Rotation().Cos() - 1; diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index 37b86f2dc6..27af5ed25a 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -64,7 +64,7 @@ Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const { } void frc::to_json(wpi::json& json, const Rotation2d& rotation) { - json = wpi::json{{"radians", rotation.Radians().to()}}; + json = wpi::json{{"radians", rotation.Radians().value()}}; } void frc::from_json(const wpi::json& json, Rotation2d& rotation) { diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 5e066bb40b..5a30ec2b81 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -59,8 +59,8 @@ bool Translation2d::operator!=(const Translation2d& other) const { } void frc::to_json(wpi::json& json, const Translation2d& translation) { - json = wpi::json{{"x", translation.X().to()}, - {"y", translation.Y().to()}}; + json = + wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}}; } void frc::from_json(const wpi::json& json, Translation2d& translation) { diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index dddd7da88a..3efddd4532 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -50,13 +50,13 @@ std::vector GetElementsFromTrajectory( elements.reserve(trajectory.States().size() * 7); for (auto&& state : trajectory.States()) { - elements.push_back(state.t.to()); - elements.push_back(state.velocity.to()); - elements.push_back(state.acceleration.to()); - elements.push_back(state.pose.X().to()); - elements.push_back(state.pose.Y().to()); - elements.push_back(state.pose.Rotation().Radians().to()); - elements.push_back(state.curvature.to()); + elements.push_back(state.t.value()); + elements.push_back(state.velocity.value()); + elements.push_back(state.acceleration.value()); + elements.push_back(state.pose.X().value()); + elements.push_back(state.pose.Y().value()); + elements.push_back(state.pose.Rotation().Radians().value()); + elements.push_back(state.curvature.value()); } return elements; diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index 7f2f75daf4..c4a71fdf24 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -21,9 +21,9 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds( m_previousCoR = centerOfRotation; } - Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.to(), - chassisSpeeds.vy.to(), - chassisSpeeds.omega.to()}; + Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(), + chassisSpeeds.vy.value(), + chassisSpeeds.omega.value()}; Eigen::Vector wheelsVector = m_inverseKinematics * chassisSpeedsVector; @@ -39,8 +39,8 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds( ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds( const MecanumDriveWheelSpeeds& wheelSpeeds) const { Eigen::Vector wheelSpeedsVector{ - wheelSpeeds.frontLeft.to(), wheelSpeeds.frontRight.to(), - wheelSpeeds.rearLeft.to(), wheelSpeeds.rearRight.to()}; + wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(), + wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()}; Eigen::Vector3d chassisSpeedsVector = m_forwardKinematics.solve(wheelSpeedsVector); @@ -54,9 +54,9 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) const { - m_inverseKinematics = Eigen::Matrix{ - {1, -1, (-(fl.X() + fl.Y())).template to()}, - {1, 1, (fr.X() - fr.Y()).template to()}, - {1, 1, (rl.X() - rl.Y()).template to()}, - {1, -1, (-(rr.X() + rr.Y())).template to()}}; + m_inverseKinematics = + Eigen::Matrix{{1, -1, (-(fl.X() + fl.Y())).value()}, + {1, 1, (fr.X() - fr.Y()).value()}, + {1, 1, (rl.X() - rl.Y()).value()}, + {1, -1, (-(rr.X() + rr.Y())).value()}}; } diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp index c35c58d83b..cec620ce7a 100644 --- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp +++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp @@ -52,29 +52,27 @@ std::vector SplineHelper::CubicSplinesFromControlVectors( c.emplace_back(0); // populate rhs vectors - dx.emplace_back( - 3 * (waypoints[2].X().to() - waypoints[0].X().to()) - - xInitial[1]); - dy.emplace_back( - 3 * (waypoints[2].Y().to() - waypoints[0].Y().to()) - - yInitial[1]); + dx.emplace_back(3 * (waypoints[2].X().value() - waypoints[0].X().value()) - + xInitial[1]); + dy.emplace_back(3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) - + yInitial[1]); if (waypoints.size() > 4) { for (size_t i = 1; i <= waypoints.size() - 4; ++i) { // dx and dy represent the derivatives of the internal waypoints. The // derivative of the second internal waypoint should involve the third // and first internal waypoint, which have indices of 1 and 3 in the // waypoints list (which contains ALL waypoints). - dx.emplace_back(3 * (waypoints[i + 2].X().to() - - waypoints[i].X().to())); - dy.emplace_back(3 * (waypoints[i + 2].Y().to() - - waypoints[i].Y().to())); + dx.emplace_back( + 3 * (waypoints[i + 2].X().value() - waypoints[i].X().value())); + dy.emplace_back( + 3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value())); } } - dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to() - - waypoints[waypoints.size() - 3].X().to()) - + dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() - + waypoints[waypoints.size() - 3].X().value()) - xFinal[1]); - dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to() - - waypoints[waypoints.size() - 3].Y().to()) - + dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() - + waypoints[waypoints.size() - 3].Y().value()) - yFinal[1]); // Compute solution to tridiagonal system @@ -89,10 +87,10 @@ std::vector SplineHelper::CubicSplinesFromControlVectors( for (size_t i = 0; i < fx.size() - 1; ++i) { // Create the spline. const CubicHermiteSpline spline{ - {waypoints[i].X().to(), fx[i]}, - {waypoints[i + 1].X().to(), fx[i + 1]}, - {waypoints[i].Y().to(), fy[i]}, - {waypoints[i + 1].Y().to(), fy[i + 1]}}; + {waypoints[i].X().value(), fx[i]}, + {waypoints[i + 1].X().value(), fx[i + 1]}, + {waypoints[i].Y().value(), fy[i]}, + {waypoints[i + 1].Y().value(), fy[i + 1]}}; splines.push_back(spline); } @@ -102,10 +100,8 @@ std::vector SplineHelper::CubicSplinesFromControlVectors( const double yDeriv = (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0; - wpi::array midXControlVector{waypoints[0].X().to(), - xDeriv}; - wpi::array midYControlVector{waypoints[0].Y().to(), - yDeriv}; + wpi::array midXControlVector{waypoints[0].X().value(), xDeriv}; + wpi::array midYControlVector{waypoints[0].Y().value(), yDeriv}; splines.emplace_back(xInitial, midXControlVector, yInitial, midYControlVector); @@ -140,16 +136,14 @@ SplineHelper::CubicControlVectorsFromWaypoints( const Pose2d& end) { double scalar; if (interiorWaypoints.empty()) { - scalar = 1.2 * start.Translation().Distance(end.Translation()).to(); + scalar = 1.2 * start.Translation().Distance(end.Translation()).value(); } else { scalar = - 1.2 * - start.Translation().Distance(interiorWaypoints.front()).to(); + 1.2 * start.Translation().Distance(interiorWaypoints.front()).value(); } const auto initialCV = CubicControlVector(scalar, start); if (!interiorWaypoints.empty()) { - scalar = - 1.2 * end.Translation().Distance(interiorWaypoints.back()).to(); + scalar = 1.2 * end.Translation().Distance(interiorWaypoints.back()).value(); } const auto finalCV = CubicControlVector(scalar, end); return {initialCV, finalCV}; @@ -165,7 +159,7 @@ std::vector SplineHelper::QuinticSplinesFromWaypoints( // This just makes the splines look better. const auto scalar = - 1.2 * p0.Translation().Distance(p1.Translation()).to(); + 1.2 * p0.Translation().Distance(p1.Translation()).value(); auto controlVectorA = QuinticControlVector(scalar, p0); auto controlVectorB = QuinticControlVector(scalar, p1); diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp index ec2eff011f..db419f7849 100644 --- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp +++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp @@ -146,11 +146,11 @@ Trajectory Trajectory::operator+(const Trajectory& other) const { } void frc::to_json(wpi::json& json, const Trajectory::State& state) { - json = wpi::json{{"time", state.t.to()}, - {"velocity", state.velocity.to()}, - {"acceleration", state.acceleration.to()}, + json = wpi::json{{"time", state.t.value()}, + {"velocity", state.velocity.value()}, + {"acceleration", state.acceleration.value()}, {"pose", state.pose}, - {"curvature", state.curvature.to()}}; + {"curvature", state.curvature.value()}}; } void frc::from_json(const wpi::json& json, Trajectory::State& state) { diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp index 18fcd78090..d397d0ce64 100644 --- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp +++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp @@ -85,7 +85,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( // Now enforce all acceleration limits. EnforceAccelerationLimits(reversed, constraints, &constrainedState); - if (ds.to() < kEpsilon) { + if (ds.value() < kEpsilon) { break; } @@ -141,7 +141,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( // Check all acceleration constraints with the new max velocity. EnforceAccelerationLimits(reversed, constraints, &constrainedState); - if (ds.to() > -kEpsilon) { + if (ds.value() > -kEpsilon) { break; } diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h index 8646084c45..dac66ce7c9 100644 --- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h @@ -166,7 +166,7 @@ class ControlAffinePlantInversionFeedforward { Eigen::Vector Calculate( const Eigen::Vector& r, const Eigen::Vector& nextR) { - Eigen::Vector rDot = (nextR - r) / m_dt.to(); + Eigen::Vector rDot = (nextR - r) / m_dt.value(); m_uff = m_B.householderQr().solve( rDot - m_f(r, Eigen::Vector::Zero())); diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h index 9fc75e7f23..e0b10c761b 100644 --- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -193,8 +193,8 @@ class ProfiledPIDController * @param maximumInput The maximum value expected from the input. */ void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) { - m_controller.EnableContinuousInput(minimumInput.template to(), - maximumInput.template to()); + m_controller.EnableContinuousInput(minimumInput.value(), + maximumInput.value()); m_minimumInput = minimumInput; m_maximumInput = maximumInput; } @@ -227,8 +227,8 @@ class ProfiledPIDController void SetTolerance( Distance_t positionTolerance, Velocity_t velocityTolerance = std::numeric_limits::infinity()) { - m_controller.SetTolerance(positionTolerance.template to(), - velocityTolerance.template to()); + m_controller.SetTolerance(positionTolerance.value(), + velocityTolerance.value()); } /** @@ -273,8 +273,8 @@ class ProfiledPIDController frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint}; m_setpoint = profile.Calculate(GetPeriod()); - return m_controller.Calculate(measurement.template to(), - m_setpoint.position.template to()); + return m_controller.Calculate(measurement.value(), + m_setpoint.position.value()); } /** @@ -351,7 +351,7 @@ class ProfiledPIDController builder.AddDoubleProperty( "d", [this] { return GetD(); }, [this](double value) { SetD(value); }); builder.AddDoubleProperty( - "goal", [this] { return GetGoal().position.template to(); }, + "goal", [this] { return GetGoal().position.value(); }, [this](double value) { SetGoal(Distance_t{value}); }); } diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index a00d343081..df1a52cad7 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -70,10 +70,10 @@ class SimpleMotorFeedforward { auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; - Eigen::Vector r{currentVelocity.template to()}; - Eigen::Vector nextR{nextVelocity.template to()}; + Eigen::Vector r{currentVelocity.value()}; + Eigen::Vector nextR{nextVelocity.value()}; - return kS * wpi::sgn(currentVelocity.template to()) + + return kS * wpi::sgn(currentVelocity.value()) + units::volt_t{feedforward.Calculate(r, nextR)(0)}; } diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h index 30d2b41ae3..16cccbcab9 100644 --- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h +++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h @@ -25,7 +25,7 @@ Eigen::Vector AngleResidual( const Eigen::Vector& b, int angleStateIdx) { Eigen::Vector ret = a - b; ret[angleStateIdx] = - AngleModulus(units::radian_t{ret[angleStateIdx]}).to(); + AngleModulus(units::radian_t{ret[angleStateIdx]}).value(); return ret; } diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index e439622747..81260dfa86 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -269,11 +269,10 @@ class SwerveDrivePoseEstimator { Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s) .RotateBy(angle); - Eigen::Vector u{fieldRelativeSpeeds.X().template to(), - fieldRelativeSpeeds.Y().template to(), - omega.template to()}; + Eigen::Vector u{fieldRelativeSpeeds.X().value(), + fieldRelativeSpeeds.Y().value(), omega.value()}; - Eigen::Vector localY{angle.Radians().template to()}; + Eigen::Vector localY{angle.Radians().value()}; m_previousAngle = angle; m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index b6ea33a817..92d8bdcf53 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -125,7 +125,7 @@ class LinearFilter { */ static LinearFilter SinglePoleIIR(double timeConstant, units::second_t period) { - double gain = std::exp(-period.to() / timeConstant); + double gain = std::exp(-period.value() / timeConstant); return LinearFilter({1.0 - gain}, {-gain}); } @@ -144,7 +144,7 @@ class LinearFilter { * user. */ static LinearFilter HighPass(double timeConstant, units::second_t period) { - double gain = std::exp(-period.to() / timeConstant); + double gain = std::exp(-period.value() / timeConstant); return LinearFilter({gain, -gain}, {-gain}); } @@ -225,7 +225,7 @@ class LinearFilter { } Eigen::Vector a = - S.householderQr().solve(d) / std::pow(period.to(), Derivative); + S.householderQr().solve(d) / std::pow(period.value(), Derivative); // Reverse gains list std::vector gains; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 056f7c829d..84057ac363 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -65,8 +65,8 @@ class SwerveDriveKinematics { for (size_t i = 0; i < NumModules; i++) { // clang-format off m_inverseKinematics.template block<2, 3>(i * 2, 0) << - 1, 0, (-m_modules[i].Y()).template to(), - 0, 1, (+m_modules[i].X()).template to(); + 1, 0, (-m_modules[i].Y()).value(), + 0, 1, (+m_modules[i].X()).value(); // clang-format on } @@ -82,8 +82,8 @@ class SwerveDriveKinematics { for (size_t i = 0; i < NumModules; i++) { // clang-format off m_inverseKinematics.template block<2, 3>(i * 2, 0) << - 1, 0, (-m_modules[i].Y()).template to(), - 0, 1, (+m_modules[i].X()).template to(); + 1, 0, (-m_modules[i].Y()).value(), + 0, 1, (+m_modules[i].X()).value(); // clang-format on } diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index 8601adf097..1747453869 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -26,16 +26,16 @@ SwerveDriveKinematics::ToSwerveModuleStates( // clang-format off m_inverseKinematics.template block<2, 3>(i * 2, 0) = Eigen::Matrix{ - {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to()}, - {0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to()}}; + {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()}, + {0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}}; // clang-format on } m_previousCoR = centerOfRotation; } - Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.to(), - chassisSpeeds.vy.to(), - chassisSpeeds.omega.to()}; + Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(), + chassisSpeeds.vy.value(), + chassisSpeeds.omega.value()}; Eigen::Matrix moduleStatesMatrix = m_inverseKinematics * chassisSpeedsVector; @@ -46,7 +46,7 @@ SwerveDriveKinematics::ToSwerveModuleStates( units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)}; auto speed = units::math::hypot(x, y); - Rotation2d rotation{x.to(), y.to()}; + Rotation2d rotation{x.value(), y.value()}; moduleStates[i] = {speed, rotation}; } @@ -74,10 +74,9 @@ ChassisSpeeds SwerveDriveKinematics::ToChassisSpeeds( for (size_t i = 0; i < NumModules; ++i) { SwerveModuleState module = moduleStates[i]; - moduleStatesMatrix(i * 2, 0) = - module.speed.to() * module.angle.Cos(); + moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos(); moduleStatesMatrix(i * 2 + 1, 0) = - module.speed.to() * module.angle.Sin(); + module.speed.value() * module.angle.Sin(); } Eigen::Vector3d chassisSpeedsVector = diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h index 615dffd939..2dd248a528 100644 --- a/wpimath/src/main/native/include/frc/spline/Spline.h +++ b/wpimath/src/main/native/include/frc/spline/Spline.h @@ -109,8 +109,7 @@ class Spline { * @return The vector. */ static Eigen::Vector2d ToVector(const Translation2d& translation) { - return Eigen::Vector2d{translation.X().to(), - translation.Y().to()}; + return Eigen::Vector2d{translation.X().value(), translation.Y().value()}; } /** diff --git a/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/wpimath/src/main/native/include/frc/spline/SplineHelper.h index 008224e961..90b61078cf 100644 --- a/wpimath/src/main/native/include/frc/spline/SplineHelper.h +++ b/wpimath/src/main/native/include/frc/spline/SplineHelper.h @@ -80,14 +80,14 @@ class WPILIB_DLLEXPORT SplineHelper { private: static Spline<3>::ControlVector CubicControlVector(double scalar, const Pose2d& point) { - return {{point.X().to(), scalar * point.Rotation().Cos()}, - {point.Y().to(), scalar * point.Rotation().Sin()}}; + return {{point.X().value(), scalar * point.Rotation().Cos()}, + {point.Y().value(), scalar * point.Rotation().Sin()}}; } static Spline<5>::ControlVector QuinticControlVector(double scalar, const Pose2d& point) { - return {{point.X().to(), scalar * point.Rotation().Cos(), 0.0}, - {point.Y().to(), scalar * point.Rotation().Sin(), 0.0}}; + return {{point.X().value(), scalar * point.Rotation().Cos(), 0.0}, + {point.Y().value(), scalar * point.Rotation().Sin(), 0.0}}; } /** diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h index 09b75e530e..ff1dfb837d 100644 --- a/wpimath/src/main/native/include/frc/system/Discretization.h +++ b/wpimath/src/main/native/include/frc/system/Discretization.h @@ -21,7 +21,7 @@ template void DiscretizeA(const Eigen::Matrix& contA, units::second_t dt, Eigen::Matrix* discA) { - *discA = (contA * dt.to()).exp(); + *discA = (contA * dt.value()).exp(); } /** @@ -42,8 +42,8 @@ void DiscretizeAB(const Eigen::Matrix& contA, // Matrices are blocked here to minimize matrix exponentiation calculations Eigen::Matrix Mcont; Mcont.setZero(); - Mcont.template block(0, 0) = contA * dt.to(); - Mcont.template block(0, States) = contB * dt.to(); + Mcont.template block(0, 0) = contA * dt.value(); + Mcont.template block(0, States) = contB * dt.value(); // Discretize A and B with the given timestep Eigen::Matrix Mdisc = Mcont.exp(); @@ -76,8 +76,7 @@ void DiscretizeAQ(const Eigen::Matrix& contA, M.template block(States, 0).setZero(); M.template block(States, States) = contA.transpose(); - Eigen::Matrix phi = - (M * dt.to()).exp(); + Eigen::Matrix phi = (M * dt.value()).exp(); // Phi12 = phi[0:States, States:2*States] // Phi22 = phi[States:2*States, States:2*States] @@ -122,7 +121,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix& contA, Eigen::Matrix Q = (contQ + contQ.transpose()) / 2.0; Eigen::Matrix lastTerm = Q; - double lastCoeff = dt.to(); + double lastCoeff = dt.value(); // Aᵀⁿ Eigen::Matrix Atn = contA.transpose(); @@ -132,7 +131,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix& contA, // i = 6 i.e. 5th order should be enough precision for (int i = 2; i < 6; ++i) { lastTerm = -contA * lastTerm + Q * Atn; - lastCoeff *= dt.to() / static_cast(i); + lastCoeff *= dt.value() / static_cast(i); phi12 += lastTerm * lastCoeff; @@ -156,7 +155,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix& contA, template Eigen::Matrix DiscretizeR( const Eigen::Matrix& R, units::second_t dt) { - return R / dt.to(); + return R / dt.value(); } } // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h index 9b77f35653..ca1609c177 100644 --- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h +++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h @@ -50,8 +50,7 @@ class LinearSystemLoop { : LinearSystemLoop( plant, controller, observer, [=](const Eigen::Vector& u) { - return frc::NormalizeInputVector( - u, maxVoltage.template to()); + return frc::NormalizeInputVector(u, maxVoltage.value()); }, dt) {} @@ -97,7 +96,7 @@ class LinearSystemLoop { : LinearSystemLoop(controller, feedforward, observer, [=](const Eigen::Vector& u) { return frc::NormalizeInputVector( - u, maxVoltage.template to()); + u, maxVoltage.value()); }) {} /** diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h index 0017be221e..68d047fee9 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h +++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h @@ -23,7 +23,7 @@ namespace frc { */ template T RK4(F&& f, T x, units::second_t dt) { - const auto h = dt.to(); + const auto h = dt.value(); T k1 = f(x); T k2 = f(x + h * 0.5 * k1); @@ -43,7 +43,7 @@ T RK4(F&& f, T x, units::second_t dt) { */ template T RK4(F&& f, T x, U u, units::second_t dt) { - const auto h = dt.to(); + const auto h = dt.value(); T k1 = f(x, u); T k2 = f(x + h * 0.5 * k1, u); @@ -91,13 +91,13 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { double truncationError; double dtElapsed = 0.0; - double h = dt.to(); + double h = dt.value(); // Loop until we've gotten to our desired dt - while (dtElapsed < dt.to()) { + while (dtElapsed < dt.value()) { do { // Only allow us to advance up to the dt remaining - h = std::min(h, dt.to() - dtElapsed); + h = std::min(h, dt.value() - dtElapsed); // Notice how the derivative in the Wikipedia notation is dy/dx. // That means their y is our x and their x is our t @@ -167,13 +167,13 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { double truncationError; double dtElapsed = 0.0; - double h = dt.to(); + double h = dt.value(); // Loop until we've gotten to our desired dt - while (dtElapsed < dt.to()) { + while (dtElapsed < dt.value()) { do { // Only allow us to advance up to the dt remaining - h = std::min(h, dt.to() - dtElapsed); + h = std::min(h, dt.value() - dtElapsed); // clang-format off T k1 = f(x, u); diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index 714857f6b0..d9e6e9bfe4 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -47,9 +47,9 @@ class WPILIB_DLLEXPORT LinearSystemId { {0.0, 1.0}, {0.0, (-std::pow(G, 2) * motor.Kt / (motor.R * units::math::pow<2>(r) * m * motor.Kv)) - .to()}}; - Eigen::Matrix B{ - 0.0, (G * motor.Kt / (motor.R * r * m)).to()}; + .value()}}; + Eigen::Matrix B{0.0, + (G * motor.Kt / (motor.R * r * m)).value()}; Eigen::Matrix C{1.0, 0.0}; Eigen::Matrix D{0.0}; @@ -71,10 +71,8 @@ class WPILIB_DLLEXPORT LinearSystemId { DCMotor motor, units::kilogram_square_meter_t J, double G) { Eigen::Matrix A{ {0.0, 1.0}, - {0.0, - (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to()}}; - Eigen::Matrix B{0.0, - (G * motor.Kt / (motor.R * J)).to()}; + {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Eigen::Matrix B{0.0, (G * motor.Kt / (motor.R * J)).value()}; Eigen::Matrix C{1.0, 0.0}; Eigen::Matrix D{0.0}; @@ -107,9 +105,8 @@ class WPILIB_DLLEXPORT LinearSystemId { static LinearSystem<1, 1, 1> IdentifyVelocitySystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { - Eigen::Matrix A{-kV.template to() / - kA.template to()}; - Eigen::Matrix B{1.0 / kA.template to()}; + Eigen::Matrix A{-kV.value() / kA.value()}; + Eigen::Matrix B{1.0 / kA.value()}; Eigen::Matrix C{1.0}; Eigen::Matrix D{0.0}; @@ -142,10 +139,8 @@ class WPILIB_DLLEXPORT LinearSystemId { static LinearSystem<2, 1, 1> IdentifyPositionSystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA) { - Eigen::Matrix A{ - {0.0, 1.0}, - {0.0, -kV.template to() / kA.template to()}}; - Eigen::Matrix B{0.0, 1.0 / kA.template to()}; + Eigen::Matrix A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}}; + Eigen::Matrix B{0.0, 1.0 / kA.value()}; Eigen::Matrix C{1.0, 0.0}; Eigen::Matrix D{0.0}; @@ -170,12 +165,12 @@ class WPILIB_DLLEXPORT LinearSystemId { static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear, decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) { - double A1 = -(kVlinear.to() / kAlinear.to() + - kVangular.to() / kAangular.to()); - double A2 = -(kVlinear.to() / kAlinear.to() - - kVangular.to() / kAangular.to()); - double B1 = 1.0 / kAlinear.to() + 1.0 / kAangular.to(); - double B2 = 1.0 / kAlinear.to() - 1.0 / kAangular.to(); + double A1 = -(kVlinear.value() / kAlinear.value() + + kVangular.value() / kAangular.value()); + double A2 = -(kVlinear.value() / kAlinear.value() - + kVangular.value() / kAangular.value()); + double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value(); + double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value(); Eigen::Matrix A = 0.5 * Eigen::Matrix{{A1, A2}, {A2, A1}}; @@ -239,8 +234,8 @@ class WPILIB_DLLEXPORT LinearSystemId { units::kilogram_square_meter_t J, double G) { Eigen::Matrix A{ - (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to()}; - Eigen::Matrix B{(G * motor.Kt / (motor.R * J)).to()}; + (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; + Eigen::Matrix B{(G * motor.Kt / (motor.R * J)).value()}; Eigen::Matrix C{1.0}; Eigen::Matrix D{0.0}; @@ -269,15 +264,15 @@ class WPILIB_DLLEXPORT LinearSystemId { auto C2 = G * motor.Kt / (motor.R * r); Eigen::Matrix A{ - {((1 / m + units::math::pow<2>(rb) / J) * C1).to(), - ((1 / m - units::math::pow<2>(rb) / J) * C1).to()}, - {((1 / m - units::math::pow<2>(rb) / J) * C1).to(), - ((1 / m + units::math::pow<2>(rb) / J) * C1).to()}}; + {((1 / m + units::math::pow<2>(rb) / J) * C1).value(), + ((1 / m - units::math::pow<2>(rb) / J) * C1).value()}, + {((1 / m - units::math::pow<2>(rb) / J) * C1).value(), + ((1 / m + units::math::pow<2>(rb) / J) * C1).value()}}; Eigen::Matrix B{ - {((1 / m + units::math::pow<2>(rb) / J) * C2).to(), - ((1 / m - units::math::pow<2>(rb) / J) * C2).to()}, - {((1 / m - units::math::pow<2>(rb) / J) * C2).to(), - ((1 / m + units::math::pow<2>(rb) / J) * C2).to()}}; + {((1 / m + units::math::pow<2>(rb) / J) * C2).value(), + ((1 / m - units::math::pow<2>(rb) / J) * C2).value()}, + {((1 / m - units::math::pow<2>(rb) / J) * C2).value(), + ((1 / m + units::math::pow<2>(rb) / J) * C2).value()}}; Eigen::Matrix C{{1.0, 0.0}, {0.0, 1.0}}; Eigen::Matrix D{{0.0, 0.0}, {0.0, 0.0}}; diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index 23c85542f1..6b5af2baf5 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -6,11 +6,9 @@ #include "gtest/gtest.h" #include "units/angle.h" -#define EXPECT_UNITS_EQ(a, b) \ - EXPECT_DOUBLE_EQ((a).to(), (b).to()) +#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value()) -#define EXPECT_UNITS_NEAR(a, b, c) \ - EXPECT_NEAR((a).to(), (b).to(), c) +#define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c) TEST(MathUtilTest, ApplyDeadband) { // < 0 diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp index 2117cb8f9f..ec38a471e3 100644 --- a/wpimath/src/test/native/cpp/UnitsTest.cpp +++ b/wpimath/src/test/native/cpp/UnitsTest.cpp @@ -1324,7 +1324,7 @@ TEST_F(UnitContainer, scalarTypeImplicitConversion) { } TEST_F(UnitContainer, valueMethod) { - double test = meter_t(3.0).to(); + double test = meter_t(3.0).value(); EXPECT_DOUBLE_EQ(3.0, test); auto test2 = meter_t(4.0).value(); @@ -1333,7 +1333,7 @@ TEST_F(UnitContainer, valueMethod) { } TEST_F(UnitContainer, convertMethod) { - double test = meter_t(3.0).convert().to(); + double test = meter_t(3.0).convert().value(); EXPECT_NEAR(9.84252, test, 5.0e-6); } @@ -1562,13 +1562,13 @@ TEST_F(UnitContainer, nameAndAbbreviation) { TEST_F(UnitContainer, negative) { meter_t a(5.3); meter_t b(-5.3); - EXPECT_NEAR(a.to(), -b.to(), 5.0e-320); - EXPECT_NEAR(b.to(), -a.to(), 5.0e-320); + EXPECT_NEAR(a.value(), -b.value(), 5.0e-320); + EXPECT_NEAR(b.value(), -a.value(), 5.0e-320); dB_t c(2.87); dB_t d(-2.87); - EXPECT_NEAR(c.to(), -d.to(), 5.0e-320); - EXPECT_NEAR(d.to(), -c.to(), 5.0e-320); + EXPECT_NEAR(c.value(), -d.value(), 5.0e-320); + EXPECT_NEAR(d.value(), -c.value(), 5.0e-320); ppm_t e = -1 * ppm_t(10); EXPECT_EQ(e, -ppm_t(10)); @@ -1579,7 +1579,7 @@ TEST_F(UnitContainer, concentration) { ppb_t a(ppm_t(1)); EXPECT_EQ(ppb_t(1000), a); EXPECT_EQ(0.000001, a); - EXPECT_EQ(0.000001, a.to()); + EXPECT_EQ(0.000001, a.value()); scalar_t b(ppm_t(1)); EXPECT_EQ(0.000001, b); @@ -1789,7 +1789,7 @@ TEST_F(UnitConversion, time) { year_t twoYears(2.0); week_t twoYearsInWeeks = twoYears; - EXPECT_NEAR(week_t(104.286).to(), twoYearsInWeeks.to(), + EXPECT_NEAR(week_t(104.286).value(), twoYearsInWeeks.value(), 5.0e-4); double test; @@ -1826,8 +1826,8 @@ TEST_F(UnitConversion, time) { TEST_F(UnitConversion, angle) { angle::degree_t quarterCircleDeg(90.0); angle::radian_t quarterCircleRad = quarterCircleDeg; - EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).to(), - quarterCircleRad.to(), 5.0e-12); + EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).value(), + quarterCircleRad.value(), 5.0e-12); double test; @@ -2631,7 +2631,7 @@ TEST_F(UnitConversion, pi) { EXPECT_TRUE(constants::pi < 4.0); // explicit conversion - EXPECT_NEAR(3.14159, constants::pi.to(), 5.0e-6); + EXPECT_NEAR(3.14159, constants::pi.value(), 5.0e-6); // auto multiplication EXPECT_TRUE( @@ -2640,16 +2640,16 @@ TEST_F(UnitConversion, pi) { (std::is_same::value)); EXPECT_NEAR(constants::detail::PI_VAL, - (constants::pi * meter_t(1)).to(), 5.0e-10); + (constants::pi * meter_t(1)).value(), 5.0e-10); EXPECT_NEAR(constants::detail::PI_VAL, - (meter_t(1) * constants::pi).to(), 5.0e-10); + (meter_t(1) * constants::pi).value(), 5.0e-10); // explicit multiplication meter_t a = constants::pi * meter_t(1); meter_t b = meter_t(1) * constants::pi; - EXPECT_NEAR(constants::detail::PI_VAL, a.to(), 5.0e-10); - EXPECT_NEAR(constants::detail::PI_VAL, b.to(), 5.0e-10); + EXPECT_NEAR(constants::detail::PI_VAL, a.value(), 5.0e-10); + EXPECT_NEAR(constants::detail::PI_VAL, b.value(), 5.0e-10); // auto division EXPECT_TRUE( @@ -2658,16 +2658,16 @@ TEST_F(UnitConversion, pi) { (std::is_same::value)); EXPECT_NEAR(constants::detail::PI_VAL, - (constants::pi / second_t(1)).to(), 5.0e-10); + (constants::pi / second_t(1)).value(), 5.0e-10); EXPECT_NEAR(1.0 / constants::detail::PI_VAL, - (second_t(1) / constants::pi).to(), 5.0e-10); + (second_t(1) / constants::pi).value(), 5.0e-10); // explicit hertz_t c = constants::pi / second_t(1); second_t d = second_t(1) / constants::pi; - EXPECT_NEAR(constants::detail::PI_VAL, c.to(), 5.0e-10); - EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.to(), 5.0e-10); + EXPECT_NEAR(constants::detail::PI_VAL, c.value(), 5.0e-10); + EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.value(), 5.0e-10); } TEST_F(UnitConversion, constants) { @@ -2774,12 +2774,12 @@ TEST_F(UnitMath, acos) { (std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(2).to(), - acos(scalar_t(-0.41614683654)).to(), 5.0e-11); + EXPECT_NEAR(angle::radian_t(2).value(), + acos(scalar_t(-0.41614683654)).value(), 5.0e-11); EXPECT_NEAR( - angle::degree_t(135).to(), + angle::degree_t(135).value(), angle::degree_t(acos(scalar_t(-0.70710678118654752440084436210485))) - .to(), + .value(), 5.0e-12); } @@ -2788,12 +2788,12 @@ TEST_F(UnitMath, asin) { (std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(1.14159265).to(), - asin(scalar_t(0.90929742682)).to(), 5.0e-9); + EXPECT_NEAR(angle::radian_t(1.14159265).value(), + asin(scalar_t(0.90929742682)).value(), 5.0e-9); EXPECT_NEAR( - angle::degree_t(45).to(), + angle::degree_t(45).value(), angle::degree_t(asin(scalar_t(0.70710678118654752440084436210485))) - .to(), + .value(), 5.0e-12); } @@ -2802,32 +2802,32 @@ TEST_F(UnitMath, atan) { (std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(-1.14159265).to(), - atan(scalar_t(-2.18503986326)).to(), 5.0e-9); - EXPECT_NEAR(angle::degree_t(-45).to(), - angle::degree_t(atan(scalar_t(-1.0))).to(), 5.0e-12); + EXPECT_NEAR(angle::radian_t(-1.14159265).value(), + atan(scalar_t(-2.18503986326)).value(), 5.0e-9); + EXPECT_NEAR(angle::degree_t(-45).value(), + angle::degree_t(atan(scalar_t(-1.0))).value(), 5.0e-12); } TEST_F(UnitMath, atan2) { EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).to(), - atan2(scalar_t(2), scalar_t(2)).to(), 5.0e-12); + EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).value(), + atan2(scalar_t(2), scalar_t(2)).value(), 5.0e-12); EXPECT_NEAR( - angle::degree_t(45).to(), - angle::degree_t(atan2(scalar_t(2), scalar_t(2))).to(), + angle::degree_t(45).value(), + angle::degree_t(atan2(scalar_t(2), scalar_t(2))).value(), 5.0e-12); EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).to(), - atan2(scalar_t(1), scalar_t(std::sqrt(3))).to(), + EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).value(), + atan2(scalar_t(1), scalar_t(std::sqrt(3))).value(), 5.0e-12); - EXPECT_NEAR(angle::degree_t(30).to(), + EXPECT_NEAR(angle::degree_t(30).value(), angle::degree_t(atan2(scalar_t(1), scalar_t(std::sqrt(3)))) - .to(), + .value(), 5.0e-12); } @@ -2859,30 +2859,30 @@ TEST_F(UnitMath, acosh) { EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(1.316957896924817).to(), - acosh(scalar_t(2.0)).to(), 5.0e-11); - EXPECT_NEAR(angle::degree_t(75.456129290216893).to(), - angle::degree_t(acosh(scalar_t(2.0))).to(), 5.0e-12); + EXPECT_NEAR(angle::radian_t(1.316957896924817).value(), + acosh(scalar_t(2.0)).value(), 5.0e-11); + EXPECT_NEAR(angle::degree_t(75.456129290216893).value(), + angle::degree_t(acosh(scalar_t(2.0))).value(), 5.0e-12); } TEST_F(UnitMath, asinh) { EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(1.443635475178810).to(), - asinh(scalar_t(2)).to(), 5.0e-9); - EXPECT_NEAR(angle::degree_t(82.714219883108939).to(), - angle::degree_t(asinh(scalar_t(2))).to(), 5.0e-12); + EXPECT_NEAR(angle::radian_t(1.443635475178810).value(), + asinh(scalar_t(2)).value(), 5.0e-9); + EXPECT_NEAR(angle::degree_t(82.714219883108939).value(), + angle::degree_t(asinh(scalar_t(2))).value(), 5.0e-12); } TEST_F(UnitMath, atanh) { EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(0.549306144334055).to(), - atanh(scalar_t(0.5)).to(), 5.0e-9); - EXPECT_NEAR(angle::degree_t(31.472923730945389).to(), - angle::degree_t(atanh(scalar_t(0.5))).to(), 5.0e-12); + EXPECT_NEAR(angle::radian_t(0.549306144334055).value(), + atanh(scalar_t(0.5)).value(), 5.0e-9); + EXPECT_NEAR(angle::degree_t(31.472923730945389).value(), + angle::degree_t(atanh(scalar_t(0.5))).value(), 5.0e-12); } TEST_F(UnitMath, exp) { @@ -2954,14 +2954,14 @@ TEST_F(UnitMath, sqrt) { EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(meter_t(2.0).to(), - sqrt(square_meter_t(4.0)).to(), 5.0e-9); + EXPECT_NEAR(meter_t(2.0).value(), + sqrt(square_meter_t(4.0)).value(), 5.0e-9); EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(angle::radian_t(4.0).to(), - sqrt(steradian_t(16.0)).to(), 5.0e-9); + EXPECT_NEAR(angle::radian_t(4.0).value(), + sqrt(steradian_t(16.0)).value(), 5.0e-9); EXPECT_TRUE((std::is_convertible::type, typename std::decay(), - sqrt(square_foot_t(10.0)).to(), 5.0e-9); - EXPECT_NEAR(foot_t(3.16227766017).to(), resultFt.to(), + EXPECT_NEAR(foot_t(3.16227766017).value(), + sqrt(square_foot_t(10.0)).value(), 5.0e-9); + EXPECT_NEAR(foot_t(3.16227766017).value(), resultFt.value(), 5.0e-9); EXPECT_EQ(resultFt, sqrt(square_foot_t(10.0))); } @@ -2981,19 +2981,19 @@ TEST_F(UnitMath, hypot) { EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(meter_t(5.0).to(), - (hypot(meter_t(3.0), meter_t(4.0))).to(), 5.0e-9); + EXPECT_NEAR(meter_t(5.0).value(), + (hypot(meter_t(3.0), meter_t(4.0))).value(), 5.0e-9); EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(foot_t(5.0).to(), - (hypot(foot_t(3.0), meter_t(1.2192))).to(), 5.0e-9); + EXPECT_NEAR(foot_t(5.0).value(), + (hypot(foot_t(3.0), meter_t(1.2192))).value(), 5.0e-9); } TEST_F(UnitMath, ceil) { double val = 101.1; - EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).to()); + EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).value()); EXPECT_TRUE((std::is_same::type, typename std::decay::type>::value)); @@ -3006,7 +3006,7 @@ TEST_F(UnitMath, floor) { TEST_F(UnitMath, fmod) { EXPECT_EQ(std::fmod(100.0, 101.2), - fmod(meter_t(100.0), meter_t(101.2)).to()); + fmod(meter_t(100.0), meter_t(101.2)).value()); } TEST_F(UnitMath, trunc) { @@ -3029,8 +3029,8 @@ TEST_F(UnitMath, copysign) { TEST_F(UnitMath, fdim) { EXPECT_EQ(meter_t(0.0), fdim(meter_t(8.0), meter_t(10.0))); EXPECT_EQ(meter_t(2.0), fdim(meter_t(10.0), meter_t(8.0))); - EXPECT_NEAR(meter_t(9.3904).to(), - fdim(meter_t(10.0), foot_t(2.0)).to(), + EXPECT_NEAR(meter_t(9.3904).value(), + fdim(meter_t(10.0), foot_t(2.0)).value(), 5.0e-320); // not sure why they aren't comparing exactly equal, // but clearly they are. } @@ -3200,7 +3200,7 @@ TEST_F(CompileTimeArithmetic, unit_value_add) { EXPECT_TRUE(( std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(5.92125984, sumf::value().to(), 5.0e-8); + EXPECT_NEAR(5.92125984, sumf::value().value(), 5.0e-8); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); @@ -3211,7 +3211,7 @@ TEST_F(CompileTimeArithmetic, unit_value_add) { EXPECT_TRUE(( std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(2.11111111111, sumc::value().to(), 5.0e-8); + EXPECT_NEAR(2.11111111111, sumc::value().value(), 5.0e-8); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3222,7 +3222,7 @@ TEST_F(CompileTimeArithmetic, unit_value_add) { EXPECT_TRUE(( std::is_same::type, typename std::decay::type>::value)); - EXPECT_NEAR(1.05235988, sumr::value().to(), 5.0e-8); + EXPECT_NEAR(1.05235988, sumr::value().value(), 5.0e-8); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); } @@ -3241,7 +3241,7 @@ TEST_F(CompileTimeArithmetic, unit_value_subtract) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(-3.92125984, difff::value().to(), 5.0e-8); + EXPECT_NEAR(-3.92125984, difff::value().value(), 5.0e-8); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); @@ -3252,7 +3252,7 @@ TEST_F(CompileTimeArithmetic, unit_value_subtract) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(-0.11111111111, diffc::value().to(), 5.0e-8); + EXPECT_NEAR(-0.11111111111, diffc::value().value(), 5.0e-8); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3263,7 +3263,7 @@ TEST_F(CompileTimeArithmetic, unit_value_subtract) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(0.947640122, diffr::value().to(), 5.0e-8); + EXPECT_NEAR(0.947640122, diffr::value().value(), 5.0e-8); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); } @@ -3282,7 +3282,7 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(4.0, productM::value().to(), 5.0e-7); + EXPECT_NEAR(4.0, productM::value().value(), 5.0e-7); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); @@ -3290,7 +3290,7 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(43.0556444224, productF::value().to(), 5.0e-6); + EXPECT_NEAR(43.0556444224, productF::value().value(), 5.0e-6); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); @@ -3299,7 +3299,7 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) { (std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(43.0556444224, productF2::value().to(), 5.0e-8); + EXPECT_NEAR(43.0556444224, productF2::value().value(), 5.0e-8); EXPECT_TRUE(( traits::is_unit_value_t_category::value)); @@ -3313,8 +3313,8 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) { EXPECT_TRUE((std::is_convertible< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(32.8084, productN::value().to(), 5.0e-8); - EXPECT_NEAR(10.0, (productN::value().convert().to()), + EXPECT_NEAR(32.8084, productN::value().value(), 5.0e-8); + EXPECT_NEAR(10.0, (productN::value().convert().value()), 5.0e-7); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3326,9 +3326,9 @@ TEST_F(CompileTimeArithmetic, unit_value_multiply) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(2.42, productR::value().to(), 5.0e-8); + EXPECT_NEAR(2.42, productR::value().value(), 5.0e-8); EXPECT_NEAR(7944.39137, - (productR::value().convert().to()), + (productR::value().convert().value()), 5.0e-6); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3348,7 +3348,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(1, productM::value().to(), 5.0e-7); + EXPECT_NEAR(1, productM::value().value(), 5.0e-7); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3356,7 +3356,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) { EXPECT_TRUE((std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(1.0, productF::value().to(), 5.0e-6); + EXPECT_NEAR(1.0, productF::value().value(), 5.0e-6); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3365,7 +3365,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) { (std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(1.0, productF2::value().to(), 5.0e-8); + EXPECT_NEAR(1.0, productF2::value().value(), 5.0e-8); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3376,7 +3376,7 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) { (std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(0.2, productMS::value().to(), 5.0e-8); + EXPECT_NEAR(0.2, productMS::value().value(), 5.0e-8); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3387,9 +3387,9 @@ TEST_F(CompileTimeArithmetic, unit_value_divide) { (std::is_same< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(2, productRS::value().to(), 5.0e-8); + EXPECT_NEAR(2, productRS::value().value(), 5.0e-8); EXPECT_NEAR(114.592, - (productRS::value().convert().to()), + (productRS::value().convert().value()), 5.0e-4); EXPECT_TRUE((traits::is_unit_value_t_category::value)); @@ -3402,7 +3402,7 @@ TEST_F(CompileTimeArithmetic, unit_value_power) { EXPECT_TRUE((std::is_convertible< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(4, sq::value().to(), 5.0e-8); + EXPECT_NEAR(4, sq::value().value(), 5.0e-8); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); @@ -3412,9 +3412,9 @@ TEST_F(CompileTimeArithmetic, unit_value_power) { EXPECT_TRUE((std::is_convertible< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(3.24, sqr::value().to(), 5.0e-8); + EXPECT_NEAR(3.24, sqr::value().value(), 5.0e-8); EXPECT_NEAR(10636.292574038049895092690529904, - (sqr::value().convert().to()), 5.0e-10); + (sqr::value().convert().value()), 5.0e-10); EXPECT_TRUE((traits::is_unit_value_t_category::value)); } @@ -3426,7 +3426,7 @@ TEST_F(CompileTimeArithmetic, unit_value_sqrt) { EXPECT_TRUE((std::is_convertible< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(3.16227766017, root::value().to(), 5.0e-9); + EXPECT_NEAR(3.16227766017, root::value().value(), 5.0e-9); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); @@ -3436,8 +3436,8 @@ TEST_F(CompileTimeArithmetic, unit_value_sqrt) { EXPECT_TRUE((std::is_convertible< typename std::decay::type, typename std::decay::type>::value)); - EXPECT_NEAR(2.69920623253, rooth::value().to(), 5.0e-8); - EXPECT_NEAR(269.920623, rooth::value().convert().to(), + EXPECT_NEAR(2.69920623253, rooth::value().value(), 5.0e-8); + EXPECT_NEAR(269.920623, rooth::value().convert().value(), 5.0e-6); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); @@ -3446,9 +3446,9 @@ TEST_F(CompileTimeArithmetic, unit_value_sqrt) { using rootr = unit_value_sqrt; EXPECT_TRUE((traits::is_angle_unit::value)); - EXPECT_NEAR(1.3416407865, rootr::value().to(), 5.0e-8); + EXPECT_NEAR(1.3416407865, rootr::value().value(), 5.0e-8); EXPECT_NEAR(76.870352574, - rootr::value().convert().to(), 5.0e-6); + rootr::value().convert().value(), 5.0e-6); EXPECT_TRUE( (traits::is_unit_value_t_category::value)); } diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp index fb3ad14bce..c6b669c0d0 100644 --- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp @@ -37,7 +37,7 @@ TEST(HolonomicDriveControllerTest, ReachesReference) { constexpr auto kDt = 0.02_s; auto totalTime = trajectory.TotalTime(); - for (size_t i = 0; i < (totalTime / kDt).to(); ++i) { + for (size_t i = 0; i < (totalTime / kDt).value(); ++i) { auto state = trajectory.Sample(kDt * i); auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad); @@ -62,5 +62,5 @@ TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) { frc::ChassisSpeeds speeds = controller.Calculate( frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad); - EXPECT_EQ(0, speeds.omega.to()); + EXPECT_EQ(0, speeds.omega.value()); } diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp index 27017316d2..379db9e993 100644 --- a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp +++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp @@ -38,7 +38,7 @@ TEST_F(PIDInputOutputTest, IntegralGainOutput) { out = controller->Calculate(0.025, 0); } - EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to(), out); + EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().value(), out); } TEST_F(PIDInputOutputTest, DerivativeGainOutput) { diff --git a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp index 00f2b1c6f8..da402ae026 100644 --- a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp @@ -104,7 +104,7 @@ TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutput) { out = controller->Calculate(0.025_deg, 0_deg); } - EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to(), out); + EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().value(), out); } TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutput) { diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp index 244e4d43c8..5e297f4294 100644 --- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -26,7 +26,7 @@ TEST(RamseteControllerTest, ReachesReference) { constexpr auto kDt = 0.02_s; auto totalTime = trajectory.TotalTime(); - for (size_t i = 0; i < (totalTime / kDt).to(); ++i) { + for (size_t i = 0; i < (totalTime / kDt).value(); ++i) { auto state = trajectory.Sample(kDt * i); auto [vx, vy, omega] = controller.Calculate(robotPose, state); static_cast(vy); diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp index 29a789e370..3cf944ef49 100644 --- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp @@ -33,14 +33,14 @@ TEST(SimpleMotorFeedforwardTest, Calculate) { Eigen::Vector nextR{3.0}; EXPECT_NEAR(37.524995834325161 + Ks, - simpleMotor.Calculate(2_mps, 3_mps, dt).to(), 0.002); + simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002); EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks, - simpleMotor.Calculate(2_mps, 3_mps, dt).to(), 0.002); + simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002); // These won't match exactly. It's just an approximation to make sure they're // in the same ballpark. EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks, - simpleMotor.Calculate(2_mps, 1_mps / dt).to(), 2.0); + simpleMotor.Calculate(2_mps, 1_mps / dt).value(), 2.0); } } // namespace frc diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index dcc948ac99..4a854fd138 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -82,7 +82,7 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) { double error = groundTruthState.pose.Translation() .Distance(xhat.Translation()) - .to(); + .value(); if (error > maxError) { maxError = error; @@ -92,8 +92,7 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) { t += dt; } - EXPECT_NEAR( - 0.0, errorSum / (trajectory.TotalTime().to() / dt.to()), - 0.2); + EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()), + 0.2); EXPECT_NEAR(0.0, maxError, 0.4); } diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp index 830895521e..6d51185661 100644 --- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp @@ -42,12 +42,12 @@ Eigen::Vector Dynamics(const Eigen::Vector& x, auto v = 0.5 * (vl + vr); return Eigen::Vector{ - v.to() * std::cos(x(2)), v.to() * std::sin(x(2)), - ((vr - vl) / (2.0 * rb)).to(), - k1.to() * ((C1 * vl).to() + (C2 * Vl).to()) + - k2.to() * ((C1 * vr).to() + (C2 * Vr).to()), - k2.to() * ((C1 * vl).to() + (C2 * Vl).to()) + - k1.to() * ((C1 * vr).to() + (C2 * Vr).to())}; + v.value() * std::cos(x(2)), v.value() * std::sin(x(2)), + ((vr - vl) / (2.0 * rb)).value(), + k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) + + k2.value() * ((C1 * vr).value() + (C2 * Vr).value()), + k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) + + k1.value() * ((C1 * vr).value() + (C2 * Vr).value())}; } Eigen::Vector LocalMeasurementModel( @@ -106,28 +106,27 @@ TEST(ExtendedKalmanFilterTest, Convergence) { Eigen::Vector::Zero()); observer.SetXhat(Eigen::Vector{ - trajectory.InitialPose().Translation().X().to(), - trajectory.InitialPose().Translation().Y().to(), - trajectory.InitialPose().Rotation().Radians().to(), 0.0, 0.0}); + trajectory.InitialPose().Translation().X().value(), + trajectory.InitialPose().Translation().Y().value(), + trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0}); auto totalTime = trajectory.TotalTime(); - for (size_t i = 0; i < (totalTime / dt).to(); ++i) { + for (size_t i = 0; i < (totalTime / dt).value(); ++i) { auto ref = trajectory.Sample(dt * i); units::meters_per_second_t vl = - ref.velocity * (1 - (ref.curvature * rb).to()); + ref.velocity * (1 - (ref.curvature * rb).value()); units::meters_per_second_t vr = - ref.velocity * (1 + (ref.curvature * rb).to()); + ref.velocity * (1 + (ref.curvature * rb).value()); - Eigen::Vector nextR{ref.pose.Translation().X().to(), - ref.pose.Translation().Y().to(), - ref.pose.Rotation().Radians().to(), - vl.to(), vr.to()}; + Eigen::Vector nextR{ + ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(), + ref.pose.Rotation().Radians().value(), vl.value(), vr.value()}; auto localY = LocalMeasurementModel(nextR, Eigen::Vector::Zero()); observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5)); - Eigen::Vector rdot = (nextR - r) / dt.to(); + Eigen::Vector rdot = (nextR - r) / dt.value(); u = B.householderQr().solve(rdot - Dynamics(r, Eigen::Vector::Zero())); @@ -144,12 +143,12 @@ TEST(ExtendedKalmanFilterTest, Convergence) { observer.Correct<5>(u, globalY, GlobalMeasurementModel, R); auto finalPosition = trajectory.Sample(trajectory.TotalTime()); - ASSERT_NEAR(finalPosition.pose.Translation().X().template to(), - observer.Xhat(0), 1.0); - ASSERT_NEAR(finalPosition.pose.Translation().Y().template to(), - observer.Xhat(1), 1.0); - ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to(), - observer.Xhat(2), 1.0); + ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0), + 1.0); + ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1), + 1.0); + ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2), + 1.0); ASSERT_NEAR(0.0, observer.Xhat(3), 1.0); ASSERT_NEAR(0.0, observer.Xhat(4), 1.0); } diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index 43f3595f6c..881d4e8fbb 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -74,7 +74,7 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) { wheelSpeeds); double error = groundTruthState.pose.Translation() .Distance(xhat.Translation()) - .to(); + .value(); if (error > maxError) { maxError = error; @@ -84,7 +84,6 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) { t += dt; } - EXPECT_LT(errorSum / (trajectory.TotalTime().to() / dt.to()), - 0.2); + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2); EXPECT_LT(maxError, 0.4); } diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index efe72a045e..ee01f6fd1a 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -74,7 +74,7 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) { moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]); double error = groundTruthState.pose.Translation() .Distance(xhat.Translation()) - .to(); + .value(); if (error > maxError) { maxError = error; @@ -84,7 +84,6 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) { t += dt; } - EXPECT_LT(errorSum / (trajectory.TotalTime().to() / dt.to()), - 0.2); + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2); EXPECT_LT(maxError, 0.4); } diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp index 8c9afc10bb..9665442bf3 100644 --- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp @@ -44,12 +44,12 @@ Eigen::Vector Dynamics(const Eigen::Vector& x, auto v = 0.5 * (vl + vr); return Eigen::Vector{ - v.to() * std::cos(x(2)), v.to() * std::sin(x(2)), - ((vr - vl) / (2.0 * rb)).to(), - k1.to() * ((C1 * vl).to() + (C2 * Vl).to()) + - k2.to() * ((C1 * vr).to() + (C2 * Vr).to()), - k2.to() * ((C1 * vl).to() + (C2 * Vl).to()) + - k1.to() * ((C1 * vr).to() + (C2 * Vr).to())}; + v.value() * std::cos(x(2)), v.value() * std::sin(x(2)), + ((vr - vl) / (2.0 * rb)).value(), + k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) + + k2.value() * ((C1 * vr).value() + (C2 * Vr).value()), + k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) + + k1.value() * ((C1 * vr).value() + (C2 * Vr).value())}; } Eigen::Vector LocalMeasurementModel( @@ -110,30 +110,29 @@ TEST(UnscentedKalmanFilterTest, Convergence) { Eigen::Vector::Zero()); observer.SetXhat(Eigen::Vector{ - trajectory.InitialPose().Translation().X().to(), - trajectory.InitialPose().Translation().Y().to(), - trajectory.InitialPose().Rotation().Radians().to(), 0.0, 0.0}); + trajectory.InitialPose().Translation().X().value(), + trajectory.InitialPose().Translation().Y().value(), + trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0}); auto trueXhat = observer.Xhat(); auto totalTime = trajectory.TotalTime(); - for (size_t i = 0; i < (totalTime / dt).to(); ++i) { + for (size_t i = 0; i < (totalTime / dt).value(); ++i) { auto ref = trajectory.Sample(dt * i); units::meters_per_second_t vl = - ref.velocity * (1 - (ref.curvature * rb).to()); + ref.velocity * (1 - (ref.curvature * rb).value()); units::meters_per_second_t vr = - ref.velocity * (1 + (ref.curvature * rb).to()); + ref.velocity * (1 + (ref.curvature * rb).value()); - Eigen::Vector nextR{ref.pose.Translation().X().to(), - ref.pose.Translation().Y().to(), - ref.pose.Rotation().Radians().to(), - vl.to(), vr.to()}; + Eigen::Vector nextR{ + ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(), + ref.pose.Rotation().Radians().value(), vl.value(), vr.value()}; auto localY = LocalMeasurementModel(trueXhat, Eigen::Vector::Zero()); observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5)); - Eigen::Vector rdot = (nextR - r) / dt.to(); + Eigen::Vector rdot = (nextR - r) / dt.value(); u = B.householderQr().solve(rdot - Dynamics(r, Eigen::Vector::Zero())); @@ -155,12 +154,12 @@ TEST(UnscentedKalmanFilterTest, Convergence) { ); auto finalPosition = trajectory.Sample(trajectory.TotalTime()); - ASSERT_NEAR(finalPosition.pose.Translation().X().template to(), - observer.Xhat(0), 1.0); - ASSERT_NEAR(finalPosition.pose.Translation().Y().template to(), - observer.Xhat(1), 1.0); - ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to(), - observer.Xhat(2), 1.0); + ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0), + 1.0); + ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1), + 1.0); + ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2), + 1.0); ASSERT_NEAR(0.0, observer.Xhat(3), 1.0); ASSERT_NEAR(0.0, observer.Xhat(4), 1.0); } diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp index 8da933382e..5ccd829d0d 100644 --- a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp +++ b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp @@ -52,7 +52,7 @@ TEST_P(LinearFilterNoiseTest, NoiseReduce) { std::normal_distribution distr{0.0, 10.0}; for (auto t = 0_s; t < kFilterTime; t += kFilterStep) { - double theory = GetData(t.to()); + double theory = GetData(t.value()); double noise = distr(gen); filterError += std::abs(m_filter.Calculate(theory + noise) - theory); noiseGenError += std::abs(noise - theory); diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp index 9ca53b3f9b..bca3f9d9ee 100644 --- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp +++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp @@ -106,7 +106,7 @@ class LinearFilterOutputTest TEST_P(LinearFilterOutputTest, Output) { double filterOutput = 0.0; for (auto t = 0_s; t < kFilterTime; t += kFilterStep) { - filterOutput = m_filter.Calculate(m_data(t.to())); + filterOutput = m_filter.Calculate(m_data(t.value())); } RecordProperty("LinearFilterOutput", filterOutput); @@ -126,18 +126,17 @@ void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min, frc::LinearFilter::BackwardFiniteDifference( h); - for (int i = min / h.to(); i < max / h.to(); ++i) { + for (int i = min / h.value(); i < max / h.value(); ++i) { // Let filter initialize - if (i < static_cast(min / h.to()) + Samples) { - filter.Calculate(f(i * h.to())); + if (i < static_cast(min / h.value()) + Samples) { + filter.Calculate(f(i * h.value())); continue; } // The order of accuracy is O(h^(N - d)) where N is number of stencil // points and d is order of derivative - EXPECT_NEAR(dfdx(i * h.to()), - filter.Calculate(f(i * h.to())), - 10.0 * std::pow(h.to(), Samples - Derivative)); + EXPECT_NEAR(dfdx(i * h.value()), filter.Calculate(f(i * h.value())), + 10.0 * std::pow(h.value(), Samples - Derivative)); } } diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index 1494aaf249..cd5b127c32 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -17,9 +17,9 @@ TEST(Pose2dTest, TransformBy) { const auto transformed = initial + transform; - EXPECT_NEAR(transformed.X().to(), 1 + 5 / std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(transformed.Y().to(), 2 + 5 / std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(transformed.Rotation().Degrees().to(), 50.0, kEpsilon); + EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon); } TEST(Pose2dTest, RelativeTo) { @@ -28,10 +28,10 @@ TEST(Pose2dTest, RelativeTo) { const auto finalRelativeToInitial = final.RelativeTo(initial); - EXPECT_NEAR(finalRelativeToInitial.X().to(), 5.0 * std::sqrt(2.0), + EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(finalRelativeToInitial.Y().to(), 0.0, kEpsilon); - EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to(), 0.0, + EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon); + EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0, kEpsilon); } @@ -53,7 +53,7 @@ TEST(Pose2dTest, Minus) { const auto transform = final - initial; - EXPECT_NEAR(transform.X().to(), 5.0 * std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(transform.Y().to(), 0.0, kEpsilon); - EXPECT_NEAR(transform.Rotation().Degrees().to(), 0.0, kEpsilon); + EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon); + EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 006267d2ea..ed3b6b59a2 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -17,38 +17,38 @@ TEST(Rotation2dTest, RadiansToDegrees) { const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)}; const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)}; - EXPECT_NEAR(rot1.Degrees().to(), 60.0, kEpsilon); - EXPECT_NEAR(rot2.Degrees().to(), 45.0, kEpsilon); + EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon); + EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon); } TEST(Rotation2dTest, DegreesToRadians) { const auto rot1 = Rotation2d(45.0_deg); const auto rot2 = Rotation2d(30.0_deg); - EXPECT_NEAR(rot1.Radians().to(), wpi::numbers::pi / 4.0, kEpsilon); - EXPECT_NEAR(rot2.Radians().to(), wpi::numbers::pi / 6.0, kEpsilon); + EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon); + EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon); } TEST(Rotation2dTest, RotateByFromZero) { const Rotation2d zero; auto sum = zero + Rotation2d(90.0_deg); - EXPECT_NEAR(sum.Radians().to(), wpi::numbers::pi / 2.0, kEpsilon); - EXPECT_NEAR(sum.Degrees().to(), 90.0, kEpsilon); + EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon); + EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon); } TEST(Rotation2dTest, RotateByNonZero) { auto rot = Rotation2d(90.0_deg); rot = rot + Rotation2d(30.0_deg); - EXPECT_NEAR(rot.Degrees().to(), 120.0, kEpsilon); + EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon); } TEST(Rotation2dTest, Minus) { const auto rot1 = Rotation2d(70.0_deg); const auto rot2 = Rotation2d(30.0_deg); - EXPECT_NEAR((rot1 - rot2).Degrees().to(), 40.0, kEpsilon); + EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon); } TEST(Rotation2dTest, Equality) { diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 2014a6c624..968ab29edf 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -21,12 +21,10 @@ TEST(Transform2dTest, Inverse) { auto transformed = initial + transform; auto untransformed = transformed + transform.Inverse(); - EXPECT_NEAR(initial.X().to(), untransformed.X().to(), - kEpsilon); - EXPECT_NEAR(initial.Y().to(), untransformed.Y().to(), - kEpsilon); - EXPECT_NEAR(initial.Rotation().Degrees().to(), - untransformed.Rotation().Degrees().to(), kEpsilon); + EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon); + EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon); + EXPECT_NEAR(initial.Rotation().Degrees().value(), + untransformed.Rotation().Degrees().value(), kEpsilon); } TEST(Transform2dTest, Composition) { @@ -37,10 +35,10 @@ TEST(Transform2dTest, Composition) { auto transformedSeparate = initial + transform1 + transform2; auto transformedCombined = initial + (transform1 + transform2); - EXPECT_NEAR(transformedSeparate.X().to(), - transformedCombined.X().to(), kEpsilon); - EXPECT_NEAR(transformedSeparate.Y().to(), - transformedCombined.Y().to(), kEpsilon); - EXPECT_NEAR(transformedSeparate.Rotation().Degrees().to(), - transformedCombined.Rotation().Degrees().to(), kEpsilon); + EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(), + kEpsilon); + EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(), + kEpsilon); + EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(), + transformedCombined.Rotation().Degrees().value(), kEpsilon); } diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 679245ebae..efdcace525 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -17,8 +17,8 @@ TEST(Translation2dTest, Sum) { const auto sum = one + two; - EXPECT_NEAR(sum.X().to(), 3.0, kEpsilon); - EXPECT_NEAR(sum.Y().to(), 8.0, kEpsilon); + EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon); + EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon); } TEST(Translation2dTest, Difference) { @@ -27,51 +27,51 @@ TEST(Translation2dTest, Difference) { const auto difference = one - two; - EXPECT_NEAR(difference.X().to(), -1.0, kEpsilon); - EXPECT_NEAR(difference.Y().to(), -2.0, kEpsilon); + EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon); + EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon); } TEST(Translation2dTest, RotateBy) { const Translation2d another{3.0_m, 0.0_m}; const auto rotated = another.RotateBy(Rotation2d(90.0_deg)); - EXPECT_NEAR(rotated.X().to(), 0.0, kEpsilon); - EXPECT_NEAR(rotated.Y().to(), 3.0, kEpsilon); + EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon); + EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon); } TEST(Translation2dTest, Multiplication) { const Translation2d original{3.0_m, 5.0_m}; const auto mult = original * 3; - EXPECT_NEAR(mult.X().to(), 9.0, kEpsilon); - EXPECT_NEAR(mult.Y().to(), 15.0, kEpsilon); + EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon); + EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon); } TEST(Translation2dTest, Division) { const Translation2d original{3.0_m, 5.0_m}; const auto div = original / 2; - EXPECT_NEAR(div.X().to(), 1.5, kEpsilon); - EXPECT_NEAR(div.Y().to(), 2.5, kEpsilon); + EXPECT_NEAR(div.X().value(), 1.5, kEpsilon); + EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon); } TEST(Translation2dTest, Norm) { const Translation2d one{3.0_m, 5.0_m}; - EXPECT_NEAR(one.Norm().to(), std::hypot(3, 5), kEpsilon); + EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon); } TEST(Translation2dTest, Distance) { const Translation2d one{1_m, 1_m}; const Translation2d two{6_m, 6_m}; - EXPECT_NEAR(one.Distance(two).to(), 5 * std::sqrt(2), kEpsilon); + EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon); } TEST(Translation2dTest, UnaryMinus) { const Translation2d original{-4.5_m, 7_m}; const auto inverted = -original; - EXPECT_NEAR(inverted.X().to(), 4.5, kEpsilon); - EXPECT_NEAR(inverted.Y().to(), -7, kEpsilon); + EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon); + EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon); } TEST(Translation2dTest, Equality) { @@ -88,10 +88,10 @@ TEST(Translation2dTest, Inequality) { TEST(Translation2dTest, PolarConstructor) { Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)}; - EXPECT_NEAR(one.X().to(), 1.0, kEpsilon); - EXPECT_NEAR(one.Y().to(), 1.0, kEpsilon); + EXPECT_NEAR(one.X().value(), 1.0, kEpsilon); + EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon); Translation2d two{2_m, Rotation2d(60_deg)}; - EXPECT_NEAR(two.X().to(), 1.0, kEpsilon); - EXPECT_NEAR(two.Y().to(), std::sqrt(3.0), kEpsilon); + EXPECT_NEAR(two.X().value(), 1.0, kEpsilon); + EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon); } diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp index 19b56fe183..fa9eecce9d 100644 --- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -17,9 +17,9 @@ TEST(Twist2dTest, Straight) { const Twist2d straight{5.0_m, 0.0_m, 0.0_rad}; const auto straightPose = Pose2d().Exp(straight); - EXPECT_NEAR(straightPose.X().to(), 5.0, kEpsilon); - EXPECT_NEAR(straightPose.Y().to(), 0.0, kEpsilon); - EXPECT_NEAR(straightPose.Rotation().Radians().to(), 0.0, kEpsilon); + EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon); + EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon); + EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon); } TEST(Twist2dTest, QuarterCircle) { @@ -27,19 +27,18 @@ TEST(Twist2dTest, QuarterCircle) { units::radian_t(wpi::numbers::pi / 2.0)}; const auto quarterCirclePose = Pose2d().Exp(quarterCircle); - EXPECT_NEAR(quarterCirclePose.X().to(), 5.0, kEpsilon); - EXPECT_NEAR(quarterCirclePose.Y().to(), 5.0, kEpsilon); - EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to(), 90.0, - kEpsilon); + EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon); + EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon); + EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon); } TEST(Twist2dTest, DiagonalNoDtheta) { const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg}; const auto diagonalPose = Pose2d().Exp(diagonal); - EXPECT_NEAR(diagonalPose.X().to(), 2.0, kEpsilon); - EXPECT_NEAR(diagonalPose.Y().to(), 2.0, kEpsilon); - EXPECT_NEAR(diagonalPose.Rotation().Degrees().to(), 0.0, kEpsilon); + EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon); + EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon); + EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon); } TEST(Twist2dTest, Equality) { @@ -60,7 +59,7 @@ TEST(Twist2dTest, Pose2dLog) { const auto twist = start.Log(end); - EXPECT_NEAR(twist.dx.to(), 5 / 2.0 * wpi::numbers::pi, kEpsilon); - EXPECT_NEAR(twist.dy.to(), 0.0, kEpsilon); - EXPECT_NEAR(twist.dtheta.to(), wpi::numbers::pi / 2.0, kEpsilon); + EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon); + EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon); + EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp index 419540c582..7665a97add 100644 --- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp @@ -11,7 +11,7 @@ TEST(ChassisSpeedsTest, FieldRelativeConstruction) { const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds( 1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg)); - EXPECT_NEAR(0.0, chassisSpeeds.vx.to(), kEpsilon); - EXPECT_NEAR(1.0, chassisSpeeds.vy.to(), kEpsilon); - EXPECT_NEAR(0.5, chassisSpeeds.omega.to(), kEpsilon); + EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon); + EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon); + EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index 3e5999058a..224e231519 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -20,8 +20,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsFromZero) { const ChassisSpeeds chassisSpeeds; const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); - EXPECT_NEAR(wheelSpeeds.left.to(), 0, kEpsilon); - EXPECT_NEAR(wheelSpeeds.right.to(), 0, kEpsilon); + EXPECT_NEAR(wheelSpeeds.left.value(), 0, kEpsilon); + EXPECT_NEAR(wheelSpeeds.right.value(), 0, kEpsilon); } TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) { @@ -29,9 +29,9 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) { const DifferentialDriveWheelSpeeds wheelSpeeds; const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(chassisSpeeds.vx.to(), 0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), 0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon); } TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) { @@ -39,8 +39,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) { const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s}; const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); - EXPECT_NEAR(wheelSpeeds.left.to(), 3, kEpsilon); - EXPECT_NEAR(wheelSpeeds.right.to(), 3, kEpsilon); + EXPECT_NEAR(wheelSpeeds.left.value(), 3, kEpsilon); + EXPECT_NEAR(wheelSpeeds.right.value(), 3, kEpsilon); } TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) { @@ -48,9 +48,9 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) { const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps}; const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(chassisSpeeds.vx.to(), 3, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), 0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 3, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon); } TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) { @@ -59,10 +59,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) { 0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}}; const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); - EXPECT_NEAR(wheelSpeeds.left.to(), -0.381 * wpi::numbers::pi, - kEpsilon); - EXPECT_NEAR(wheelSpeeds.right.to(), +0.381 * wpi::numbers::pi, - kEpsilon); + EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * wpi::numbers::pi, kEpsilon); + EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * wpi::numbers::pi, kEpsilon); } TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) { @@ -72,7 +70,7 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) { units::meters_per_second_t(-0.381 * wpi::numbers::pi)}; const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(chassisSpeeds.vx.to(), 0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), 0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), -wpi::numbers::pi, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), -wpi::numbers::pi, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp index 2ed20a3da0..da16b28482 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp @@ -18,7 +18,7 @@ TEST(DifferentialDriveOdometryTest, EncoderDistances) { const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m, units::meter_t(5 * wpi::numbers::pi)); - EXPECT_NEAR(pose.X().to(), 5.0, kEpsilon); - EXPECT_NEAR(pose.Y().to(), 5.0, kEpsilon); - EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, kEpsilon); + EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon); + EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon); + EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index abf73a6d81..18b72de85e 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -25,38 +25,38 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) { ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds); - EXPECT_NEAR(5.0, moduleStates.frontLeft.to(), 0.1); - EXPECT_NEAR(5.0, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(5.0, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(5.0, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(5.0, moduleStates.frontLeft.value(), 0.1); + EXPECT_NEAR(5.0, moduleStates.frontRight.value(), 0.1); + EXPECT_NEAR(5.0, moduleStates.rearLeft.value(), 0.1); + EXPECT_NEAR(5.0, moduleStates.rearRight.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) { MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_mps}; auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(5.0, chassisSpeeds.vx.to(), 0.1); - EXPECT_NEAR(0.0, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(0.0, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(5.0, chassisSpeeds.vx.value(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) { ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds); - EXPECT_NEAR(-4.0, moduleStates.frontLeft.to(), 0.1); - EXPECT_NEAR(4.0, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(4.0, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(-4.0, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(-4.0, moduleStates.frontLeft.value(), 0.1); + EXPECT_NEAR(4.0, moduleStates.frontRight.value(), 0.1); + EXPECT_NEAR(4.0, moduleStates.rearLeft.value(), 0.1); + EXPECT_NEAR(-4.0, moduleStates.rearRight.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) { MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_mps}; auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(0.0, chassisSpeeds.vx.to(), 0.1); - EXPECT_NEAR(5.0, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(0.0, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1); + EXPECT_NEAR(5.0, chassisSpeeds.vy.value(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) { @@ -64,10 +64,10 @@ TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) { units::radians_per_second_t(2 * wpi::numbers::pi)}; auto moduleStates = kinematics.ToWheelSpeeds(speeds); - EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.to(), 0.1); - EXPECT_NEAR(150.79644737, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(150.79644737, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1); + EXPECT_NEAR(150.79644737, moduleStates.frontRight.value(), 0.1); + EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.value(), 0.1); + EXPECT_NEAR(150.79644737, moduleStates.rearRight.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) { @@ -75,19 +75,19 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) { -150.79644737_mps, 150.79644737_mps}; auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(0.0, chassisSpeeds.vx.to(), 0.1); - EXPECT_NEAR(0.0, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1); + EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1); + EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) { ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds); - EXPECT_NEAR(-25.0, moduleStates.frontLeft.to(), 0.1); - EXPECT_NEAR(29.0, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-19.0, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(23.0, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(-25.0, moduleStates.frontLeft.value(), 0.1); + EXPECT_NEAR(29.0, moduleStates.frontRight.value(), 0.1); + EXPECT_NEAR(-19.0, moduleStates.rearLeft.value(), 0.1); + EXPECT_NEAR(23.0, moduleStates.rearRight.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) { @@ -96,19 +96,19 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) { auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(1.41335, chassisSpeeds.vx.to(), 0.1); - EXPECT_NEAR(2.1221, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(0.707, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(1.41335, chassisSpeeds.vx.value(), 0.1); + EXPECT_NEAR(2.1221, chassisSpeeds.vy.value(), 0.1); + EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl); - EXPECT_NEAR(0, moduleStates.frontLeft.to(), 0.1); - EXPECT_NEAR(24.0, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-24.0, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(48.0, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(0, moduleStates.frontLeft.value(), 0.1); + EXPECT_NEAR(24.0, moduleStates.frontRight.value(), 0.1); + EXPECT_NEAR(-24.0, moduleStates.rearLeft.value(), 0.1); + EXPECT_NEAR(48.0, moduleStates.rearRight.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) { @@ -116,9 +116,9 @@ TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) { 33.941_mps}; auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(8.48525, chassisSpeeds.vx.to(), 0.1); - EXPECT_NEAR(-8.48525, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(0.707, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(8.48525, chassisSpeeds.vx.value(), 0.1); + EXPECT_NEAR(-8.48525, chassisSpeeds.vy.value(), 0.1); + EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, @@ -126,10 +126,10 @@ TEST_F(MecanumDriveKinematicsTest, ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s}; auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl); - EXPECT_NEAR(3.0, moduleStates.frontLeft.to(), 0.1); - EXPECT_NEAR(31.0, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-17.0, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(51.0, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(3.0, moduleStates.frontLeft.value(), 0.1); + EXPECT_NEAR(31.0, moduleStates.frontRight.value(), 0.1); + EXPECT_NEAR(-17.0, moduleStates.rearLeft.value(), 0.1); + EXPECT_NEAR(51.0, moduleStates.rearRight.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, @@ -138,9 +138,9 @@ TEST_F(MecanumDriveKinematicsTest, 36.06_mps}; auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); - EXPECT_NEAR(12.02, chassisSpeeds.vx.to(), 0.1); - EXPECT_NEAR(-7.07, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(0.707, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(12.02, chassisSpeeds.vx.value(), 0.1); + EXPECT_NEAR(-7.07, chassisSpeeds.vy.value(), 0.1); + EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1); } TEST_F(MecanumDriveKinematicsTest, Normalize) { @@ -149,8 +149,8 @@ TEST_F(MecanumDriveKinematicsTest, Normalize) { double kFactor = 5.5 / 7.0; - EXPECT_NEAR(wheelSpeeds.frontLeft.to(), 5.0 * kFactor, 1E-9); - EXPECT_NEAR(wheelSpeeds.frontRight.to(), 6.0 * kFactor, 1E-9); - EXPECT_NEAR(wheelSpeeds.rearLeft.to(), 4.0 * kFactor, 1E-9); - EXPECT_NEAR(wheelSpeeds.rearRight.to(), 7.0 * kFactor, 1E-9); + EXPECT_NEAR(wheelSpeeds.frontLeft.value(), 5.0 * kFactor, 1E-9); + EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9); + EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9); + EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9); } diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp index 61536d89d3..152506db18 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -26,9 +26,9 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) { odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds); auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds); - EXPECT_NEAR(secondPose.X().to(), 0.0, 0.01); - EXPECT_NEAR(secondPose.Y().to(), 0.0, 0.01); - EXPECT_NEAR(secondPose.Rotation().Radians().to(), 0.0, 0.01); + EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Rotation().Radians().value(), 0.0, 0.01); } TEST_F(MecanumDriveOdometryTest, TwoIterations) { @@ -38,9 +38,9 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) { odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds); - EXPECT_NEAR(pose.X().to(), 0.3536, 0.01); - EXPECT_NEAR(pose.Y().to(), 0.0, 0.01); - EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, 0.01); + EXPECT_NEAR(pose.X().value(), 0.3536, 0.01); + EXPECT_NEAR(pose.Y().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01); } TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) { @@ -50,9 +50,9 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) { odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds); - EXPECT_NEAR(pose.X().to(), 8.4855, 0.01); - EXPECT_NEAR(pose.Y().to(), 8.4855, 0.01); - EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, 0.01); + EXPECT_NEAR(pose.X().value(), 8.4855, 0.01); + EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01); + EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, 0.01); } TEST_F(MecanumDriveOdometryTest, GyroAngleReset) { @@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) { odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{}); auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds); - EXPECT_NEAR(pose.X().to(), 0.3536, 0.01); - EXPECT_NEAR(pose.Y().to(), 0.0, 0.01); - EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, 0.01); + EXPECT_NEAR(pose.X().value(), 0.3536, 0.01); + EXPECT_NEAR(pose.Y().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01); } diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index f63ddbb32e..9384d891ee 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -28,15 +28,15 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) { auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); - EXPECT_NEAR(fl.speed.to(), 5.0, kEpsilon); - EXPECT_NEAR(fr.speed.to(), 5.0, kEpsilon); - EXPECT_NEAR(bl.speed.to(), 5.0, kEpsilon); - EXPECT_NEAR(br.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon); + EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon); + EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon); + EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon); - EXPECT_NEAR(fl.angle.Radians().to(), 0.0, kEpsilon); - EXPECT_NEAR(fr.angle.Radians().to(), 0.0, kEpsilon); - EXPECT_NEAR(bl.angle.Radians().to(), 0.0, kEpsilon); - EXPECT_NEAR(br.angle.Radians().to(), 0.0, kEpsilon); + EXPECT_NEAR(fl.angle.Radians().value(), 0.0, kEpsilon); + EXPECT_NEAR(fr.angle.Radians().value(), 0.0, kEpsilon); + EXPECT_NEAR(bl.angle.Radians().value(), 0.0, kEpsilon); + EXPECT_NEAR(br.angle.Radians().value(), 0.0, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) { @@ -44,33 +44,33 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) { auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state); - EXPECT_NEAR(chassisSpeeds.vx.to(), 5.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), 0.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 5.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) { ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s}; auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); - EXPECT_NEAR(fl.speed.to(), 5.0, kEpsilon); - EXPECT_NEAR(fr.speed.to(), 5.0, kEpsilon); - EXPECT_NEAR(bl.speed.to(), 5.0, kEpsilon); - EXPECT_NEAR(br.speed.to(), 5.0, kEpsilon); + EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon); + EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon); + EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon); + EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon); - EXPECT_NEAR(fl.angle.Degrees().to(), 90.0, kEpsilon); - EXPECT_NEAR(fr.angle.Degrees().to(), 90.0, kEpsilon); - EXPECT_NEAR(bl.angle.Degrees().to(), 90.0, kEpsilon); - EXPECT_NEAR(br.angle.Degrees().to(), 90.0, kEpsilon); + EXPECT_NEAR(fl.angle.Degrees().value(), 90.0, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().value(), 90.0, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().value(), 90.0, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().value(), 90.0, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) { SwerveModuleState state{5_mps, Rotation2d(90_deg)}; auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state); - EXPECT_NEAR(chassisSpeeds.vx.to(), 0.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), 5.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), 5.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) { @@ -78,15 +78,15 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) { units::radians_per_second_t(2 * wpi::numbers::pi)}; auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); - EXPECT_NEAR(fl.speed.to(), 106.63, kEpsilon); - EXPECT_NEAR(fr.speed.to(), 106.63, kEpsilon); - EXPECT_NEAR(bl.speed.to(), 106.63, kEpsilon); - EXPECT_NEAR(br.speed.to(), 106.63, kEpsilon); + EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon); + EXPECT_NEAR(fr.speed.value(), 106.63, kEpsilon); + EXPECT_NEAR(bl.speed.value(), 106.63, kEpsilon); + EXPECT_NEAR(br.speed.value(), 106.63, kEpsilon); - EXPECT_NEAR(fl.angle.Degrees().to(), 135.0, kEpsilon); - EXPECT_NEAR(fr.angle.Degrees().to(), 45.0, kEpsilon); - EXPECT_NEAR(bl.angle.Degrees().to(), -135.0, kEpsilon); - EXPECT_NEAR(br.angle.Degrees().to(), -45.0, kEpsilon); + EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) { @@ -97,9 +97,9 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) { auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br); - EXPECT_NEAR(chassisSpeeds.vx.to(), 0.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), 0.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::numbers::pi, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) { @@ -107,15 +107,15 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) { units::radians_per_second_t(2 * wpi::numbers::pi)}; auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl); - EXPECT_NEAR(fl.speed.to(), 0.0, kEpsilon); - EXPECT_NEAR(fr.speed.to(), 150.796, kEpsilon); - EXPECT_NEAR(bl.speed.to(), 150.796, kEpsilon); - EXPECT_NEAR(br.speed.to(), 213.258, kEpsilon); + EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon); + EXPECT_NEAR(fr.speed.value(), 150.796, kEpsilon); + EXPECT_NEAR(bl.speed.value(), 150.796, kEpsilon); + EXPECT_NEAR(br.speed.value(), 213.258, kEpsilon); - EXPECT_NEAR(fl.angle.Degrees().to(), 0.0, kEpsilon); - EXPECT_NEAR(fr.angle.Degrees().to(), 0.0, kEpsilon); - EXPECT_NEAR(bl.angle.Degrees().to(), -90.0, kEpsilon); - EXPECT_NEAR(br.angle.Degrees().to(), -45.0, kEpsilon); + EXPECT_NEAR(fl.angle.Degrees().value(), 0.0, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().value(), 0.0, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().value(), -90.0, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) { @@ -126,9 +126,9 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) { auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br); - EXPECT_NEAR(chassisSpeeds.vx.to(), 75.398, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), -75.398, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::numbers::pi, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, @@ -137,15 +137,15 @@ TEST_F(SwerveDriveKinematicsTest, auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m)); - EXPECT_NEAR(fl.speed.to(), 23.43, kEpsilon); - EXPECT_NEAR(fr.speed.to(), 23.43, kEpsilon); - EXPECT_NEAR(bl.speed.to(), 54.08, kEpsilon); - EXPECT_NEAR(br.speed.to(), 54.08, kEpsilon); + EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon); + EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon); + EXPECT_NEAR(bl.speed.value(), 54.08, kEpsilon); + EXPECT_NEAR(br.speed.value(), 54.08, kEpsilon); - EXPECT_NEAR(fl.angle.Degrees().to(), -140.19, kEpsilon); - EXPECT_NEAR(fr.angle.Degrees().to(), -39.81, kEpsilon); - EXPECT_NEAR(bl.angle.Degrees().to(), -109.44, kEpsilon); - EXPECT_NEAR(br.angle.Degrees().to(), -70.56, kEpsilon); + EXPECT_NEAR(fl.angle.Degrees().value(), -140.19, kEpsilon); + EXPECT_NEAR(fr.angle.Degrees().value(), -39.81, kEpsilon); + EXPECT_NEAR(bl.angle.Degrees().value(), -109.44, kEpsilon); + EXPECT_NEAR(br.angle.Degrees().value(), -70.56, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, @@ -157,9 +157,9 @@ TEST_F(SwerveDriveKinematicsTest, auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br); - EXPECT_NEAR(chassisSpeeds.vx.to(), 0.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.vy.to(), -33.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 1.5, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.vy.value(), -33.0, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, Normalize) { @@ -173,8 +173,8 @@ TEST_F(SwerveDriveKinematicsTest, Normalize) { double kFactor = 5.5 / 7.0; - EXPECT_NEAR(arr[0].speed.to(), 5.0 * kFactor, kEpsilon); - EXPECT_NEAR(arr[1].speed.to(), 6.0 * kFactor, kEpsilon); - EXPECT_NEAR(arr[2].speed.to(), 4.0 * kFactor, kEpsilon); - EXPECT_NEAR(arr[3].speed.to(), 7.0 * kFactor, kEpsilon); + EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon); + EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon); + EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon); + EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp index 279a80c462..27d2a6e7d9 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -31,9 +31,9 @@ TEST_F(SwerveDriveOdometryTest, TwoIterations) { auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state, state, state); - EXPECT_NEAR(0.5, pose.X().to(), kEpsilon); - EXPECT_NEAR(0.0, pose.Y().to(), kEpsilon); - EXPECT_NEAR(0.0, pose.Rotation().Degrees().to(), kEpsilon); + EXPECT_NEAR(0.5, pose.X().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon); } TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) { @@ -49,9 +49,9 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) { auto pose = m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br); - EXPECT_NEAR(12.0, pose.X().to(), kEpsilon); - EXPECT_NEAR(12.0, pose.Y().to(), kEpsilon); - EXPECT_NEAR(90.0, pose.Rotation().Degrees().to(), kEpsilon); + EXPECT_NEAR(12.0, pose.X().value(), kEpsilon); + EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon); + EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon); } TEST_F(SwerveDriveOdometryTest, GyroAngleReset) { @@ -65,7 +65,7 @@ TEST_F(SwerveDriveOdometryTest, GyroAngleReset) { auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state, state, state); - EXPECT_NEAR(0.5, pose.X().to(), kEpsilon); - EXPECT_NEAR(0.0, pose.Y().to(), kEpsilon); - EXPECT_NEAR(0.0, pose.Rotation().Degrees().to(), kEpsilon); + EXPECT_NEAR(0.5, pose.X().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp index d1ef188cbf..4880bef5a3 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp @@ -13,15 +13,15 @@ TEST(SwerveModuleStateTest, Optimize) { frc::SwerveModuleState refA{-2_mps, 180_deg}; auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA); - EXPECT_NEAR(optimizedA.speed.to(), 2.0, kEpsilon); - EXPECT_NEAR(optimizedA.angle.Degrees().to(), 0.0, kEpsilon); + EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon); + EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon); frc::Rotation2d angleB{-50_deg}; frc::SwerveModuleState refB{4.7_mps, 41_deg}; auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB); - EXPECT_NEAR(optimizedB.speed.to(), -4.7, kEpsilon); - EXPECT_NEAR(optimizedB.angle.Degrees().to(), -139.0, kEpsilon); + EXPECT_NEAR(optimizedB.speed.value(), -4.7, kEpsilon); + EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon); } TEST(SwerveModuleStateTest, NoOptimize) { @@ -29,13 +29,13 @@ TEST(SwerveModuleStateTest, NoOptimize) { frc::SwerveModuleState refA{2_mps, 89_deg}; auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA); - EXPECT_NEAR(optimizedA.speed.to(), 2.0, kEpsilon); - EXPECT_NEAR(optimizedA.angle.Degrees().to(), 89.0, kEpsilon); + EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon); + EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon); frc::Rotation2d angleB{0_deg}; frc::SwerveModuleState refB{-2_mps, -2_deg}; auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB); - EXPECT_NEAR(optimizedB.speed.to(), -2.0, kEpsilon); - EXPECT_NEAR(optimizedB.angle.Degrees().to(), -2.0, kEpsilon); + EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon); + EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp index d7d6b88187..69e202fab6 100644 --- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp +++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -53,27 +53,26 @@ class CubicHermiteSplineTest : public ::testing::Test { // Make sure the twist is under the tolerance defined by the Spline class. auto twist = p0.first.Log(p1.first); - EXPECT_LT(std::abs(twist.dx.to()), - SplineParameterizer::kMaxDx.to()); - EXPECT_LT(std::abs(twist.dy.to()), - SplineParameterizer::kMaxDy.to()); - EXPECT_LT(std::abs(twist.dtheta.to()), - SplineParameterizer::kMaxDtheta.to()); + EXPECT_LT(std::abs(twist.dx.value()), + SplineParameterizer::kMaxDx.value()); + EXPECT_LT(std::abs(twist.dy.value()), + SplineParameterizer::kMaxDy.value()); + EXPECT_LT(std::abs(twist.dtheta.value()), + SplineParameterizer::kMaxDtheta.value()); } // Check first point. - EXPECT_NEAR(poses.front().first.X().to(), a.X().to(), 1E-9); - EXPECT_NEAR(poses.front().first.Y().to(), a.Y().to(), 1E-9); - EXPECT_NEAR(poses.front().first.Rotation().Radians().to(), - a.Rotation().Radians().to(), 1E-9); + EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9); + EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9); + EXPECT_NEAR(poses.front().first.Rotation().Radians().value(), + a.Rotation().Radians().value(), 1E-9); // Check interior waypoints bool interiorsGood = true; for (auto& waypoint : waypoints) { bool found = false; for (auto& state : poses) { - if (std::abs( - waypoint.Distance(state.first.Translation()).to()) < + if (std::abs(waypoint.Distance(state.first.Translation()).value()) < 1E-9) { found = true; } @@ -84,10 +83,10 @@ class CubicHermiteSplineTest : public ::testing::Test { EXPECT_TRUE(interiorsGood); // Check last point. - EXPECT_NEAR(poses.back().first.X().to(), b.X().to(), 1E-9); - EXPECT_NEAR(poses.back().first.Y().to(), b.Y().to(), 1E-9); - EXPECT_NEAR(poses.back().first.Rotation().Radians().to(), - b.Rotation().Radians().to(), 1E-9); + EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9); + EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9); + EXPECT_NEAR(poses.back().first.Rotation().Radians().value(), + b.Rotation().Radians().value(), 1E-9); static_cast(duration); } diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp index 5642ced348..25449fb6bb 100644 --- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp +++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -40,25 +40,25 @@ class QuinticHermiteSplineTest : public ::testing::Test { // Make sure the twist is under the tolerance defined by the Spline class. auto twist = p0.first.Log(p1.first); - EXPECT_LT(std::abs(twist.dx.to()), - SplineParameterizer::kMaxDx.to()); - EXPECT_LT(std::abs(twist.dy.to()), - SplineParameterizer::kMaxDy.to()); - EXPECT_LT(std::abs(twist.dtheta.to()), - SplineParameterizer::kMaxDtheta.to()); + EXPECT_LT(std::abs(twist.dx.value()), + SplineParameterizer::kMaxDx.value()); + EXPECT_LT(std::abs(twist.dy.value()), + SplineParameterizer::kMaxDy.value()); + EXPECT_LT(std::abs(twist.dtheta.value()), + SplineParameterizer::kMaxDtheta.value()); } // Check first point. - EXPECT_NEAR(poses.front().first.X().to(), a.X().to(), 1E-9); - EXPECT_NEAR(poses.front().first.Y().to(), a.Y().to(), 1E-9); - EXPECT_NEAR(poses.front().first.Rotation().Radians().to(), - a.Rotation().Radians().to(), 1E-9); + EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9); + EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9); + EXPECT_NEAR(poses.front().first.Rotation().Radians().value(), + a.Rotation().Radians().value(), 1E-9); // Check last point. - EXPECT_NEAR(poses.back().first.X().to(), b.X().to(), 1E-9); - EXPECT_NEAR(poses.back().first.Y().to(), b.Y().to(), 1E-9); - EXPECT_NEAR(poses.back().first.Rotation().Radians().to(), - b.Rotation().Radians().to(), 1E-9); + EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9); + EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9); + EXPECT_NEAR(poses.back().first.Rotation().Radians().value(), + b.Rotation().Radians().value(), 1E-9); static_cast(duration); } diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index e24517d4ca..b5a0fdc807 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -66,8 +66,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) { Eigen::Matrix>( [&](units::second_t t, const Eigen::Matrix&) { return Eigen::Matrix( - (contA * t.to()).exp() * contQ * - (contA.transpose() * t.to()).exp()); + (contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, 0_s, Eigen::Matrix::Zero(), dt); @@ -96,8 +96,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) { Eigen::Matrix>( [&](units::second_t t, const Eigen::Matrix&) { return Eigen::Matrix( - (contA * t.to()).exp() * contQ * - (contA.transpose() * t.to()).exp()); + (contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, 0_s, Eigen::Matrix::Zero(), dt); @@ -134,8 +134,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) { Eigen::Matrix>( [&](units::second_t t, const Eigen::Matrix&) { return Eigen::Matrix( - (contA * t.to()).exp() * contQ * - (contA.transpose() * t.to()).exp()); + (contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, 0_s, Eigen::Matrix::Zero(), dt); @@ -178,8 +178,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) { Eigen::Matrix>( [&](units::second_t t, const Eigen::Matrix&) { return Eigen::Matrix( - (contA * t.to()).exp() * contQ * - (contA.transpose() * t.to()).exp()); + (contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); }, 0_s, Eigen::Matrix::Zero(), dt); diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp index 8a7217618f..f1be861580 100644 --- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp +++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp @@ -28,7 +28,7 @@ TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) { Eigen::Vector y1 = frc::RungeKuttaTimeVarying( [](units::second_t t, const Eigen::Vector& x) { return Eigen::Vector{ - x(0) * (2.0 / (std::exp(t.to()) + 1.0) - 1.0)}; + x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)}; }, 5_s, y0, 1_s); EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3); diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp index 108ad9c765..2b733a8642 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp @@ -19,8 +19,8 @@ TEST(TrajectoryConcatenateTest, States) { const auto& state = t.States()[i]; // Make sure that the timestamps are strictly increasing. - EXPECT_GT(state.t.to(), time); - time = state.t.to(); + EXPECT_GT(state.t.value(), time); + time = state.t.value(); // Ensure that the states in t are the same as those in t1 and t2. if (i < t1.States().size()) { diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp index 40a6e9d523..0c6e07a0db 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp @@ -21,10 +21,10 @@ void TestSameShapedTrajectory(std::vector statesA, auto a = a2.RelativeTo(a1); auto b = b2.RelativeTo(b1); - EXPECT_NEAR(a.X().to(), b.X().to(), 1E-9); - EXPECT_NEAR(a.Y().to(), b.Y().to(), 1E-9); - EXPECT_NEAR(a.Rotation().Radians().to(), - b.Rotation().Radians().to(), 1E-9); + EXPECT_NEAR(a.X().value(), b.X().value(), 1E-9); + EXPECT_NEAR(a.Y().value(), b.Y().value(), 1E-9); + EXPECT_NEAR(a.Rotation().Radians().value(), b.Rotation().Radians().value(), + 1E-9); } } @@ -39,9 +39,9 @@ TEST(TrajectoryTransformsTest, TransformBy) { auto firstPose = transformedTrajectory.Sample(0_s).pose; - EXPECT_NEAR(firstPose.X().to(), 1.0, 1E-9); - EXPECT_NEAR(firstPose.Y().to(), 2.0, 1E-9); - EXPECT_NEAR(firstPose.Rotation().Degrees().to(), 30.0, 1E-9); + EXPECT_NEAR(firstPose.X().value(), 1.0, 1E-9); + EXPECT_NEAR(firstPose.Y().value(), 2.0, 1E-9); + EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 30.0, 1E-9); TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States()); } @@ -57,9 +57,9 @@ TEST(TrajectoryTransformsTest, RelativeTo) { auto firstPose = transformedTrajectory.Sample(0_s).pose; - EXPECT_NEAR(firstPose.X().to(), 0, 1E-9); - EXPECT_NEAR(firstPose.Y().to(), 0, 1E-9); - EXPECT_NEAR(firstPose.Rotation().Degrees().to(), 0, 1E-9); + EXPECT_NEAR(firstPose.X().value(), 0, 1E-9); + EXPECT_NEAR(firstPose.Y().value(), 0, 1E-9); + EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 0, 1E-9); TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States()); } diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h index 19cb9798d7..23a20e80d9 100644 --- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h +++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h @@ -21,7 +21,7 @@ namespace frc { */ template T RungeKuttaTimeVarying(F&& f, units::second_t t, T y, units::second_t dt) { - const auto h = dt.to(); + const auto h = dt.value(); T k1 = f(t, y); T k2 = f(t + dt * 0.5, y + h * k1 * 0.5);