From 59dbfc9c2d267d5f4587f90fde844b0e982056e6 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Wed, 27 Nov 2024 23:02:31 -0800 Subject: [PATCH] [wpimath] Improve C++ SimpleMotorFeedforward unit type support (#7440) Allow using non-base types Allow using angles for serde --- .../frc/controller/SimpleMotorFeedforward.h | 3 +- .../proto/SimpleMotorFeedforwardProto.h | 30 ++++++++++--------- .../struct/SimpleMotorFeedforwardStruct.h | 25 +++++++++------- .../proto/SimpleMotorFeedforwardProtoTest.cpp | 20 +++++++++---- .../SimpleMotorFeedforwardStructTest.cpp | 20 +++++++++---- 5 files changed, 60 insertions(+), 38 deletions(-) diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 69a4a05eff..932172810d 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -20,8 +20,7 @@ namespace frc { * permanent-magnet DC motor. */ template - requires std::same_as || - std::same_as + requires units::length_unit || units::angle_unit class SimpleMotorFeedforward { public: using Velocity = diff --git a/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h b/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h index fc5bfb1cb4..aed953150a 100644 --- a/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h +++ b/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h @@ -12,10 +12,12 @@ #include "wpimath/protobuf/controller.npb.h" // Everything is converted into units for -// frc::SimpleMotorFeedforward +// frc::SimpleMotorFeedforward or +// frc::SimpleMotorFeedforward template -struct wpi::Protobuf> { // NOLINT + requires units::length_unit || units::angle_unit +struct wpi::Protobuf> { using MessageStruct = wpi_proto_ProtobufSimpleMotorFeedforward; using InputStream = wpi::ProtoInputStream>; @@ -24,6 +26,9 @@ struct wpi::Protobuf> { // NOLINT static std::optional> Unpack( InputStream& stream) { + using BaseUnit = + units::unit, units::traits::base_unit_of>; + using BaseFeedforward = frc::SimpleMotorFeedforward; wpi_proto_ProtobufSimpleMotorFeedforward msg; if (!stream.Decode(msg)) { return {}; @@ -31,26 +36,23 @@ struct wpi::Protobuf> { // NOLINT return frc::SimpleMotorFeedforward{ units::volt_t{msg.ks}, - units::unit_t::kv_unit>{ - msg.kv}, - units::unit_t::ka_unit>{ - msg.ka}, + units::unit_t{msg.kv}, + units::unit_t{msg.ka}, units::second_t{msg.dt}, }; } static bool Pack(OutputStream& stream, const frc::SimpleMotorFeedforward& value) { + using BaseUnit = + units::unit, units::traits::base_unit_of>; + using BaseFeedforward = frc::SimpleMotorFeedforward; wpi_proto_ProtobufSimpleMotorFeedforward msg{ .ks = value.GetKs().value(), - .kv = - units::unit_t::kv_unit>{ - value.GetKv()} - .value(), - .ka = - units::unit_t::ka_unit>{ - value.GetKa()} - .value(), + .kv = units::unit_t{value.GetKv()} + .value(), + .ka = units::unit_t{value.GetKa()} + .value(), .dt = units::second_t{value.GetDt()}.value(), }; return stream.Encode(msg); diff --git a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h index 156eef5d4b..12b3186f7b 100644 --- a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h +++ b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h @@ -14,6 +14,7 @@ // frc::SimpleMotorFeedforward template + requires units::length_unit || units::angle_unit struct wpi::Struct> { static constexpr std::string_view GetTypeName() { return "SimpleMotorFeedforward"; @@ -25,40 +26,44 @@ struct wpi::Struct> { static frc::SimpleMotorFeedforward Unpack( std::span data) { + using BaseUnit = + units::unit, units::traits::base_unit_of>; + using BaseFeedforward = frc::SimpleMotorFeedforward; constexpr size_t kKsOff = 0; constexpr size_t kKvOff = kKsOff + 8; constexpr size_t kKaOff = kKvOff + 8; constexpr size_t kDtOff = kKaOff + 8; return {units::volt_t{wpi::UnpackStruct(data)}, - units::unit_t::kv_unit>{ + units::unit_t{ wpi::UnpackStruct(data)}, - units::unit_t::ka_unit>{ + units::unit_t{ wpi::UnpackStruct(data)}, units::second_t{wpi::UnpackStruct(data)}}; } static void Pack(std::span data, const frc::SimpleMotorFeedforward& value) { + using BaseUnit = + units::unit, units::traits::base_unit_of>; + using BaseFeedforward = frc::SimpleMotorFeedforward; constexpr size_t kKsOff = 0; constexpr size_t kKvOff = kKsOff + 8; constexpr size_t kKaOff = kKvOff + 8; constexpr size_t kDtOff = kKaOff + 8; wpi::PackStruct(data, value.GetKs().value()); wpi::PackStruct( - data, - units::unit_t::kv_unit>{ - value.GetKv()} - .value()); + data, units::unit_t{value.GetKv()} + .value()); wpi::PackStruct( - data, - units::unit_t::ka_unit>{ - value.GetKa()} - .value()); + data, units::unit_t{value.GetKa()} + .value()); wpi::PackStruct(data, units::second_t{value.GetDt()}.value()); } }; static_assert( wpi::StructSerializable>); +static_assert( + wpi::StructSerializable>); static_assert( wpi::StructSerializable>); diff --git a/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp index e49bafc652..ff3ee3a75a 100644 --- a/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp +++ b/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp @@ -12,12 +12,13 @@ using namespace frc; +template struct SimpleMotorFeedforwardProtoTestData { - using Type = SimpleMotorFeedforward; + using Type = SimpleMotorFeedforward; - inline static const Type kTestData = {units::volt_t{0.4}, - units::volt_t{4.0} / 1_mps, - units::volt_t{0.7} / 1_mps_sq, 25_ms}; + inline static const Type kTestData = { + units::volt_t{0.4}, units::volt_t{4.0} / (units::unit_t{1} / 1_s), + units::volt_t{0.7} / (units::unit_t{1} / 1_s / 1_s), 25_ms}; static void CheckEq(const Type& testData, const Type& data) { EXPECT_EQ(testData.GetKs().value(), data.GetKs().value()); @@ -27,5 +28,12 @@ struct SimpleMotorFeedforwardProtoTestData { } }; -INSTANTIATE_TYPED_TEST_SUITE_P(SimpleMotorFeedforwardMeters, ProtoTest, - SimpleMotorFeedforwardProtoTestData); +INSTANTIATE_TYPED_TEST_SUITE_P( + SimpleMotorFeedforwardMeters, ProtoTest, + SimpleMotorFeedforwardProtoTestData); +INSTANTIATE_TYPED_TEST_SUITE_P( + SimpleMotorFeedforwardFeet, ProtoTest, + SimpleMotorFeedforwardProtoTestData); +INSTANTIATE_TYPED_TEST_SUITE_P( + SimpleMotorFeedforwardRadians, ProtoTest, + SimpleMotorFeedforwardProtoTestData); diff --git a/wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp index ca88fdaa47..cb8aec9e97 100644 --- a/wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp +++ b/wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp @@ -12,12 +12,13 @@ using namespace frc; +template struct SimpleMotorFeedforwardStructTestData { - using Type = SimpleMotorFeedforward; + using Type = SimpleMotorFeedforward; - inline static const Type kTestData = {units::volt_t{0.4}, - units::volt_t{4.0} / 1_mps, - units::volt_t{0.7} / 1_mps_sq, 25_ms}; + inline static const Type kTestData = { + units::volt_t{0.4}, units::volt_t{4.0} / (units::unit_t{1} / 1_s), + units::volt_t{0.7} / (units::unit_t{1} / 1_s / 1_s), 25_ms}; static void CheckEq(const Type& testData, const Type& data) { EXPECT_EQ(testData.GetKs().value(), data.GetKs().value()); @@ -27,5 +28,12 @@ struct SimpleMotorFeedforwardStructTestData { } }; -INSTANTIATE_TYPED_TEST_SUITE_P(SimpleMotorFeedforwardMeters, StructTest, - SimpleMotorFeedforwardStructTestData); +INSTANTIATE_TYPED_TEST_SUITE_P( + SimpleMotorFeedforwardMeters, StructTest, + SimpleMotorFeedforwardStructTestData); +INSTANTIATE_TYPED_TEST_SUITE_P( + SimpleMotorFeedforwardFeet, StructTest, + SimpleMotorFeedforwardStructTestData); +INSTANTIATE_TYPED_TEST_SUITE_P( + SimpleMotorFeedforwardRadians, StructTest, + SimpleMotorFeedforwardStructTestData);