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

SCRIPT: wpiformat

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

27
wpimath/src/main/native/include/wpi/math/controller/ArmFeedforward.hpp
View File

@@ -23,13 +23,13 @@ class WPILIB_DLLEXPORT ArmFeedforward {
public:
using Angle = wpi::units::radians;
using Velocity = wpi::units::radians_per_second;
using Acceleration = wpi::units::compound_unit<wpi::units::radians_per_second,
wpi::units::inverse<wpi::units::second>>;
using kv_unit =
wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<wpi::units::radians_per_second>>;
using ka_unit =
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
using Acceleration =
wpi::units::compound_unit<wpi::units::radians_per_second,
wpi::units::inverse<wpi::units::second>>;
using kv_unit = wpi::units::compound_unit<
wpi::units::volts, wpi::units::inverse<wpi::units::radians_per_second>>;
using ka_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Acceleration>>;
/**
* Creates a new ArmFeedforward with the specified gains.
@@ -44,7 +44,8 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ArmFeedforward(
wpi::units::volt_t kS, wpi::units::volt_t kG, wpi::units::unit_t<kv_unit> kV,
wpi::units::volt_t kS, wpi::units::volt_t kG,
wpi::units::unit_t<kv_unit> kV,
wpi::units::unit_t<ka_unit> kA = wpi::units::unit_t<ka_unit>(0),
wpi::units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
@@ -104,9 +105,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
*/
[[deprecated("Use the current/next velocity overload instead.")]]
wpi::units::volt_t Calculate(wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
return Calculate(currentAngle, currentVelocity, nextVelocity);
}
@@ -141,8 +142,8 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @return The computed feedforward in volts.
*/
wpi::units::volt_t Calculate(wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const;
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const;
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:

4
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp
View File

@@ -84,8 +84,8 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
*/
DifferentialDriveWheelVoltages Calculate(
wpi::units::meters_per_second_t leftVelocity,
wpi::units::meters_per_second_t rightVelocity, wpi::units::volt_t leftVoltage,
wpi::units::volt_t rightVoltage);
wpi::units::meters_per_second_t rightVelocity,
wpi::units::volt_t leftVoltage, wpi::units::volt_t rightVoltage);
private:
LinearSystem<2, 2, 2> m_system;

3
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp
View File

@@ -89,7 +89,8 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
wpi::units::meters_per_second_t currentLeftVelocity,
wpi::units::meters_per_second_t nextLeftVelocity,
wpi::units::meters_per_second_t currentRightVelocity,
wpi::units::meters_per_second_t nextRightVelocity, wpi::units::second_t dt);
wpi::units::meters_per_second_t nextRightVelocity,
wpi::units::second_t dt);
decltype(1_V / 1_mps) m_kVLinear;
decltype(1_V / 1_mps_sq) m_kALinear;

32
wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp
View File

@@ -22,12 +22,15 @@ class ElevatorFeedforward {
public:
using Distance = wpi::units::meters;
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Velocity>>;
using ka_unit =
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Velocity>>;
using ka_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Acceleration>>;
/**
* Creates a new ElevatorFeedforward with the specified gains.
@@ -42,7 +45,8 @@ class ElevatorFeedforward {
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ElevatorFeedforward(
wpi::units::volt_t kS, wpi::units::volt_t kG, wpi::units::unit_t<kv_unit> kV,
wpi::units::volt_t kS, wpi::units::volt_t kG,
wpi::units::unit_t<kv_unit> kV,
wpi::units::unit_t<ka_unit> kA = wpi::units::unit_t<ka_unit>(0),
wpi::units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
@@ -79,7 +83,8 @@ class ElevatorFeedforward {
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> velocity,
wpi::units::unit_t<Acceleration> acceleration) const {
return kS * wpi::util::sgn(velocity) + kG + kV * velocity + kA * acceleration;
return kS * wpi::util::sgn(velocity) + kG + kV * velocity +
kA * acceleration;
}
/**
@@ -93,8 +98,8 @@ class ElevatorFeedforward {
*/
[[deprecated("Use the current/next velocity overload instead.")]]
wpi::units::volt_t Calculate(wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
@@ -161,7 +166,8 @@ class ElevatorFeedforward {
* @return The maximum possible velocity at the given acceleration.
*/
constexpr wpi::units::unit_t<Velocity> MaxAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Acceleration> acceleration) {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kG - kA * acceleration) / kV;
}
@@ -178,7 +184,8 @@ class ElevatorFeedforward {
* @return The minimum possible velocity at the given acceleration.
*/
constexpr wpi::units::unit_t<Velocity> MinAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Acceleration> acceleration) {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
return (-maxVoltage + kS - kG - kA * acceleration) / kV;
}
@@ -196,7 +203,8 @@ class ElevatorFeedforward {
*/
constexpr wpi::units::unit_t<Acceleration> MaxAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::util::sgn(velocity) - kG - kV * velocity) / kA;
return (maxVoltage - kS * wpi::util::sgn(velocity) - kG - kV * velocity) /
kA;
}
/**

3
wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp
View File

@@ -54,7 +54,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param dt Discretization timestep.
*/
LTVUnicycleController(const wpi::util::array<double, 3>& Qelems,
const wpi::util::array<double, 2>& Relems, wpi::units::second_t dt)
const wpi::util::array<double, 2>& Relems,
wpi::units::second_t dt)
: m_Q{wpi::math::MakeCostMatrix(Qelems)},
m_R{wpi::math::MakeCostMatrix(Relems)},
m_dt{dt} {}

3
wpimath/src/main/native/include/wpi/math/controller/LinearPlantInversionFeedforward.hpp
View File

@@ -44,7 +44,8 @@ class LinearPlantInversionFeedforward {
*/
template <int Outputs>
LinearPlantInversionFeedforward(
const LinearSystem<States, Inputs, Outputs>& plant, wpi::units::second_t dt)
const LinearSystem<States, Inputs, Outputs>& plant,
wpi::units::second_t dt)
: LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}
/**

3
wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp
View File

@@ -291,7 +291,8 @@ class LinearQuadraticRegulator {
*/
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
wpi::units::second_t dt, wpi::units::second_t inputDelay) {
wpi::units::second_t dt,
wpi::units::second_t inputDelay) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);

9
wpimath/src/main/native/include/wpi/math/controller/ProfiledPIDController.hpp
View File

@@ -34,10 +34,12 @@ class ProfiledPIDController
public:
using Distance_t = wpi::units::unit_t<Distance>;
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Velocity_t = wpi::units::unit_t<Velocity>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using Acceleration_t = wpi::units::unit_t<Acceleration>;
using State = typename TrapezoidProfile<Distance>::State;
using Constraints = typename TrapezoidProfile<Distance>::Constraints;
@@ -64,7 +66,8 @@ class ProfiledPIDController
int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
wpi::math::MathSharedStore::ReportUsage("ProfiledPIDController",
std::to_string(instances));
wpi::util::SendableRegistry::Add(this, "ProfiledPIDController", instances);
wpi::util::SendableRegistry::Add(this, "ProfiledPIDController",
instances);
}
}

22
wpimath/src/main/native/include/wpi/math/controller/SimpleMotorFeedforward.hpp
View File

@@ -24,12 +24,15 @@ template <class Distance>
class SimpleMotorFeedforward {
public:
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Velocity>>;
using ka_unit =
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Velocity>>;
using ka_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Acceleration>>;
/**
* Creates a new SimpleMotorFeedforward with the specified gains.
@@ -75,7 +78,8 @@ class SimpleMotorFeedforward {
* @param velocity The velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr wpi::units::volt_t Calculate(wpi::units::unit_t<Velocity> velocity) const {
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> velocity) const {
return Calculate(velocity, velocity);
}
@@ -158,7 +162,8 @@ class SimpleMotorFeedforward {
* @return The maximum possible acceleration at the given velocity.
*/
constexpr wpi::units::unit_t<Acceleration> MaxAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) const {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Velocity> velocity) const {
return (maxVoltage - kS * wpi::util::sgn(velocity) - kV * velocity) / kA;
}
@@ -174,7 +179,8 @@ class SimpleMotorFeedforward {
* @return The minimum possible acceleration at the given velocity.
*/
constexpr wpi::units::unit_t<Acceleration> MinAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) const {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Velocity> velocity) const {
return MaxAchievableAcceleration(-maxVoltage, velocity);
}

3
wpimath/src/main/native/include/wpi/math/controller/proto/ArmFeedforwardProto.hpp
View File

@@ -16,5 +16,6 @@ struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ArmFeedforward> {
using InputStream = wpi::util::ProtoInputStream<wpi::math::ArmFeedforward>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ArmFeedforward>;
static std::optional<wpi::math::ArmFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ArmFeedforward& value);
static bool Pack(OutputStream& stream,
const wpi::math::ArmFeedforward& value);
};

6
wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveFeedforwardProto.hpp
View File

@@ -11,9 +11,11 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveFeedforward;
using InputStream = wpi::util::ProtoInputStream<wpi::math::DifferentialDriveFeedforward>;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveFeedforward>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveFeedforward>;
static std::optional<wpi::math::DifferentialDriveFeedforward> Unpack(

3
wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveWheelVoltagesProto.hpp
View File

@@ -11,7 +11,8 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelVoltages;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelVoltages>;

12
wpimath/src/main/native/include/wpi/math/controller/proto/ElevatorFeedforwardProto.hpp
View File

@@ -13,8 +13,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ElevatorFeedforward> {
using MessageStruct = wpi_proto_ProtobufElevatorFeedforward;
using InputStream = wpi::util::ProtoInputStream<wpi::math::ElevatorFeedforward>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ElevatorFeedforward>;
static std::optional<wpi::math::ElevatorFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ElevatorFeedforward& value);
using InputStream =
wpi::util::ProtoInputStream<wpi::math::ElevatorFeedforward>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::ElevatorFeedforward>;
static std::optional<wpi::math::ElevatorFeedforward> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::ElevatorFeedforward& value);
};

16
wpimath/src/main/native/include/wpi/math/controller/proto/SimpleMotorFeedforwardProto.hpp
View File

@@ -26,7 +26,8 @@ struct wpi::util::Protobuf<wpi::math::SimpleMotorFeedforward<Distance>> {
static std::optional<wpi::math::SimpleMotorFeedforward<Distance>> Unpack(
InputStream& stream) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
wpi_proto_ProtobufSimpleMotorFeedforward msg;
if (!stream.Decode(msg)) {
@@ -44,14 +45,17 @@ struct wpi::util::Protobuf<wpi::math::SimpleMotorFeedforward<Distance>> {
static bool Pack(OutputStream& stream,
const wpi::math::SimpleMotorFeedforward<Distance>& value) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
wpi_proto_ProtobufSimpleMotorFeedforward msg{
.ks = value.GetKs().value(),
.kv = wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value(),
.ka = wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value(),
.kv =
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value(),
.ka =
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value(),
.dt = wpi::units::second_t{value.GetDt()}.value(),
};
return stream.Encode(msg);

3
wpimath/src/main/native/include/wpi/math/controller/struct/ArmFeedforwardStruct.hpp
View File

@@ -17,7 +17,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ArmFeedforward> {
}
static wpi::math::ArmFeedforward Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::ArmFeedforward& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::ArmFeedforward& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::ArmFeedforward>);

6
wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveFeedforwardStruct.hpp
View File

@@ -9,7 +9,8 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveFeedforward> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveFeedforward> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveFeedforward";
}
@@ -25,4 +26,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveFeedforwar
const wpi::math::DifferentialDriveFeedforward& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveFeedforward>);
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveFeedforward>);

6
wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveWheelVoltagesStruct.hpp
View File

@@ -9,7 +9,8 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveWheelVoltages";
}
@@ -24,4 +25,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelVolta
const wpi::math::DifferentialDriveWheelVoltages& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelVoltages>);
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelVoltages>);

44
wpimath/src/main/native/include/wpi/math/controller/struct/SimpleMotorFeedforwardStruct.hpp
View File

@@ -26,24 +26,27 @@ struct wpi::util::Struct<wpi::math::SimpleMotorFeedforward<Distance>> {
static wpi::math::SimpleMotorFeedforward<Distance> Unpack(
std::span<const uint8_t> data) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
return {wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{
wpi::util::UnpackStruct<double, kKvOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{
wpi::util::UnpackStruct<double, kKaOff>(data)},
wpi::units::second_t{wpi::util::UnpackStruct<double, kDtOff>(data)}};
return {
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{
wpi::util::UnpackStruct<double, kKvOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{
wpi::util::UnpackStruct<double, kKaOff>(data)},
wpi::units::second_t{wpi::util::UnpackStruct<double, kDtOff>(data)}};
}
static void Pack(std::span<uint8_t> data,
const wpi::math::SimpleMotorFeedforward<Distance>& value) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
@@ -51,18 +54,21 @@ struct wpi::util::Struct<wpi::math::SimpleMotorFeedforward<Distance>> {
constexpr size_t kDtOff = kKaOff + 8;
wpi::util::PackStruct<kKsOff>(data, value.GetKs().value());
wpi::util::PackStruct<kKvOff>(
data, wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value());
data,
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value());
wpi::util::PackStruct<kKaOff>(
data, wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value());
wpi::util::PackStruct<kDtOff>(data, wpi::units::second_t{value.GetDt()}.value());
data,
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value());
wpi::util::PackStruct<kDtOff>(data,
wpi::units::second_t{value.GetDt()}.value());
}
};
static_assert(
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::meters>>);
static_assert(
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::feet>>);
static_assert(
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::radians>>);
static_assert(wpi::util::StructSerializable<
wpi::math::SimpleMotorFeedforward<wpi::units::meters>>);
static_assert(wpi::util::StructSerializable<
wpi::math::SimpleMotorFeedforward<wpi::units::feet>>);
static_assert(wpi::util::StructSerializable<
wpi::math::SimpleMotorFeedforward<wpi::units::radians>>);

6
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp
View File

@@ -75,7 +75,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
DifferentialDrivePoseEstimator(
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const Pose2d& initialPose,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs);
/**
@@ -86,7 +87,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
* @param rightDistance The distance traveled by the right encoder.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
void ResetPosition(const Rotation2d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose2d& pose) {
PoseEstimator::ResetPosition(gyroAngle, {leftDistance, rightDistance},
pose);

6
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp
View File

@@ -80,7 +80,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const Pose3d& initialPose,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs);
/**
@@ -91,7 +92,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
* @param rightDistance The distance traveled by the right encoder.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
void ResetPosition(const Rotation3d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose3d& pose) {
PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance},
pose);

14
wpimath/src/main/native/include/wpi/math/estimator/KalmanFilterLatencyCompensator.hpp
View File

@@ -62,7 +62,8 @@ class KalmanFilterLatencyCompensator {
* @param timestamp The timestamp of the state.
*/
void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
Vectord<Outputs> localY, wpi::units::second_t timestamp) {
Vectord<Outputs> localY,
wpi::units::second_t timestamp) {
// Add the new state into the vector.
m_pastObserverSnapshots.emplace_back(timestamp,
ObserverSnapshot{observer, u, localY});
@@ -86,7 +87,8 @@ class KalmanFilterLatencyCompensator {
*/
template <int Rows>
void ApplyPastGlobalMeasurement(
KalmanFilterType* observer, wpi::units::second_t nominalDt, Vectord<Rows> y,
KalmanFilterType* observer, wpi::units::second_t nominalDt,
Vectord<Rows> y,
std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
globalMeasurementCorrect,
wpi::units::second_t timestamp) {
@@ -130,10 +132,10 @@ class KalmanFilterLatencyCompensator {
int prevIdx = nextIdx - 1;
// Find the snapshot closest in time to global measurement
wpi::units::second_t prevTimeDiff =
wpi::units::math::abs(timestamp - m_pastObserverSnapshots[prevIdx].first);
wpi::units::second_t nextTimeDiff =
wpi::units::math::abs(timestamp - m_pastObserverSnapshots[nextIdx].first);
wpi::units::second_t prevTimeDiff = wpi::units::math::abs(
timestamp - m_pastObserverSnapshots[prevIdx].first);
wpi::units::second_t nextTimeDiff = wpi::units::math::abs(
timestamp - m_pastObserverSnapshots[nextIdx].first);
indexOfClosestEntry = prevTimeDiff < nextTimeDiff ? prevIdx : nextIdx;
}

3
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp
View File

@@ -74,7 +74,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
MecanumDrivePoseEstimator(
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const Pose2d& initialPose,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs);
private:

3
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp
View File

@@ -79,7 +79,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
MecanumDrivePoseEstimator3d(
MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const Pose3d& initialPose,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs);
private:

14
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -279,14 +279,16 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// measurements compared to our current pose.
wpi::math::Vectord<6> k_times_twist =
m_visionK * wpi::math::Vectord<6>{twist.dx.value(), twist.dy.value(),
twist.dz.value(), twist.rx.value(),
twist.ry.value(), twist.rz.value()};
twist.dz.value(), twist.rx.value(),
twist.ry.value(), twist.rz.value()};
// Step 6: Convert back to Twist3d.
Twist3d scaledTwist{
wpi::units::meter_t{k_times_twist(0)}, wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)}, wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)}, wpi::units::radian_t{k_times_twist(5)}};
Twist3d scaledTwist{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)},
wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)},
wpi::units::radian_t{k_times_twist(5)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),

3
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp
View File

@@ -79,7 +79,8 @@ class SwerveDrivePoseEstimator
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation2d& gyroAngle,
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const Pose2d& initialPose,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
: SwerveDrivePoseEstimator::PoseEstimator(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),

8
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp
View File

@@ -34,8 +34,9 @@ namespace wpi::math {
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator3d
: public PoseEstimator3d<wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModulePosition, NumModules>> {
: public PoseEstimator3d<
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModulePosition, NumModules>> {
public:
/**
* Constructs a SwerveDrivePoseEstimator3d with default standard deviations
@@ -85,7 +86,8 @@ class SwerveDrivePoseEstimator3d
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation3d& gyroAngle,
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const Pose3d& initialPose,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
: SwerveDrivePoseEstimator3d::PoseEstimator3d(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),

4
wpimath/src/main/native/include/wpi/math/filter/Debouncer.hpp
View File

@@ -63,7 +63,9 @@ class WPILIB_DLLEXPORT Debouncer {
* @return The number of seconds the value must change from baseline
* for the filtered value to change.
*/
constexpr wpi::units::second_t GetDebounceTime() const { return m_debounceTime; }
constexpr wpi::units::second_t GetDebounceTime() const {
return m_debounceTime;
}
/**
* Set the debounce type.

3
wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp
View File

@@ -191,7 +191,8 @@ class LinearFilter {
*/
template <int Derivative, int Samples>
static LinearFilter<T> FiniteDifference(
const wpi::util::array<int, Samples>& stencil, wpi::units::second_t period) {
const wpi::util::array<int, Samples>& stencil,
wpi::units::second_t period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//

10
wpimath/src/main/native/include/wpi/math/filter/SlewRateLimiter.hpp
View File

@@ -24,7 +24,8 @@ template <class Unit>
class SlewRateLimiter {
public:
using Unit_t = wpi::units::unit_t<Unit>;
using Rate = wpi::units::compound_unit<Unit, wpi::units::inverse<wpi::units::seconds>>;
using Rate =
wpi::units::compound_unit<Unit, wpi::units::inverse<wpi::units::seconds>>;
using Rate_t = wpi::units::unit_t<Rate>;
/**
@@ -44,8 +45,8 @@ class SlewRateLimiter {
: m_positiveRateLimit{positiveRateLimit},
m_negativeRateLimit{negativeRateLimit},
m_prevVal{initialValue},
m_prevTime{
wpi::units::microsecond_t{wpi::math::MathSharedStore::GetTimestamp()}} {}
m_prevTime{wpi::units::microsecond_t{
wpi::math::MathSharedStore::GetTimestamp()}} {}
/**
* Creates a new SlewRateLimiter with the given positive rate limit and
@@ -64,7 +65,8 @@ class SlewRateLimiter {
* rate.
*/
Unit_t Calculate(Unit_t input) {
wpi::units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
wpi::units::second_t currentTime =
wpi::math::MathSharedStore::GetTimestamp();
wpi::units::second_t elapsedTime = currentTime - m_prevTime;
m_prevVal +=
std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,

6
wpimath/src/main/native/include/wpi/math/geometry/Pose2d.hpp
View File

@@ -49,7 +49,8 @@ class WPILIB_DLLEXPORT Pose2d {
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose2d(wpi::units::meter_t x, wpi::units::meter_t y, Rotation2d rotation)
constexpr Pose2d(wpi::units::meter_t x, wpi::units::meter_t y,
Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
@@ -284,7 +285,8 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
return Transform2d{pose.Translation(), pose.Rotation()};
}
constexpr Pose2d Pose2d::TransformBy(const wpi::math::Transform2d& other) const {
constexpr Pose2d Pose2d::TransformBy(
const wpi::math::Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}

4
wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp
View File

@@ -53,8 +53,8 @@ class WPILIB_DLLEXPORT Pose3d {
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
constexpr Pose3d(wpi::units::meter_t x, wpi::units::meter_t y, wpi::units::meter_t z,
Rotation3d rotation)
constexpr Pose3d(wpi::units::meter_t x, wpi::units::meter_t y,
wpi::units::meter_t z, Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
/**

6
wpimath/src/main/native/include/wpi/math/geometry/Rectangle2d.hpp
View File

@@ -114,12 +114,12 @@ class WPILIB_DLLEXPORT Rectangle2d {
auto pointInRect = point - m_center.Translation();
pointInRect = pointInRect.RotateBy(-m_center.Rotation());
if (wpi::units::math::abs(wpi::units::math::abs(pointInRect.X()) - m_xWidth / 2.0) <=
1E-9_m) {
if (wpi::units::math::abs(wpi::units::math::abs(pointInRect.X()) -
m_xWidth / 2.0) <= 1E-9_m) {
// Point rests on left/right perimeter
return wpi::units::math::abs(pointInRect.Y()) <= m_yWidth / 2.0;
} else if (wpi::units::math::abs(wpi::units::math::abs(pointInRect.Y()) -
m_yWidth / 2.0) <= 1E-9_m) {
m_yWidth / 2.0) <= 1E-9_m) {
// Point rests on top/bottom perimeter
return wpi::units::math::abs(pointInRect.X()) <= m_xWidth / 2.0;
}

3
wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp
View File

@@ -37,7 +37,8 @@ class WPILIB_DLLEXPORT Rotation2d {
*/
constexpr Rotation2d(wpi::units::angle_unit auto value) // NOLINT
: m_cos{gcem::cos(value.template convert<wpi::units::radian>().value())},
m_sin{gcem::sin(value.template convert<wpi::units::radian>().value())} {}
m_sin{gcem::sin(value.template convert<wpi::units::radian>().value())} {
}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)

16
wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp
View File

@@ -77,7 +77,8 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
constexpr Rotation3d(const Eigen::Vector3d& axis, wpi::units::radian_t angle) {
constexpr Rotation3d(const Eigen::Vector3d& axis,
wpi::units::radian_t angle) {
double norm = ct_matrix{axis}.norm();
if (norm == 0.0) {
return;
@@ -281,11 +282,13 @@ class WPILIB_DLLEXPORT Rotation3d {
constexpr Rotation3d operator*(double scalar) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.W() >= 0.0) {
return Rotation3d{Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(m_q.W())}};
return Rotation3d{
Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(m_q.W())}};
} else {
return Rotation3d{Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(-m_q.W())}};
return Rotation3d{
Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
2.0 * wpi::units::radian_t{scalar * gcem::acos(-m_q.W())}};
}
}
@@ -360,7 +363,8 @@ class WPILIB_DLLEXPORT Rotation3d {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
double ratio = 2.0 * (w * y - z * x);
if (gcem::abs(ratio) >= 1.0) {
return wpi::units::radian_t{gcem::copysign(std::numbers::pi / 2.0, ratio)};
return wpi::units::radian_t{
gcem::copysign(std::numbers::pi / 2.0, ratio)};
} else {
return wpi::units::radian_t{gcem::asin(ratio)};
}

6
wpimath/src/main/native/include/wpi/math/geometry/Transform2d.hpp
View File

@@ -46,7 +46,8 @@ class WPILIB_DLLEXPORT Transform2d {
* @param y The y component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
constexpr Transform2d(wpi::units::meter_t x, wpi::units::meter_t y, Rotation2d rotation)
constexpr Transform2d(wpi::units::meter_t x, wpi::units::meter_t y,
Rotation2d rotation)
: m_translation{x, y}, m_rotation{std::move(rotation)} {}
/**
@@ -207,7 +208,8 @@ constexpr Twist2d Transform2d::Log() const {
m_translation.RotateBy({halfThetaByTanOfHalfDtheta, -halfDtheta}) *
gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
return {translationPart.X(), translationPart.Y(), wpi::units::radian_t{dtheta}};
return {translationPart.X(), translationPart.Y(),
wpi::units::radian_t{dtheta}};
}
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp
View File

@@ -47,8 +47,8 @@ class WPILIB_DLLEXPORT Transform3d {
* @param z The z component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
constexpr Transform3d(wpi::units::meter_t x, wpi::units::meter_t y, wpi::units::meter_t z,
Rotation3d rotation)
constexpr Transform3d(wpi::units::meter_t x, wpi::units::meter_t y,
wpi::units::meter_t z, Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
/**

7
wpimath/src/main/native/include/wpi/math/geometry/Translation2d.hpp
View File

@@ -61,7 +61,8 @@ class WPILIB_DLLEXPORT Translation2d {
* @param vector The translation vector.
*/
constexpr explicit Translation2d(const Eigen::Vector2d& vector)
: m_x{wpi::units::meter_t{vector.x()}}, m_y{wpi::units::meter_t{vector.y()}} {}
: m_x{wpi::units::meter_t{vector.x()}},
m_y{wpi::units::meter_t{vector.y()}} {}
/**
* Calculates the distance between two translations in 2D space.
@@ -121,7 +122,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The norm of the translation.
*/
constexpr wpi::units::meter_t Norm() const { return wpi::units::math::hypot(m_x, m_y); }
constexpr wpi::units::meter_t Norm() const {
return wpi::units::math::hypot(m_x, m_y);
}
/**
* Returns the squared norm, or squared distance from the origin to the

13
wpimath/src/main/native/include/wpi/math/geometry/Translation3d.hpp
View File

@@ -43,7 +43,8 @@ class WPILIB_DLLEXPORT Translation3d {
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
constexpr Translation3d(wpi::units::meter_t x, wpi::units::meter_t y, wpi::units::meter_t z)
constexpr Translation3d(wpi::units::meter_t x, wpi::units::meter_t y,
wpi::units::meter_t z)
: m_x{x}, m_y{y}, m_z{z} {}
/**
@@ -53,7 +54,8 @@ class WPILIB_DLLEXPORT Translation3d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
constexpr Translation3d(wpi::units::meter_t distance, const Rotation3d& angle) {
constexpr Translation3d(wpi::units::meter_t distance,
const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
m_y = rectangular.Y();
@@ -93,8 +95,8 @@ class WPILIB_DLLEXPORT Translation3d {
*/
constexpr wpi::units::meter_t Distance(const Translation3d& other) const {
return wpi::units::math::sqrt(wpi::units::math::pow<2>(other.m_x - m_x) +
wpi::units::math::pow<2>(other.m_y - m_y) +
wpi::units::math::pow<2>(other.m_z - m_z));
wpi::units::math::pow<2>(other.m_y - m_y) +
wpi::units::math::pow<2>(other.m_z - m_z));
}
/**
@@ -178,7 +180,8 @@ class WPILIB_DLLEXPORT Translation3d {
constexpr Translation3d RotateBy(const Rotation3d& other) const {
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
return Translation3d{wpi::units::meter_t{qprime.X()}, wpi::units::meter_t{qprime.Y()},
return Translation3d{wpi::units::meter_t{qprime.X()},
wpi::units::meter_t{qprime.Y()},
wpi::units::meter_t{qprime.Z()}};
}

3
wpimath/src/main/native/include/wpi/math/geometry/struct/Rectangle2dStruct.hpp
View File

@@ -19,7 +19,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Rectangle2d> {
}
static wpi::math::Rectangle2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Rectangle2d& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::Rectangle2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::util::ForEachStructSchema<wpi::math::Pose2d>(fn);

3
wpimath/src/main/native/include/wpi/math/geometry/struct/Transform2dStruct.hpp
View File

@@ -20,7 +20,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Transform2d> {
}
static wpi::math::Transform2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Transform2d& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::Transform2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::util::ForEachStructSchema<wpi::math::Translation2d>(fn);

3
wpimath/src/main/native/include/wpi/math/geometry/struct/Transform3dStruct.hpp
View File

@@ -20,7 +20,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Transform3d> {
}
static wpi::math::Transform3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Transform3d& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::Transform3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::util::ForEachStructSchema<wpi::math::Translation3d>(fn);

3
wpimath/src/main/native/include/wpi/math/geometry/struct/Translation2dStruct.hpp
View File

@@ -15,7 +15,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Translation2d> {
static constexpr std::string_view GetSchema() { return "double x;double y"; }
static wpi::math::Translation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Translation2d& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::Translation2d& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::Translation2d>);

3
wpimath/src/main/native/include/wpi/math/geometry/struct/Translation3dStruct.hpp
View File

@@ -17,7 +17,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::Translation3d> {
}
static wpi::math::Translation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::Translation3d& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::Translation3d& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::Translation3d>);

6
wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp
View File

@@ -44,7 +44,8 @@ class TimeInterpolatableBuffer {
/**
* Create a new TimeInterpolatableBuffer. By default, the interpolation
* function is wpi::util::Lerp except for Pose2d, which uses the pose exponential.
* function is wpi::util::Lerp except for Pose2d, which uses the pose
* exponential.
*
* @param historySize The history size of the buffer.
*/
@@ -144,7 +145,8 @@ class TimeInterpolatableBuffer {
/**
* Grant access to the internal sample buffer.
*/
const std::vector<std::pair<wpi::units::second_t, T>>& GetInternalBuffer() const {
const std::vector<std::pair<wpi::units::second_t, T>>& GetInternalBuffer()
const {
return m_pastSnapshots;
}

12
wpimath/src/main/native/include/wpi/math/kinematics/ChassisSpeeds.hpp
View File

@@ -93,9 +93,9 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*/
constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated =
Translation2d{wpi::units::meter_t{vx.value()}, wpi::units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
@@ -113,9 +113,9 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*/
constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated =
Translation2d{wpi::units::meter_t{vx.value()}, wpi::units::meter_t{vy.value()}}
.RotateBy(robotAngle);
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}

6
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp
View File

@@ -59,7 +59,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*/
void ResetPosition(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
void ResetPosition(const Rotation2d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose2d& pose) {
Odometry::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
}
@@ -75,7 +76,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry
* @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
const Pose2d& Update(const Rotation2d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return Odometry::Update(gyroAngle, {leftDistance, rightDistance});
}

6
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp
View File

@@ -59,7 +59,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*/
void ResetPosition(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
void ResetPosition(const Rotation3d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose3d& pose) {
Odometry3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
}
@@ -75,7 +76,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
* @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
const Pose3d& Update(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
const Pose3d& Update(const Rotation3d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance) {
return Odometry3d::Update(gyroAngle, {leftDistance, rightDistance});
}

7
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
View File

@@ -35,9 +35,10 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
constexpr void Desaturate(wpi::units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
wpi::units::math::max(wpi::units::math::abs(left), wpi::units::math::abs(right));
constexpr void Desaturate(
wpi::units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed = wpi::units::math::max(wpi::units::math::abs(left),
wpi::units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;

15
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
View File

@@ -52,13 +52,14 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
*/
constexpr MecanumDriveWheelSpeeds Desaturate(
wpi::units::meters_per_second_t attainableMaxSpeed) const {
std::array<wpi::units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
wpi::units::meters_per_second_t realMaxSpeed = wpi::units::math::abs(
*std::max_element(wheelSpeeds.begin(), wheelSpeeds.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a) < wpi::units::math::abs(b);
}));
std::array<wpi::units::meters_per_second_t, 4> wheelSpeeds{
frontLeft, frontRight, rearLeft, rearRight};
wpi::units::meters_per_second_t realMaxSpeed =
wpi::units::math::abs(*std::max_element(
wheelSpeeds.begin(), wheelSpeeds.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a) < wpi::units::math::abs(b);
}));
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {

66
wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
View File

@@ -67,7 +67,8 @@ class SwerveDriveKinematics
template <std::convertible_to<Translation2d>... ModuleTranslations>
requires(sizeof...(ModuleTranslations) == NumModules)
explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations)
: m_modules{moduleTranslations...}, m_moduleHeadings(wpi::util::empty_array) {
: m_modules{moduleTranslations...},
m_moduleHeadings(wpi::util::empty_array) {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -145,9 +146,9 @@ class SwerveDriveKinematics
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
* DesaturateWheelSpeeds(wpi::util::array<SwerveModuleState, NumModules>*,
* wpi::units::meters_per_second_t) function to rectify this issue. In addition,
* you can leverage the power of C++17 to directly assign the module states to
* variables:
* wpi::units::meters_per_second_t) function to rectify this issue. In
* addition, you can leverage the power of C++17 to directly assign the module
* states to variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
@@ -156,7 +157,8 @@ class SwerveDriveKinematics
wpi::util::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
const Translation2d& centerOfRotation = Translation2d{}) const {
wpi::util::array<SwerveModuleState, NumModules> moduleStates(wpi::util::empty_array);
wpi::util::array<SwerveModuleState, NumModules> moduleStates(
wpi::util::empty_array);
if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
chassisSpeeds.omega == 0_rad_per_s) {
@@ -239,8 +241,9 @@ class SwerveDriveKinematics
*
* @return The resulting chassis speed.
*/
ChassisSpeeds ToChassisSpeeds(const wpi::util::array<SwerveModuleState, NumModules>&
moduleStates) const override {
ChassisSpeeds ToChassisSpeeds(
const wpi::util::array<SwerveModuleState, NumModules>& moduleStates)
const override {
Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
@@ -313,9 +316,10 @@ class SwerveDriveKinematics
Twist2d ToTwist2d(
const wpi::util::array<SwerveModulePosition, NumModules>& start,
const wpi::util::array<SwerveModulePosition, NumModules>& end) const override {
auto result =
wpi::util::array<SwerveModulePosition, NumModules>(wpi::util::empty_array);
const wpi::util::array<SwerveModulePosition, NumModules>& end)
const override {
auto result = wpi::util::array<SwerveModulePosition, NumModules>(
wpi::util::empty_array);
for (size_t i = 0; i < NumModules; i++) {
result[i] = {end[i].distance - start[i].distance, end[i].angle};
}
@@ -346,13 +350,13 @@ class SwerveDriveKinematics
wpi::util::array<SwerveModuleState, NumModules>* moduleStates,
wpi::units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed =
wpi::units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
})
->speed);
auto realMaxSpeed = wpi::units::math::abs(
std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
})
->speed);
if (realMaxSpeed > attainableMaxSpeed) {
for (auto& module : states) {
@@ -395,13 +399,13 @@ class SwerveDriveKinematics
wpi::units::radians_per_second_t attainableMaxRobotRotationSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed =
wpi::units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
})
->speed);
auto realMaxSpeed = wpi::units::math::abs(
std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
})
->speed);
if (attainableMaxRobotTranslationSpeed == 0_mps ||
attainableMaxRobotRotationSpeed == 0_rad_per_s ||
@@ -409,17 +413,17 @@ class SwerveDriveKinematics
return;
}
auto translationalK =
wpi::units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed;
auto translationalK = wpi::units::math::hypot(desiredChassisSpeed.vx,
desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed;
auto rotationalK = wpi::units::math::abs(desiredChassisSpeed.omega) /
attainableMaxRobotRotationSpeed;
auto k = wpi::units::math::max(translationalK, rotationalK);
auto scale = wpi::units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
wpi::units::scalar_t{1});
auto scale = wpi::units::math::min(
k * attainableMaxModuleSpeed / realMaxSpeed, wpi::units::scalar_t{1});
for (auto& module : states) {
module.speed = module.speed * scale;
}
@@ -429,8 +433,8 @@ class SwerveDriveKinematics
const wpi::util::array<SwerveModulePosition, NumModules>& start,
const wpi::util::array<SwerveModulePosition, NumModules>& end,
double t) const override {
auto result =
wpi::util::array<SwerveModulePosition, NumModules>(wpi::util::empty_array);
auto result = wpi::util::array<SwerveModulePosition, NumModules>(
wpi::util::empty_array);
for (size_t i = 0; i < NumModules; ++i) {
result[i] = start[i].Interpolate(end[i], t);
}

9
wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveKinematicsProto.hpp
View File

@@ -10,10 +10,13 @@
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveKinematics> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveKinematics> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveKinematics;
using InputStream = wpi::util::ProtoInputStream<wpi::math::DifferentialDriveKinematics>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveKinematics>;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveKinematics>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveKinematics>;
static std::optional<wpi::math::DifferentialDriveKinematics> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,

3
wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelPositionsProto.hpp
View File

@@ -10,7 +10,8 @@
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveWheelPositions> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelPositions> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelPositions;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelPositions>;

6
wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp
View File

@@ -10,9 +10,11 @@
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelSpeeds;
using InputStream = wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelSpeeds>;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelSpeeds>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveWheelSpeeds>;
static std::optional<wpi::math::DifferentialDriveWheelSpeeds> Unpack(

9
wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveKinematicsProto.hpp
View File

@@ -12,9 +12,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::MecanumDriveKinematics> {
using MessageStruct = wpi_proto_ProtobufMecanumDriveKinematics;
using InputStream = wpi::util::ProtoInputStream<wpi::math::MecanumDriveKinematics>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::MecanumDriveKinematics>;
static std::optional<wpi::math::MecanumDriveKinematics> Unpack(InputStream& stream);
using InputStream =
wpi::util::ProtoInputStream<wpi::math::MecanumDriveKinematics>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveKinematics>;
static std::optional<wpi::math::MecanumDriveKinematics> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::MecanumDriveKinematics& value);
};

9
wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelPositionsProto.hpp
View File

@@ -10,10 +10,13 @@
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::MecanumDriveWheelPositions> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::MecanumDriveWheelPositions> {
using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelPositions;
using InputStream = wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelPositions>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelPositions>;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelPositions>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelPositions>;
static std::optional<wpi::math::MecanumDriveWheelPositions> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,

9
wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp
View File

@@ -10,10 +10,13 @@
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds> {
using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelSpeeds;
using InputStream = wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelSpeeds>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelSpeeds>;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelSpeeds>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelSpeeds>;
static std::optional<wpi::math::MecanumDriveWheelSpeeds> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,

10
wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveDriveKinematicsProto.hpp
View File

@@ -18,12 +18,13 @@ struct wpi::util::Protobuf<wpi::math::SwerveDriveKinematics<NumModules>> {
using MessageStruct = wpi_proto_ProtobufSwerveDriveKinematics;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::SwerveDriveKinematics<NumModules>>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::SwerveDriveKinematics<NumModules>>;
using OutputStream = wpi::util::ProtoOutputStream<
wpi::math::SwerveDriveKinematics<NumModules>>;
static std::optional<wpi::math::SwerveDriveKinematics<NumModules>> Unpack(
InputStream& stream) {
wpi::util::WpiArrayUnpackCallback<wpi::math::Translation2d, NumModules> modules;
wpi::util::WpiArrayUnpackCallback<wpi::math::Translation2d, NumModules>
modules;
wpi_proto_ProtobufSwerveDriveKinematics msg{
.modules = modules.Callback(),
};
@@ -37,7 +38,8 @@ struct wpi::util::Protobuf<wpi::math::SwerveDriveKinematics<NumModules>> {
static bool Pack(OutputStream& stream,
const wpi::math::SwerveDriveKinematics<NumModules>& value) {
wpi::util::PackCallback<wpi::math::Translation2d> modules{value.GetModules()};
wpi::util::PackCallback<wpi::math::Translation2d> modules{
value.GetModules()};
wpi_proto_ProtobufSwerveDriveKinematics msg{
.modules = modules.Callback(),
};

9
wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModulePositionProto.hpp
View File

@@ -12,9 +12,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::SwerveModulePosition> {
using MessageStruct = wpi_proto_ProtobufSwerveModulePosition;
using InputStream = wpi::util::ProtoInputStream<wpi::math::SwerveModulePosition>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::SwerveModulePosition>;
static std::optional<wpi::math::SwerveModulePosition> Unpack(InputStream& stream);
using InputStream =
wpi::util::ProtoInputStream<wpi::math::SwerveModulePosition>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::SwerveModulePosition>;
static std::optional<wpi::math::SwerveModulePosition> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::SwerveModulePosition& value);
};

9
wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleStateProto.hpp
View File

@@ -13,7 +13,10 @@ template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::SwerveModuleState> {
using MessageStruct = wpi_proto_ProtobufSwerveModuleState;
using InputStream = wpi::util::ProtoInputStream<wpi::math::SwerveModuleState>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::SwerveModuleState>;
static std::optional<wpi::math::SwerveModuleState> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::SwerveModuleState& value);
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::SwerveModuleState>;
static std::optional<wpi::math::SwerveModuleState> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::SwerveModuleState& value);
};

3
wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp
View File

@@ -17,7 +17,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ChassisSpeeds> {
}
static wpi::math::ChassisSpeeds Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::ChassisSpeeds& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::ChassisSpeeds& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::ChassisSpeeds>);

9
wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveKinematicsStruct.hpp
View File

@@ -9,16 +9,19 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveKinematics> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveKinematics> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveKinematics";
}
static constexpr size_t GetSize() { return 8; }
static constexpr std::string_view GetSchema() { return "double trackwidth"; }
static wpi::math::DifferentialDriveKinematics Unpack(std::span<const uint8_t> data);
static wpi::math::DifferentialDriveKinematics Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::DifferentialDriveKinematics& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveKinematics>);
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveKinematics>);

6
wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.hpp
View File

@@ -9,7 +9,8 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveWheelPositions";
}
@@ -24,4 +25,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelPosit
const wpi::math::DifferentialDriveWheelPositions& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelPositions>);
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelPositions>);

6
wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp
View File

@@ -9,7 +9,8 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveWheelSpeeds";
}
@@ -24,4 +25,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeed
const wpi::math::DifferentialDriveWheelSpeeds& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelSpeeds>);
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelSpeeds>);

3
wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveKinematicsStruct.hpp
View File

@@ -21,7 +21,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveKinematics> {
"rear_left;Translation2d rear_right";
}
static wpi::math::MecanumDriveKinematics Unpack(std::span<const uint8_t> data);
static wpi::math::MecanumDriveKinematics Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::MecanumDriveKinematics& value);
static void ForEachNested(

9
wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelPositionsStruct.hpp
View File

@@ -9,7 +9,8 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveWheelPositions> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::MecanumDriveWheelPositions> {
static constexpr std::string_view GetTypeName() {
return "MecanumDriveWheelPositions";
}
@@ -19,9 +20,11 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveWheelPositions>
"rear_right";
}
static wpi::math::MecanumDriveWheelPositions Unpack(std::span<const uint8_t> data);
static wpi::math::MecanumDriveWheelPositions Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::MecanumDriveWheelPositions& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::MecanumDriveWheelPositions>);
static_assert(
wpi::util::StructSerializable<wpi::math::MecanumDriveWheelPositions>);

6
wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.hpp
View File

@@ -19,9 +19,11 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds> {
"double rear_right";
}
static wpi::math::MecanumDriveWheelSpeeds Unpack(std::span<const uint8_t> data);
static wpi::math::MecanumDriveWheelSpeeds Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::MecanumDriveWheelSpeeds& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::MecanumDriveWheelSpeeds>);
static_assert(
wpi::util::StructSerializable<wpi::math::MecanumDriveWheelSpeeds>);

20
wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveDriveKinematicsStruct.hpp
View File

@@ -12,29 +12,31 @@
template <size_t NumModules>
struct wpi::util::Struct<wpi::math::SwerveDriveKinematics<NumModules>> {
static constexpr ct_string kTypeName = wpi::util::Concat(
"SwerveDriveKinematics__"_ct_string, wpi::util::NumToCtString<NumModules>());
static constexpr ct_string kTypeName =
wpi::util::Concat("SwerveDriveKinematics__"_ct_string,
wpi::util::NumToCtString<NumModules>());
static constexpr std::string_view GetTypeName() { return kTypeName; }
static constexpr size_t GetSize() {
return NumModules * wpi::util::Struct<wpi::math::Translation2d>::GetSize();
}
static constexpr ct_string kSchema =
wpi::util::Concat("Translation2d modules["_ct_string,
wpi::util::NumToCtString<NumModules>(), "]"_ct_string);
wpi::util::NumToCtString<NumModules>(), "]"_ct_string);
static constexpr std::string_view GetSchema() { return kSchema; }
static wpi::math::SwerveDriveKinematics<NumModules> Unpack(
std::span<const uint8_t> data) {
constexpr size_t kModulesOff = 0;
return wpi::math::SwerveDriveKinematics<NumModules>{
wpi::util::UnpackStructArray<wpi::math::Translation2d, kModulesOff, NumModules>(
data)};
wpi::util::UnpackStructArray<wpi::math::Translation2d, kModulesOff,
NumModules>(data)};
}
static void Pack(std::span<uint8_t> data,
const wpi::math::SwerveDriveKinematics<NumModules>& value) {
constexpr size_t kModulesOff = 0;
wpi::util::PackStructArray<kModulesOff, NumModules>(data, value.GetModules());
wpi::util::PackStructArray<kModulesOff, NumModules>(data,
value.GetModules());
}
static void ForEachNested(
@@ -43,7 +45,9 @@ struct wpi::util::Struct<wpi::math::SwerveDriveKinematics<NumModules>> {
}
};
static_assert(wpi::util::StructSerializable<wpi::math::SwerveDriveKinematics<4>>);
static_assert(
wpi::util::StructSerializable<wpi::math::SwerveDriveKinematics<4>>);
static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveDriveKinematics<4>>);
static_assert(wpi::util::StructSerializable<wpi::math::SwerveDriveKinematics<3>>);
static_assert(
wpi::util::StructSerializable<wpi::math::SwerveDriveKinematics<3>>);
static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveDriveKinematics<3>>);

12
wpimath/src/main/native/include/wpi/math/linalg/proto/MatrixProto.hpp
View File

@@ -15,19 +15,21 @@
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
struct wpi::util::Protobuf<wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
struct wpi::util::Protobuf<
wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
using MessageStruct = wpi_proto_ProtobufMatrix;
using InputStream = wpi::util::ProtoInputStream<
wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>;
using OutputStream = wpi::util::ProtoOutputStream<
wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>;
static std::optional<wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>
static std::optional<
wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>
Unpack(InputStream& stream) {
constexpr bool isSmall = Rows * Cols * sizeof(double) < 256;
using UnpackType =
std::conditional_t<isSmall, wpi::util::UnpackCallback<double, Rows * Cols>,
wpi::util::StdVectorUnpackCallback<double, Rows * Cols>>;
using UnpackType = std::conditional_t<
isSmall, wpi::util::UnpackCallback<double, Rows * Cols>,
wpi::util::StdVectorUnpackCallback<double, Rows * Cols>>;
UnpackType data;
data.Vec().reserve(Rows * Cols);
data.SetLimits(wpi::util::DecodeLimits::Fail);

15
wpimath/src/main/native/include/wpi/math/linalg/proto/VectorProto.hpp
View File

@@ -14,15 +14,16 @@
#include "wpimath/protobuf/wpimath.npb.h"
template <int Size, int Options, int MaxRows, int MaxCols>
struct wpi::util::Protobuf<wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
struct wpi::util::Protobuf<
wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
using MessageStruct = wpi_proto_ProtobufVector;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>>;
using InputStream = wpi::util::ProtoInputStream<
wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>>;
using OutputStream = wpi::util::ProtoOutputStream<
wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>>;
static std::optional<wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>> Unpack(
InputStream& stream) {
static std::optional<wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>>
Unpack(InputStream& stream) {
constexpr bool isSmall = Size * sizeof(double) < 256;
using UnpackType =
std::conditional_t<isSmall, wpi::util::UnpackCallback<double, Size>,

9
wpimath/src/main/native/include/wpi/math/linalg/struct/StructProto.hpp
View File

@@ -12,15 +12,16 @@
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
struct wpi::util::Struct<wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
struct wpi::util::Struct<
wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
static constexpr ct_string kTypeName =
wpi::util::Concat("Matrix__"_ct_string, wpi::util::NumToCtString<Rows>(),
"_"_ct_string, wpi::util::NumToCtString<Cols>());
"_"_ct_string, wpi::util::NumToCtString<Cols>());
static constexpr std::string_view GetTypeName() { return kTypeName; }
static constexpr size_t GetSize() { return Rows * Cols * 8; }
static constexpr ct_string kSchema =
wpi::util::Concat("double data["_ct_string, wpi::util::NumToCtString<Rows * Cols>(),
"]"_ct_string);
wpi::util::Concat("double data["_ct_string,
wpi::util::NumToCtString<Rows * Cols>(), "]"_ct_string);
static constexpr std::string_view GetSchema() { return kSchema; }
static wpi::math::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(

8
wpimath/src/main/native/include/wpi/math/linalg/struct/VectorStruct.hpp
View File

@@ -11,13 +11,15 @@
#include "wpi/util/struct/Struct.hpp"
template <int Size, int Options, int MaxRows, int MaxCols>
struct wpi::util::Struct<wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
struct wpi::util::Struct<
wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
static constexpr ct_string kTypeName =
wpi::util::Concat("Vector__"_ct_string, wpi::util::NumToCtString<Size>());
static constexpr std::string_view GetTypeName() { return kTypeName; }
static constexpr size_t GetSize() { return Size * 8; }
static constexpr ct_string kSchema = wpi::util::Concat(
"double data["_ct_string, wpi::util::NumToCtString<Size>(), "]"_ct_string);
static constexpr ct_string kSchema =
wpi::util::Concat("double data["_ct_string,
wpi::util::NumToCtString<Size>(), "]"_ct_string);
static constexpr std::string_view GetSchema() { return kSchema; }
static wpi::math::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(

4
wpimath/src/main/native/include/wpi/math/path/TravelingSalesman.hpp
View File

@@ -55,8 +55,8 @@ class TravelingSalesman {
* @return The optimized path as an array of Pose2ds.
*/
template <size_t Poses>
wpi::util::array<Pose2d, Poses> Solve(const wpi::util::array<Pose2d, Poses>& poses,
int iterations) {
wpi::util::array<Pose2d, Poses> Solve(
const wpi::util::array<Pose2d, Poses>& poses, int iterations) {
SimulatedAnnealing<Vectord<Poses>> solver{
1, &Neighbor<Poses>, [&](const Vectord<Poses>& state) {
// Total cost is sum of all costs between adjacent pairs in path

3
wpimath/src/main/native/include/wpi/math/spline/CubicHermiteSpline.hpp
View File

@@ -137,7 +137,8 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* @return The control vector matrix for a dimension.
*/
static constexpr Eigen::Vector4d ControlVectorFromArrays(
wpi::util::array<double, 2> initialVector, wpi::util::array<double, 2> finalVector) {
wpi::util::array<double, 2> initialVector,
wpi::util::array<double, 2> finalVector) {
return Eigen::Vector4d{{initialVector[0]},
{initialVector[1]},
{finalVector[0]},

3
wpimath/src/main/native/include/wpi/math/spline/QuinticHermiteSpline.hpp
View File

@@ -145,7 +145,8 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
* @return The control vector matrix for a dimension.
*/
static constexpr Vectord<6> ControlVectorFromArrays(
wpi::util::array<double, 3> initialVector, wpi::util::array<double, 3> finalVector) {
wpi::util::array<double, 3> initialVector,
wpi::util::array<double, 3> finalVector) {
return Vectord<6>{{initialVector[0]}, {initialVector[1]},
{initialVector[2]}, {finalVector[0]},
{finalVector[1]}, {finalVector[2]}};

3
wpimath/src/main/native/include/wpi/math/spline/Spline.hpp
View File

@@ -143,7 +143,8 @@ class Spline {
* @return The Translation2d.
*/
static constexpr Translation2d FromVector(const Eigen::Vector2d& vector) {
return Translation2d{wpi::units::meter_t{vector(0)}, wpi::units::meter_t{vector(1)}};
return Translation2d{wpi::units::meter_t{vector(0)},
wpi::units::meter_t{vector(1)}};
}
};

10
wpimath/src/main/native/include/wpi/math/spline/SplineHelper.hpp
View File

@@ -106,8 +106,8 @@ class WPILIB_DLLEXPORT SplineHelper {
waypoints.emplace(waypoints.begin(),
Translation2d{wpi::units::meter_t{xInitial[0]},
wpi::units::meter_t{yInitial[0]}});
waypoints.emplace_back(
Translation2d{wpi::units::meter_t{xFinal[0]}, wpi::units::meter_t{yFinal[0]}});
waypoints.emplace_back(Translation2d{wpi::units::meter_t{xFinal[0]},
wpi::units::meter_t{yFinal[0]}});
// Populate tridiagonal system for clamped cubic
/* See:
@@ -186,8 +186,10 @@ class WPILIB_DLLEXPORT SplineHelper {
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
wpi::util::array<double, 2> midXControlVector{waypoints[0].X().value(), xDeriv};
wpi::util::array<double, 2> midYControlVector{waypoints[0].Y().value(), yDeriv};
wpi::util::array<double, 2> midXControlVector{waypoints[0].X().value(),
xDeriv};
wpi::util::array<double, 2> midYControlVector{waypoints[0].Y().value(),
yDeriv};
splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);

12
wpimath/src/main/native/include/wpi/math/spline/proto/CubicHermiteSplineProto.hpp
View File

@@ -12,8 +12,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::CubicHermiteSpline> {
using MessageStruct = wpi_proto_ProtobufCubicHermiteSpline;
using InputStream = wpi::util::ProtoInputStream<wpi::math::CubicHermiteSpline>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::CubicHermiteSpline>;
static std::optional<wpi::math::CubicHermiteSpline> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::CubicHermiteSpline& value);
using InputStream =
wpi::util::ProtoInputStream<wpi::math::CubicHermiteSpline>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::CubicHermiteSpline>;
static std::optional<wpi::math::CubicHermiteSpline> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::CubicHermiteSpline& value);
};

9
wpimath/src/main/native/include/wpi/math/spline/proto/QuinticHermiteSplineProto.hpp
View File

@@ -12,9 +12,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::QuinticHermiteSpline> {
using MessageStruct = wpi_proto_ProtobufQuinticHermiteSpline;
using InputStream = wpi::util::ProtoInputStream<wpi::math::QuinticHermiteSpline>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::QuinticHermiteSpline>;
static std::optional<wpi::math::QuinticHermiteSpline> Unpack(InputStream& stream);
using InputStream =
wpi::util::ProtoInputStream<wpi::math::QuinticHermiteSpline>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::QuinticHermiteSpline>;
static std::optional<wpi::math::QuinticHermiteSpline> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::QuinticHermiteSpline& value);
};

15
wpimath/src/main/native/include/wpi/math/system/LinearSystemLoop.hpp
View File

@@ -61,7 +61,8 @@ class LinearSystemLoop {
: LinearSystemLoop(
plant, controller, observer,
[=](const InputVector& u) {
return wpi::math::DesaturateInputVector<Inputs>(u, maxVoltage.value());
return wpi::math::DesaturateInputVector<Inputs>(
u, maxVoltage.value());
},
dt) {}
@@ -101,11 +102,13 @@ class LinearSystemLoop {
LinearSystemLoop(
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer, wpi::units::volt_t maxVoltage)
: LinearSystemLoop(
controller, feedforward, observer, [=](const InputVector& u) {
return wpi::math::DesaturateInputVector<Inputs>(u, maxVoltage.value());
}) {}
KalmanFilter<States, Inputs, Outputs>& observer,
wpi::units::volt_t maxVoltage)
: LinearSystemLoop(controller, feedforward, observer,
[=](const InputVector& u) {
return wpi::math::DesaturateInputVector<Inputs>(
u, maxVoltage.value());
}) {}
/**
* Constructs a state-space loop with the given controller, feedforward,

35
wpimath/src/main/native/include/wpi/math/system/plant/DCMotor.hpp
View File

@@ -18,12 +18,12 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DCMotor {
public:
using radians_per_second_per_volt_t =
wpi::units::unit_t<wpi::units::compound_unit<wpi::units::radians_per_second,
wpi::units::inverse<wpi::units::volt>>>;
using radians_per_second_per_volt_t = wpi::units::unit_t<
wpi::units::compound_unit<wpi::units::radians_per_second,
wpi::units::inverse<wpi::units::volt>>>;
using newton_meters_per_ampere_t =
wpi::units::unit_t<wpi::units::compound_unit<wpi::units::newton_meters,
wpi::units::inverse<wpi::units::ampere>>>;
wpi::units::unit_t<wpi::units::compound_unit<
wpi::units::newton_meters, wpi::units::inverse<wpi::units::ampere>>>;
/// Voltage at which the motor constants were measured.
wpi::units::volt_t nominalVoltage;
@@ -61,8 +61,10 @@ class WPILIB_DLLEXPORT DCMotor {
*/
constexpr DCMotor(wpi::units::volt_t nominalVoltage,
wpi::units::newton_meter_t stallTorque,
wpi::units::ampere_t stallCurrent, wpi::units::ampere_t freeCurrent,
wpi::units::radians_per_second_t freeSpeed, int numMotors = 1)
wpi::units::ampere_t stallCurrent,
wpi::units::ampere_t freeCurrent,
wpi::units::radians_per_second_t freeSpeed,
int numMotors = 1)
: nominalVoltage(nominalVoltage),
stallTorque(stallTorque * numMotors),
stallCurrent(stallCurrent * numMotors),
@@ -78,8 +80,9 @@ class WPILIB_DLLEXPORT DCMotor {
* @param speed The current angular velocity of the motor.
* @param inputVoltage The voltage being applied to the motor.
*/
constexpr wpi::units::ampere_t Current(wpi::units::radians_per_second_t speed,
wpi::units::volt_t inputVoltage) const {
constexpr wpi::units::ampere_t Current(
wpi::units::radians_per_second_t speed,
wpi::units::volt_t inputVoltage) const {
return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
}
@@ -88,7 +91,8 @@ class WPILIB_DLLEXPORT DCMotor {
*
* @param torque The torque produced by the motor.
*/
constexpr wpi::units::ampere_t Current(wpi::units::newton_meter_t torque) const {
constexpr wpi::units::ampere_t Current(
wpi::units::newton_meter_t torque) const {
return torque / Kt;
}
@@ -97,7 +101,8 @@ class WPILIB_DLLEXPORT DCMotor {
*
* @param current The current drawn by the motor.
*/
constexpr wpi::units::newton_meter_t Torque(wpi::units::ampere_t current) const {
constexpr wpi::units::newton_meter_t Torque(
wpi::units::ampere_t current) const {
return current * Kt;
}
@@ -108,8 +113,9 @@ class WPILIB_DLLEXPORT DCMotor {
* @param torque The torque produced by the motor.
* @param speed The current angular velocity of the motor.
*/
constexpr wpi::units::volt_t Voltage(wpi::units::newton_meter_t torque,
wpi::units::radians_per_second_t speed) const {
constexpr wpi::units::volt_t Voltage(
wpi::units::newton_meter_t torque,
wpi::units::radians_per_second_t speed) const {
return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
}
@@ -121,7 +127,8 @@ class WPILIB_DLLEXPORT DCMotor {
* @param inputVoltage The input voltage provided to the motor.
*/
constexpr wpi::units::radians_per_second_t Speed(
wpi::units::newton_meter_t torque, wpi::units::volt_t inputVoltage) const {
wpi::units::newton_meter_t torque,
wpi::units::volt_t inputVoltage) const {
return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv;
}

38
wpimath/src/main/native/include/wpi/math/system/plant/LinearSystemId.hpp
View File

@@ -27,12 +27,13 @@ namespace wpi::math {
class WPILIB_DLLEXPORT LinearSystemId {
public:
template <typename Distance>
using Velocity_t = wpi::units::unit_t<
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>>;
using Velocity_t = wpi::units::unit_t<wpi::units::compound_unit<
Distance, wpi::units::inverse<wpi::units::seconds>>>;
template <typename Distance>
using Acceleration_t = wpi::units::unit_t<wpi::units::compound_unit<
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>,
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>,
wpi::units::inverse<wpi::units::seconds>>>;
/**
@@ -46,10 +47,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to carriage.
* @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
*/
static constexpr LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor,
wpi::units::kilogram_t mass,
wpi::units::meter_t radius,
double gearing) {
static constexpr LinearSystem<2, 1, 2> ElevatorSystem(
DCMotor motor, wpi::units::kilogram_t mass, wpi::units::meter_t radius,
double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
@@ -274,7 +274,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_rad_per_s) kVAngular,
decltype(1_V / 1_rad_per_s_sq) kAAngular, wpi::units::meter_t trackwidth) {
decltype(1_V / 1_rad_per_s_sq) kAAngular,
wpi::units::meter_t trackwidth) {
if (kVLinear <= decltype(kVLinear){0}) {
throw std::domain_error("Kv,linear must be greater than zero.");
}
@@ -427,7 +428,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
*/
static constexpr LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
const DCMotor& motor, wpi::units::kilogram_t mass, wpi::units::meter_t r,
wpi::units::meter_t rb, wpi::units::kilogram_square_meter_t J, double gearing) {
wpi::units::meter_t rb, wpi::units::kilogram_square_meter_t J,
double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
@@ -448,14 +450,16 @@ class WPILIB_DLLEXPORT LinearSystemId {
(motor.Kv * motor.R * wpi::units::math::pow<2>(r));
auto C2 = gearing * motor.Kt / (motor.R * r);
Matrixd<2, 2> A{{((1 / mass + wpi::units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass - wpi::units::math::pow<2>(rb) / J) * C1).value()},
{((1 / mass - wpi::units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass + wpi::units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{{((1 / mass + wpi::units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass - wpi::units::math::pow<2>(rb) / J) * C2).value()},
{((1 / mass - wpi::units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass + wpi::units::math::pow<2>(rb) / J) * C2).value()}};
Matrixd<2, 2> A{
{((1 / mass + wpi::units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass - wpi::units::math::pow<2>(rb) / J) * C1).value()},
{((1 / mass - wpi::units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass + wpi::units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{
{((1 / mass + wpi::units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass - wpi::units::math::pow<2>(rb) / J) * C2).value()},
{((1 / mass - wpi::units::math::pow<2>(rb) / J) * C2).value(),
((1 / mass + wpi::units::math::pow<2>(rb) / J) * C2).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};

13
wpimath/src/main/native/include/wpi/math/system/proto/LinearSystemProto.hpp
View File

@@ -18,10 +18,10 @@
template <int States, int Inputs, int Outputs>
struct wpi::util::Protobuf<wpi::math::LinearSystem<States, Inputs, Outputs>> {
using MessageStruct = wpi_proto_ProtobufLinearSystem;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::LinearSystem<States, Inputs, Outputs>>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::LinearSystem<States, Inputs, Outputs>>;
using InputStream = wpi::util::ProtoInputStream<
wpi::math::LinearSystem<States, Inputs, Outputs>>;
using OutputStream = wpi::util::ProtoOutputStream<
wpi::math::LinearSystem<States, Inputs, Outputs>>;
static std::optional<wpi::math::LinearSystem<States, Inputs, Outputs>> Unpack(
InputStream& stream) {
@@ -66,8 +66,9 @@ struct wpi::util::Protobuf<wpi::math::LinearSystem<States, Inputs, Outputs>> {
};
}
static bool Pack(OutputStream& stream,
const wpi::math::LinearSystem<States, Inputs, Outputs>& value) {
static bool Pack(
OutputStream& stream,
const wpi::math::LinearSystem<States, Inputs, Outputs>& value) {
wpi::util::PackCallback a{&value.A()};
wpi::util::PackCallback b{&value.B()};
wpi::util::PackCallback c{&value.C()};

37
wpimath/src/main/native/include/wpi/math/system/struct/LinearSystemStruct.hpp
View File

@@ -13,10 +13,10 @@
template <int States, int Inputs, int Outputs>
struct wpi::util::Struct<wpi::math::LinearSystem<States, Inputs, Outputs>> {
static constexpr ct_string kTypeName =
wpi::util::Concat("LinearSystem__"_ct_string, wpi::util::NumToCtString<States>(),
"_"_ct_string, wpi::util::NumToCtString<Inputs>(), "_"_ct_string,
wpi::util::NumToCtString<Outputs>());
static constexpr ct_string kTypeName = wpi::util::Concat(
"LinearSystem__"_ct_string, wpi::util::NumToCtString<States>(),
"_"_ct_string, wpi::util::NumToCtString<Inputs>(), "_"_ct_string,
wpi::util::NumToCtString<Outputs>());
static constexpr std::string_view GetTypeName() { return kTypeName; }
static constexpr size_t GetSize() {
return wpi::util::Struct<wpi::math::Matrixd<States, States>>::GetSize() +
@@ -25,10 +25,14 @@ struct wpi::util::Struct<wpi::math::LinearSystem<States, Inputs, Outputs>> {
wpi::util::Struct<wpi::math::Matrixd<Outputs, Inputs>>::GetSize();
}
static constexpr ct_string kSchema = wpi::util::Concat(
wpi::util::Struct<wpi::math::Matrixd<States, States>>::kTypeName, " a;"_ct_string,
wpi::util::Struct<wpi::math::Matrixd<States, Inputs>>::kTypeName, " b;"_ct_string,
wpi::util::Struct<wpi::math::Matrixd<Outputs, States>>::kTypeName, " c;"_ct_string,
wpi::util::Struct<wpi::math::Matrixd<Outputs, Inputs>>::kTypeName, " d"_ct_string);
wpi::util::Struct<wpi::math::Matrixd<States, States>>::kTypeName,
" a;"_ct_string,
wpi::util::Struct<wpi::math::Matrixd<States, Inputs>>::kTypeName,
" b;"_ct_string,
wpi::util::Struct<wpi::math::Matrixd<Outputs, States>>::kTypeName,
" c;"_ct_string,
wpi::util::Struct<wpi::math::Matrixd<Outputs, Inputs>>::kTypeName,
" d"_ct_string);
static constexpr std::string_view GetSchema() { return kSchema; }
static wpi::math::LinearSystem<States, Inputs, Outputs> Unpack(
@@ -41,14 +45,19 @@ struct wpi::util::Struct<wpi::math::LinearSystem<States, Inputs, Outputs>> {
constexpr size_t kDOff =
kCOff + wpi::util::GetStructSize<wpi::math::Matrixd<Outputs, States>>();
return wpi::math::LinearSystem<States, Inputs, Outputs>{
wpi::util::UnpackStruct<wpi::math::Matrixd<States, States>, kAOff>(data),
wpi::util::UnpackStruct<wpi::math::Matrixd<States, Inputs>, kBOff>(data),
wpi::util::UnpackStruct<wpi::math::Matrixd<Outputs, States>, kCOff>(data),
wpi::util::UnpackStruct<wpi::math::Matrixd<Outputs, Inputs>, kDOff>(data)};
wpi::util::UnpackStruct<wpi::math::Matrixd<States, States>, kAOff>(
data),
wpi::util::UnpackStruct<wpi::math::Matrixd<States, Inputs>, kBOff>(
data),
wpi::util::UnpackStruct<wpi::math::Matrixd<Outputs, States>, kCOff>(
data),
wpi::util::UnpackStruct<wpi::math::Matrixd<Outputs, Inputs>, kDOff>(
data)};
}
static void Pack(std::span<uint8_t> data,
const wpi::math::LinearSystem<States, Inputs, Outputs>& value) {
static void Pack(
std::span<uint8_t> data,
const wpi::math::LinearSystem<States, Inputs, Outputs>& value) {
constexpr size_t kAOff = 0;
constexpr size_t kBOff =
kAOff + wpi::util::GetStructSize<wpi::math::Matrixd<States, States>>();

30
wpimath/src/main/native/include/wpi/math/trajectory/ExponentialProfile.hpp
View File

@@ -42,17 +42,20 @@ class ExponentialProfile {
public:
using Distance_t = wpi::units::unit_t<Distance>;
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Velocity_t = wpi::units::unit_t<Velocity>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using Input_t = wpi::units::unit_t<Input>;
using A_t = wpi::units::unit_t<wpi::units::inverse<wpi::units::seconds>>;
using B_t =
wpi::units::unit_t<wpi::units::compound_unit<Acceleration, wpi::units::inverse<Input>>>;
using B_t = wpi::units::unit_t<
wpi::units::compound_unit<Acceleration, wpi::units::inverse<Input>>>;
using KV = wpi::units::compound_unit<Input, wpi::units::inverse<Velocity>>;
using kV_t = wpi::units::unit_t<KV>;
using KA = wpi::units::compound_unit<Input, wpi::units::inverse<Acceleration>>;
using KA =
wpi::units::compound_unit<Input, wpi::units::inverse<Acceleration>>;
using kA_t = wpi::units::unit_t<KA>;
/**
@@ -199,7 +202,7 @@ class ExponentialProfile {
* @return The total duration of this profile.
*/
constexpr wpi::units::second_t TimeLeftUntil(const State& current,
const State& goal) const {
const State& goal) const {
auto timing = CalculateProfileTiming(current, goal);
return timing.totalTime;
@@ -282,7 +285,7 @@ class ExponentialProfile {
// point velocity. For case 5, we have reached inflection point velocity.
auto epsilon = Velocity_t(1e-9);
if (wpi::units::math::abs(u_dir * m_constraints.MaxVelocity() -
inflectionPoint.velocity) < epsilon) {
inflectionPoint.velocity) < epsilon) {
auto solvableV = inflectionPoint.velocity;
wpi::units::second_t t_to_solvable_v;
Distance_t x_at_solvable_v;
@@ -291,7 +294,8 @@ class ExponentialProfile {
t_to_solvable_v = 0_s;
x_at_solvable_v = current.position;
} else {
if (wpi::units::math::abs(current.velocity) > m_constraints.MaxVelocity()) {
if (wpi::units::math::abs(current.velocity) >
m_constraints.MaxVelocity()) {
solvableV += u_dir * epsilon;
} else {
solvableV -= u_dir * epsilon;
@@ -335,8 +339,8 @@ class ExponentialProfile {
auto u = input;
return initial.position +
(-B * u * time +
(initial.velocity + B * u / A) * (wpi::units::math::exp(A * time) - 1)) /
(-B * u * time + (initial.velocity + B * u / A) *
(wpi::units::math::exp(A * time) - 1)) /
A;
}
@@ -378,7 +382,9 @@ class ExponentialProfile {
auto B = m_constraints.B;
auto u = input;
return wpi::units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A;
return wpi::units::math::log((A * velocity + B * u) /
(A * initial + B * u)) /
A;
}
/**
@@ -401,7 +407,7 @@ class ExponentialProfile {
return initial.position + (velocity - initial.velocity) / A -
B * u / (A * A) *
wpi::units::math::log((A * velocity + B * u) /
(A * initial.velocity + B * u));
(A * initial.velocity + B * u));
}
/**

11
wpimath/src/main/native/include/wpi/math/trajectory/Trajectory.hpp
View File

@@ -74,8 +74,8 @@ class WPILIB_DLLEXPORT Trajectory {
// Check whether the robot is reversing at this stage.
const auto reversing =
velocity < 0_mps ||
(wpi::units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
velocity < 0_mps || (wpi::units::math::abs(velocity) < 1E-9_mps &&
acceleration < 0_mps_sq);
// Calculate the new velocity.
// v = v_0 + at
@@ -95,9 +95,10 @@ class WPILIB_DLLEXPORT Trajectory {
const double interpolationFrac =
newS / endValue.pose.Translation().Distance(pose.Translation());
return {newT, newV, acceleration,
wpi::util::Lerp(pose, endValue.pose, interpolationFrac),
wpi::util::Lerp(curvature, endValue.curvature, interpolationFrac)};
return {
newT, newV, acceleration,
wpi::util::Lerp(pose, endValue.pose, interpolationFrac),
wpi::util::Lerp(curvature, endValue.curvature, interpolationFrac)};
}
};

4
wpimath/src/main/native/include/wpi/math/trajectory/TrajectoryConfig.hpp
View File

@@ -115,7 +115,9 @@ class WPILIB_DLLEXPORT TrajectoryConfig {
* Returns the starting velocity of the trajectory.
* @return The starting velocity of the trajectory.
*/
wpi::units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
wpi::units::meters_per_second_t StartVelocity() const {
return m_startVelocity;
}
/**
* Returns the ending velocity of the trajectory.

28
wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp
View File

@@ -47,10 +47,12 @@ class TrapezoidProfile {
public:
using Distance_t = wpi::units::unit_t<Distance>;
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Velocity_t = wpi::units::unit_t<Velocity>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using Acceleration_t = wpi::units::unit_t<Acceleration>;
/**
@@ -134,8 +136,8 @@ class TrapezoidProfile {
m_current = Direct(current);
goal = Direct(goal);
if (wpi::units::math::abs(m_current.velocity) > m_constraints.maxVelocity) {
m_current.velocity =
wpi::units::math::copysign(m_constraints.maxVelocity, m_current.velocity);
m_current.velocity = wpi::units::math::copysign(m_constraints.maxVelocity,
m_current.velocity);
}
// Deal with a possibly truncated motion profile (with nonzero initial or
@@ -146,7 +148,8 @@ class TrapezoidProfile {
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
wpi::units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
wpi::units::second_t cutoffEnd =
goal.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistEnd =
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
@@ -164,8 +167,8 @@ class TrapezoidProfile {
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < Distance_t{0}) {
accelerationTime =
wpi::units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
accelerationTime = wpi::units::math::sqrt(fullTrapezoidDist /
m_constraints.maxAcceleration);
fullSpeedDist = Distance_t{0};
}
@@ -236,8 +239,8 @@ class TrapezoidProfile {
Velocity_t decelVelocity;
if (endAccel > 0_s) {
decelVelocity = wpi::units::math::sqrt(
wpi::units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
decelVelocity = wpi::units::math::sqrt(wpi::units::math::abs(
velocity * velocity + 2 * acceleration * accelDist));
} else {
decelVelocity = velocity;
}
@@ -263,11 +266,12 @@ class TrapezoidProfile {
wpi::units::second_t decelTime =
(-decelVelocity +
wpi::units::math::sqrt(wpi::units::math::abs(decelVelocity * decelVelocity +
2 * deceleration * decelDist))) /
wpi::units::math::sqrt(wpi::units::math::abs(
decelVelocity * decelVelocity + 2 * deceleration * decelDist))) /
deceleration;
wpi::units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
wpi::units::second_t fullSpeedTime =
fullSpeedDist / m_constraints.maxVelocity;
return accelTime + fullSpeedTime + decelTime;
}

2
wpimath/src/main/native/include/wpi/math/trajectory/constraint/CentripetalAccelerationConstraint.hpp
View File

@@ -43,7 +43,7 @@ class WPILIB_DLLEXPORT CentripetalAccelerationConstraint
// The units library defines a unit for radians although it is technically
// unitless.
return wpi::units::math::sqrt(m_maxCentripetalAcceleration /
wpi::units::math::abs(curvature) * 1_rad);
wpi::units::math::abs(curvature) * 1_rad);
}
constexpr MinMax MinMaxAcceleration(

9
wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveVoltageConstraint.hpp
View File

@@ -89,10 +89,12 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
if (speed == 0_mps) {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackwidth * wpi::units::math::abs(curvature) / (2_rad));
(1 + m_kinematics.trackwidth * wpi::units::math::abs(curvature) /
(2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 + m_kinematics.trackwidth * wpi::units::math::abs(curvature) / (2_rad));
(1 + m_kinematics.trackwidth * wpi::units::math::abs(curvature) /
(2_rad));
} else {
maxChassisAcceleration =
maxWheelAcceleration /
@@ -110,7 +112,8 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
// wheel when this happens. We can accurately account for this by simply
// negating the inner wheel.
if ((m_kinematics.trackwidth / 2) > 1_rad / wpi::units::math::abs(curvature)) {
if ((m_kinematics.trackwidth / 2) >
1_rad / wpi::units::math::abs(curvature)) {
if (speed > 0_mps) {
minChassisAcceleration = -minChassisAcceleration;
} else if (speed < 0_mps) {

5
wpimath/src/main/native/include/wpi/math/trajectory/constraint/MecanumDriveKinematicsConstraint.hpp
View File

@@ -40,8 +40,9 @@ class WPILIB_DLLEXPORT MecanumDriveKinematicsConstraint
return wpi::units::math::hypot(normSpeeds.vx, normSpeeds.vy);
}
MinMax MinMaxAcceleration(const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
return {};
}

5
wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp
View File

@@ -40,8 +40,9 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
return wpi::units::math::hypot(normSpeeds.vx, normSpeeds.vy);
}
MinMax MinMaxAcceleration(const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
return {};
}

9
wpimath/src/main/native/include/wpi/math/trajectory/proto/TrajectoryStateProto.hpp
View File

@@ -13,7 +13,10 @@ template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::Trajectory::State> {
using MessageStruct = wpi_proto_ProtobufTrajectoryState;
using InputStream = wpi::util::ProtoInputStream<wpi::math::Trajectory::State>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::Trajectory::State>;
static std::optional<wpi::math::Trajectory::State> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::Trajectory::State& value);
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::Trajectory::State>;
static std::optional<wpi::math::Trajectory::State> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::Trajectory::State& value);
};

10
wpimath/src/main/native/include/wpi/math/util/ComputerVisionUtil.hpp
View File

@@ -25,13 +25,15 @@ namespace wpi::math {
* pose. This can either be a constant for a rigidly mounted camera, or
* variable if the camera is mounted to a turret. If the camera was mounted 3
* inches in front of the "origin" (usually physical center) of the robot,
* this would be wpi::math::Transform3d{3_in, 0_in, 0_in, wpi::math::Rotation3d{}}.
* this would be wpi::math::Transform3d{3_in, 0_in, 0_in,
* wpi::math::Rotation3d{}}.
* @return The robot's field-relative pose.
*/
WPILIB_DLLEXPORT
constexpr wpi::math::Pose3d ObjectToRobotPose(const wpi::math::Pose3d& objectInField,
const wpi::math::Transform3d& cameraToObject,
const wpi::math::Transform3d& robotToCamera) {
constexpr wpi::math::Pose3d ObjectToRobotPose(
const wpi::math::Pose3d& objectInField,
const wpi::math::Transform3d& cameraToObject,
const wpi::math::Transform3d& robotToCamera) {
const auto objectToCamera = cameraToObject.Inverse();
const auto cameraToRobot = robotToCamera.Inverse();
return objectInField + objectToCamera + cameraToRobot;

26
wpimath/src/main/native/include/wpi/math/util/MathUtil.hpp
View File

@@ -151,7 +151,8 @@ constexpr T CopyDirectionPow(T value, double exponent,
value);
} else {
return wpi::units::math::copysign(
gcem::pow((wpi::units::math::abs(value) / maxMagnitude).value(), exponent) *
gcem::pow((wpi::units::math::abs(value) / maxMagnitude).value(),
exponent) *
maxMagnitude,
value);
}
@@ -258,7 +259,8 @@ template <typename T>
requires std::is_arithmetic_v<T> || wpi::units::traits::is_unit_t_v<T>
constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
T errorBound = (max - min) / 2.0;
T error = wpi::math::InputModulus<T>(expected - actual, -errorBound, errorBound);
T error =
wpi::math::InputModulus<T>(expected - actual, -errorBound, errorBound);
if constexpr (std::is_arithmetic_v<T>) {
return std::abs(error) < tolerance;
@@ -274,9 +276,9 @@ constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
*/
WPILIB_DLLEXPORT
constexpr wpi::units::radian_t AngleModulus(wpi::units::radian_t angle) {
return InputModulus<wpi::units::radian_t>(angle,
wpi::units::radian_t{-std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
return InputModulus<wpi::units::radian_t>(
angle, wpi::units::radian_t{-std::numbers::pi},
wpi::units::radian_t{std::numbers::pi});
}
// floorDiv and floorMod algorithms taken from Java
@@ -326,10 +328,9 @@ constexpr std::signed_integral auto FloorMod(std::signed_integral auto x,
* @param maxVelocity Maximum translation velocity.
* @return Returns the next Translation2d limited to maxVelocity
*/
constexpr Translation2d SlewRateLimit(const Translation2d& current,
const Translation2d& next,
wpi::units::second_t dt,
wpi::units::meters_per_second_t maxVelocity) {
constexpr Translation2d SlewRateLimit(
const Translation2d& current, const Translation2d& next,
wpi::units::second_t dt, wpi::units::meters_per_second_t maxVelocity) {
if (maxVelocity < 0_mps) {
wpi::math::MathSharedStore::ReportError(
"maxVelocity must be a non-negative number, got {}!", maxVelocity);
@@ -357,10 +358,9 @@ constexpr Translation2d SlewRateLimit(const Translation2d& current,
* @param maxVelocity Maximum translation velocity.
* @return Returns the next Translation3d limited to maxVelocity
*/
constexpr Translation3d SlewRateLimit(const Translation3d& current,
const Translation3d& next,
wpi::units::second_t dt,
wpi::units::meters_per_second_t maxVelocity) {
constexpr Translation3d SlewRateLimit(
const Translation3d& current, const Translation3d& next,
wpi::units::second_t dt, wpi::units::meters_per_second_t maxVelocity) {
if (maxVelocity < 0_mps) {
wpi::math::MathSharedStore::ReportError(
"maxVelocity must be a non-negative number, got {}!", maxVelocity);

5
wpimath/src/main/native/include/wpi/math/util/StateSpaceUtil.hpp
View File

@@ -83,8 +83,9 @@ constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
}
}
wpi::util::for_each([&](int i, double stdDev) { result(i, i) = stdDev * stdDev; },
stdDevs...);
wpi::util::for_each(
[&](int i, double stdDev) { result(i, i) = stdDev * stdDev; },
stdDevs...);
return result;
}

5
wpimath/src/main/native/include/wpi/units/angle.hpp
View File

@@ -38,8 +38,9 @@ namespace wpi::units {
* @sa See unit_t for more information on unit type containers.
*/
#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
UNIT_ADD_WITH_METRIC_PREFIXES(angle, radian, radians, rad,
unit<std::ratio<1>, wpi::units::category::angle_unit>)
UNIT_ADD_WITH_METRIC_PREFIXES(
angle, radian, radians, rad,
unit<std::ratio<1>, wpi::units::category::angle_unit>)
UNIT_ADD(angle, degree, degrees, deg,
unit<std::ratio<1, 180>, radians, std::ratio<1>>)
UNIT_ADD(angle, arcminute, arcminutes, arcmin, unit<std::ratio<1, 60>, degrees>)

3
wpimath/src/main/native/include/wpi/units/angular_jerk.hpp
View File

@@ -20,7 +20,8 @@ namespace wpi::units {
* @sa See unit_t for more information on unit type containers.
*/
UNIT_ADD(angular_jerk, radians_per_second_cubed, radians_per_second_cubed,
rad_per_s_cu, unit<std::ratio<1>, wpi::units::category::angular_jerk_unit>)
rad_per_s_cu,
unit<std::ratio<1>, wpi::units::category::angular_jerk_unit>)
UNIT_ADD(angular_jerk, degrees_per_second_cubed, degrees_per_second_cubed,
deg_per_s_cu,
compound_unit<angle::degrees, inverse<cubed<time::seconds>>>)

5
wpimath/src/main/native/include/wpi/units/charge.hpp
View File

@@ -41,8 +41,9 @@ namespace wpi::units {
*/
#if !defined(DISABLE_PREDEFINED_UNITS) || \
defined(ENABLE_PREDEFINED_CHARGE_UNITS)
UNIT_ADD_WITH_METRIC_PREFIXES(charge, coulomb, coulombs, C,
unit<std::ratio<1>, wpi::units::category::charge_unit>)
UNIT_ADD_WITH_METRIC_PREFIXES(
charge, coulomb, coulombs, C,
unit<std::ratio<1>, wpi::units::category::charge_unit>)
UNIT_ADD_WITH_METRIC_PREFIXES(charge, ampere_hour, ampere_hours, Ah,
compound_unit<current::ampere, time::hours>)

3
wpimath/src/main/native/include/wpi/units/constants.hpp
View File

@@ -64,7 +64,8 @@ static constexpr const unit_t<compound_unit<energy::joule, time::seconds>> h(
static constexpr const unit_t<
compound_unit<force::newtons, inverse<squared<current::ampere>>>>
mu0(pi * 4.0e-7 * force::newton_t(1) /
wpi::units::math::cpow<2>(current::ampere_t(1))); ///< vacuum permeability.
wpi::units::math::cpow<2>(
current::ampere_t(1))); ///< vacuum permeability.
static constexpr const unit_t<
compound_unit<capacitance::farad, inverse<length::meter>>>
epsilon0(1.0 / (mu0 * math::cpow<2>(c))); ///< vacuum permitivity.

4
wpimath/src/main/native/include/wpi/units/curvature.hpp
View File

@@ -9,6 +9,6 @@
#include "wpi/units/length.hpp"
namespace wpi::units {
using curvature_t = wpi::units::unit_t<
wpi::units::compound_unit<wpi::units::radians, wpi::units::inverse<wpi::units::meters>>>;
using curvature_t = wpi::units::unit_t<wpi::units::compound_unit<
wpi::units::radians, wpi::units::inverse<wpi::units::meters>>>;
} // namespace wpi::units

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