Add dt to SimpleMotorFeedforward serialization

KangarooKoala · Jul 21, 2024 · a812c07 · a812c07
1 parent e7d18a1
commit a812c07
Show file tree

Hide file tree

Showing 11 changed files with 134 additions and 40 deletions.
diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProto.java
@@ -28,11 +28,11 @@ public ProtobufSimpleMotorFeedforward createMessage() {
 
   @Override
   public SimpleMotorFeedforward unpack(ProtobufSimpleMotorFeedforward msg) {
-    return new SimpleMotorFeedforward(msg.getKs(), msg.getKv(), msg.getKa());
+    return new SimpleMotorFeedforward(msg.getKs(), msg.getKv(), msg.getKa(), msg.getDt());
   }
 
   @Override
   public void pack(ProtobufSimpleMotorFeedforward msg, SimpleMotorFeedforward value) {
-    msg.setKs(value.getKs()).setKv(value.getKv()).setKa(value.getKa());
+    msg.setKs(value.getKs()).setKv(value.getKv()).setKa(value.getKa()).setDt(value.getDt());
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStruct.java
@@ -21,26 +21,28 @@ public String getTypeString() {
 
   @Override
   public int getSize() {
-    return kSizeDouble * 3;
+    return kSizeDouble * 4;
   }
 
   @Override
   public String getSchema() {
-    return "double ks;double kv;double ka";
+    return "double ks;double kv;double ka;double dt";
   }
 
   @Override
   public SimpleMotorFeedforward unpack(ByteBuffer bb) {
     double ks = bb.getDouble();
     double kv = bb.getDouble();
     double ka = bb.getDouble();
-    return new SimpleMotorFeedforward(ks, kv, ka);
+    double dt = bb.getDouble();
+    return new SimpleMotorFeedforward(ks, kv, ka, dt);
   }
 
   @Override
   public void pack(ByteBuffer bb, SimpleMotorFeedforward value) {
     bb.putDouble(value.getKs());
     bb.putDouble(value.getKv());
     bb.putDouble(value.getKa());
+    bb.putDouble(value.getDt());
   }
 }
diff --git a/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc b/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc
@@ -25,7 +25,8 @@ wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Unpack(
           units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
               m->kv()},
           units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
-              m->ka()}};
+              m->ka()},
+          units::second_t{m->dt()}};
 }
 
 template <class Distance>
@@ -40,4 +41,5 @@ void wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Pack(
   m->set_ka(units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
       value.GetKa()}
                 .value());
+  m->set_dt(units::second_t{value.GetDt()}.value());
 }
diff --git a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h
@@ -17,9 +17,9 @@ struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
   static constexpr std::string_view GetTypeString() {
     return "struct:SimpleMotorFeedforward";
   }
-  static constexpr size_t GetSize() { return 24; }
+  static constexpr size_t GetSize() { return 32; }
   static constexpr std::string_view GetSchema() {
-    return "double ks;double kv;double ka";
+    return "double ks;double kv;double ka;double dt";
   }
 
   static frc::SimpleMotorFeedforward<Distance> Unpack(

diff --git a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc
@@ -13,11 +13,13 @@ wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Unpack(
   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<double, kKsOff>(data)},
           units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
               wpi::UnpackStruct<double, kKvOff>(data)},
           units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
-              wpi::UnpackStruct<double, kKaOff>(data)}};
+              wpi::UnpackStruct<double, kKaOff>(data)},
+          units::second_t{wpi::UnpackStruct<double, kDtOff>(data)}};
 }
 
 template <class Distance>
@@ -27,6 +29,7 @@ void wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Pack(
   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<kKsOff>(data, value.GetKs().value());
   wpi::PackStruct<kKvOff>(
       data,
@@ -38,4 +41,5 @@ void wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Pack(
       units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
           value.GetKa()}
           .value());
+  wpi::PackStruct<kDtOff>(data, units::second_t{value.GetDt()}.value());
 }
diff --git a/wpimath/src/main/proto/controller.proto b/wpimath/src/main/proto/controller.proto
@@ -29,6 +29,7 @@ message ProtobufSimpleMotorFeedforward {
   double ks = 1;
   double kv = 2;
   double ka = 3;
+  double dt = 4;
 }
 
 message ProtobufDifferentialDriveWheelVoltages {

diff --git a/...th/src/test/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProtoTest.java b/...th/src/test/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProtoTest.java
@@ -14,13 +14,14 @@
 class SimpleMotorFeedforwardProtoTest
     extends ProtoTestBase<SimpleMotorFeedforward, ProtobufSimpleMotorFeedforward> {
   SimpleMotorFeedforwardProtoTest() {
-    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7), SimpleMotorFeedforward.proto);
+    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.proto);
   }
 
   @Override
   public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) {
     assertEquals(testData.getKs(), data.getKs());
     assertEquals(testData.getKv(), data.getKv());
     assertEquals(testData.getKa(), data.getKa());
+    assertEquals(testData.getDt(), data.getDt());
   }
 }
diff --git a/.../src/test/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStructTest.java b/.../src/test/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStructTest.java
@@ -12,13 +12,14 @@
 @SuppressWarnings("PMD.TestClassWithoutTestCases")
 class SimpleMotorFeedforwardStructTest extends StructTestBase<SimpleMotorFeedforward> {
   SimpleMotorFeedforwardStructTest() {
-    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7), SimpleMotorFeedforward.struct);
+    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.struct);
   }
 
   @Override
   public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) {
     assertEquals(testData.getKs(), data.getKs());
     assertEquals(testData.getKv(), data.getKv());
     assertEquals(testData.getKa(), data.getKa());
+    assertEquals(testData.getDt(), data.getDt());
   }
 }
diff --git a/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp
@@ -17,12 +17,13 @@ struct SimpleMotorFeedforwardProtoTestData {
 
   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};
+                                        units::volt_t{0.7} / 1_mps_sq, 25_ms};
 
   static void CheckEq(const Type& testData, const Type& data) {
     EXPECT_EQ(testData.GetKs().value(), data.GetKs().value());
     EXPECT_EQ(testData.GetKv().value(), data.GetKv().value());
     EXPECT_EQ(testData.GetKa().value(), data.GetKa().value());
+    EXPECT_EQ(testData.GetDt().value(), data.GetDt().value());
   }
 };