[wpimath] Add remaining struct and protobuf implementations (#5953)

wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java

@@ -5,14 +5,30 @@
 package edu.wpi.first.math.controller;
 
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.proto.DifferentialDriveFeedforwardProto;
+import edu.wpi.first.math.controller.struct.DifferentialDriveFeedforwardStruct;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 
 /** A helper class which computes the feedforward outputs for a differential drive drivetrain. */
-public class DifferentialDriveFeedforward {
+public class DifferentialDriveFeedforward implements ProtobufSerializable, StructSerializable {
   private final LinearSystem<N2, N2, N2> m_plant;
 
+  /** The linear velocity gain in volts per (meters per second). */
+  public final double m_kVLinear;
+
+  /** The linear acceleration gain in volts per (meters per second squared). */
+  public final double m_kALinear;
+
+  /** The angular velocity gain in volts per (radians per second). */
+  public final double m_kVAngular;
+
+  /** The angular acceleration gain in volts per (radians per second squared). */
+  public final double m_kAAngular;
+
   /**
    * Creates a new DifferentialDriveFeedforward with the specified parameters.
    *
@@ -25,9 +41,8 @@ public class DifferentialDriveFeedforward {
    */
   public DifferentialDriveFeedforward(
       double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
-    m_plant =
-        LinearSystemId.identifyDrivetrainSystem(
-            kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+    // See LinearSystemId.identifyDrivetrainSystem(double, double, double, double, double)
+    this(kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
   }
 
   /**
@@ -41,6 +56,10 @@ public class DifferentialDriveFeedforward {
   public DifferentialDriveFeedforward(
       double kVLinear, double kALinear, double kVAngular, double kAAngular) {
     m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+    m_kVLinear = kVLinear;
+    m_kALinear = kALinear;
+    m_kVAngular = kVAngular;
+    m_kAAngular = kAAngular;
   }
 
   /**
@@ -67,4 +86,12 @@ public class DifferentialDriveFeedforward {
     var u = feedforward.calculate(r, nextR);
     return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
   }
+
+  /** DifferentialDriveFeedforward struct for serialization. */
+  public static final DifferentialDriveFeedforwardStruct struct =
+      new DifferentialDriveFeedforwardStruct();
+
+  /** DifferentialDriveFeedforward protobuf for serialization. */
+  public static final DifferentialDriveFeedforwardProto proto =
+      new DifferentialDriveFeedforwardProto();
 }

wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java

@@ -6,13 +6,17 @@ package edu.wpi.first.math.controller;
 
 import static edu.wpi.first.units.Units.Volts;
 
+import edu.wpi.first.math.controller.proto.SimpleMotorFeedforwardProto;
+import edu.wpi.first.math.controller.struct.SimpleMotorFeedforwardStruct;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Unit;
 import edu.wpi.first.units.Velocity;
 import edu.wpi.first.units.Voltage;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 
 /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
-public class SimpleMotorFeedforward {
+public class SimpleMotorFeedforward implements ProtobufSerializable, StructSerializable {
   /** The static gain. */
   private final double ks;
 
@@ -287,4 +291,10 @@ public class SimpleMotorFeedforward {
   public double minAchievableAcceleration(double maxVoltage, double velocity) {
     return maxAchievableAcceleration(-maxVoltage, velocity);
   }
+
+  /** SimpleMotorFeedforward struct for serialization. */
+  public static final SimpleMotorFeedforwardStruct struct = new SimpleMotorFeedforwardStruct();
+
+  /** SimpleMotorFeedforward protobuf for serialization. */
+  public static final SimpleMotorFeedforwardProto proto = new SimpleMotorFeedforwardProto();
 }

wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProto.java

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.proto;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class DifferentialDriveFeedforwardProto
+    implements Protobuf<DifferentialDriveFeedforward, ProtobufDifferentialDriveFeedforward> {
+  @Override
+  public Class<DifferentialDriveFeedforward> getTypeClass() {
+    return DifferentialDriveFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveFeedforward createMessage() {
+    return ProtobufDifferentialDriveFeedforward.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveFeedforward unpack(ProtobufDifferentialDriveFeedforward msg) {
+    return new DifferentialDriveFeedforward(
+        msg.getKvLinear(), msg.getKaLinear(), msg.getKvAngular(), msg.getKaAngular());
+  }
+
+  @Override
+  public void pack(ProtobufDifferentialDriveFeedforward msg, DifferentialDriveFeedforward value) {
+    msg.setKvLinear(value.m_kVLinear);
+    msg.setKaLinear(value.m_kALinear);
+    msg.setKvAngular(value.m_kVAngular);
+    msg.setKaAngular(value.m_kAAngular);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProto.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.proto;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufSimpleMotorFeedforward;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class SimpleMotorFeedforwardProto
+    implements Protobuf<SimpleMotorFeedforward, ProtobufSimpleMotorFeedforward> {
+  @Override
+  public Class<SimpleMotorFeedforward> getTypeClass() {
+    return SimpleMotorFeedforward.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufSimpleMotorFeedforward.getDescriptor();
+  }
+
+  @Override
+  public ProtobufSimpleMotorFeedforward createMessage() {
+    return ProtobufSimpleMotorFeedforward.newInstance();
+  }
+
+  @Override
+  public SimpleMotorFeedforward unpack(ProtobufSimpleMotorFeedforward msg) {
+    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()).setDt(value.getDt());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStruct.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class DifferentialDriveFeedforwardStruct
+    implements Struct<DifferentialDriveFeedforward> {
+  @Override
+  public Class<DifferentialDriveFeedforward> getTypeClass() {
+    return DifferentialDriveFeedforward.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "DifferentialDriveFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double kVLinear;double kALinear;double kVAngular;double kAAngular";
+  }
+
+  @Override
+  public DifferentialDriveFeedforward unpack(ByteBuffer bb) {
+    double kVLinear = bb.getDouble();
+    double kALinear = bb.getDouble();
+    double kVAngular = bb.getDouble();
+    double kAAngular = bb.getDouble();
+    return new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveFeedforward value) {
+    bb.putDouble(value.m_kVLinear);
+    bb.putDouble(value.m_kALinear);
+    bb.putDouble(value.m_kVAngular);
+    bb.putDouble(value.m_kAAngular);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStruct.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller.struct;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class SimpleMotorFeedforwardStruct implements Struct<SimpleMotorFeedforward> {
+  @Override
+  public Class<SimpleMotorFeedforward> getTypeClass() {
+    return SimpleMotorFeedforward.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "SimpleMotorFeedforward";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    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();
+    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());
+  }
+}