diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index 2e9a8d8434..8c53f61958 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -4,6 +4,9 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.controller.proto.ArmFeedforwardProto; +import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct; + /** * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting * against the force of gravity on a beam suspended at an angle). @@ -14,6 +17,9 @@ public class ArmFeedforward { public final double kv; public final double ka; + public static final ArmFeedforwardProto proto = new ArmFeedforwardProto(); + public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct(); + /** * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate * units of the computed feedforward. diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java index 2fbd0aa381..ff5e81475b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java @@ -4,11 +4,19 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.controller.proto.DifferentialDriveWheelVoltagesProto; +import edu.wpi.first.math.controller.struct.DifferentialDriveWheelVoltagesStruct; + /** Motor voltages for a differential drive. */ public class DifferentialDriveWheelVoltages { public double left; public double right; + public static final DifferentialDriveWheelVoltagesProto proto = + new DifferentialDriveWheelVoltagesProto(); + public static final DifferentialDriveWheelVoltagesStruct struct = + new DifferentialDriveWheelVoltagesStruct(); + public DifferentialDriveWheelVoltages() {} public DifferentialDriveWheelVoltages(double left, double right) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 476a1c55ad..b7986caedf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -6,6 +6,8 @@ package edu.wpi.first.math.controller; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto; +import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct; import edu.wpi.first.math.system.plant.LinearSystemId; /** @@ -18,6 +20,9 @@ public class ElevatorFeedforward { public final double kv; public final double ka; + public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto(); + public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct(); + /** * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will * dictate units of the computed feedforward. diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java new file mode 100644 index 0000000000..ef3953a54d --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java @@ -0,0 +1,40 @@ +// 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.ArmFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class ArmFeedforwardProto implements Protobuf { + @Override + public Class getTypeClass() { + return ArmFeedforward.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufArmFeedforward.getDescriptor(); + } + + @Override + public ProtobufArmFeedforward createMessage() { + return ProtobufArmFeedforward.newInstance(); + } + + @Override + public ArmFeedforward unpack(ProtobufArmFeedforward msg) { + return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa()); + } + + @Override + public void pack(ProtobufArmFeedforward msg, ArmFeedforward value) { + msg.setKs(value.ks); + msg.setKg(value.kg); + msg.setKv(value.kv); + msg.setKa(value.ka); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java new file mode 100644 index 0000000000..c22db5d107 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java @@ -0,0 +1,40 @@ +// 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.DifferentialDriveWheelVoltages; +import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelVoltagesProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelVoltages.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveWheelVoltages.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveWheelVoltages createMessage() { + return ProtobufDifferentialDriveWheelVoltages.newInstance(); + } + + @Override + public DifferentialDriveWheelVoltages unpack(ProtobufDifferentialDriveWheelVoltages msg) { + return new DifferentialDriveWheelVoltages(msg.getLeft(), msg.getRight()); + } + + @Override + public void pack( + ProtobufDifferentialDriveWheelVoltages msg, DifferentialDriveWheelVoltages value) { + msg.setLeft(value.left); + msg.setRight(value.right); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java new file mode 100644 index 0000000000..66399e1a0c --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java @@ -0,0 +1,41 @@ +// 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.ElevatorFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class ElevatorFeedforwardProto + implements Protobuf { + @Override + public Class getTypeClass() { + return ElevatorFeedforward.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufElevatorFeedforward.getDescriptor(); + } + + @Override + public ProtobufElevatorFeedforward createMessage() { + return ProtobufElevatorFeedforward.newInstance(); + } + + @Override + public ElevatorFeedforward unpack(ProtobufElevatorFeedforward msg) { + return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa()); + } + + @Override + public void pack(ProtobufElevatorFeedforward msg, ElevatorFeedforward value) { + msg.setKs(value.ks); + msg.setKg(value.kg); + msg.setKv(value.kv); + msg.setKa(value.ka); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java new file mode 100644 index 0000000000..de72fea1bd --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.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.ArmFeedforward; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ArmFeedforwardStruct implements Struct { + @Override + public Class getTypeClass() { + return ArmFeedforward.class; + } + + @Override + public String getTypeString() { + return "struct:ArmFeedforward"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double ks;double kg;double kv;double ka"; + } + + @Override + public ArmFeedforward unpack(ByteBuffer bb) { + double ks = bb.getDouble(); + double kg = bb.getDouble(); + double kv = bb.getDouble(); + double ka = bb.getDouble(); + return new ArmFeedforward(ks, kg, kv, ka); + } + + @Override + public void pack(ByteBuffer bb, ArmFeedforward value) { + bb.putDouble(value.ks); + bb.putDouble(value.kg); + bb.putDouble(value.kv); + bb.putDouble(value.ka); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java new file mode 100644 index 0000000000..1695d812e0 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java @@ -0,0 +1,45 @@ +// 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.DifferentialDriveWheelVoltages; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveWheelVoltagesStruct + implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelVoltages.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveWheelVoltages"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double left;double right"; + } + + @Override + public DifferentialDriveWheelVoltages unpack(ByteBuffer bb) { + double left = bb.getDouble(); + double right = bb.getDouble(); + return new DifferentialDriveWheelVoltages(left, right); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveWheelVoltages value) { + bb.putDouble(value.left); + bb.putDouble(value.right); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java new file mode 100644 index 0000000000..f8b5559530 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.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.ElevatorFeedforward; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ElevatorFeedforwardStruct implements Struct { + @Override + public Class getTypeClass() { + return ElevatorFeedforward.class; + } + + @Override + public String getTypeString() { + return "struct:ElevatorFeedforward"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double ks;double kg;double kv;double ka"; + } + + @Override + public ElevatorFeedforward unpack(ByteBuffer bb) { + double ks = bb.getDouble(); + double kg = bb.getDouble(); + double kv = bb.getDouble(); + double ka = bb.getDouble(); + return new ElevatorFeedforward(ks, kg, kv, ka); + } + + @Override + public void pack(ByteBuffer bb, ElevatorFeedforward value) { + bb.putDouble(value.ks); + bb.putDouble(value.kg); + bb.putDouble(value.kv); + bb.putDouble(value.ka); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 43b67405d3..759398a04c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -11,6 +11,8 @@ import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto; +import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; @@ -36,6 +38,9 @@ public class ChassisSpeeds { /** Represents the angular velocity of the robot frame. (CCW is +) */ public double omegaRadiansPerSecond; + public static final ChassisSpeedsProto proto = new ChassisSpeedsProto(); + public static final ChassisSpeedsStruct struct = new ChassisSpeedsStruct(); + /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */ public ChassisSpeeds() {} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index 87907249e7..b892daa574 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -9,6 +9,8 @@ import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto; +import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; @@ -24,6 +26,11 @@ public class DifferentialDriveKinematics implements Kinematics { public final double trackWidthMeters; + public static final DifferentialDriveKinematicsProto proto = + new DifferentialDriveKinematicsProto(); + public static final DifferentialDriveKinematicsStruct struct = + new DifferentialDriveKinematicsStruct(); + /** * Constructs a differential drive kinematics object. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java index 72578226bb..17a48e226f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java @@ -6,6 +6,8 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.MetersPerSecond; +import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelSpeedsProto; +import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Velocity; @@ -18,6 +20,11 @@ public class DifferentialDriveWheelSpeeds { /** Speed of the right side of the robot. */ public double rightMetersPerSecond; + public static final DifferentialDriveWheelSpeedsProto proto = + new DifferentialDriveWheelSpeedsProto(); + public static final DifferentialDriveWheelSpeedsStruct struct = + new DifferentialDriveWheelSpeedsStruct(); + /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */ public DifferentialDriveWheelSpeeds() {} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 76c857adcc..c9ff593a9f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.proto.MecanumDriveKinematicsProto; +import edu.wpi.first.math.kinematics.struct.MecanumDriveKinematicsStruct; import org.ejml.simple.SimpleMatrix; /** @@ -42,6 +44,9 @@ public class MecanumDriveKinematics private Translation2d m_prevCoR = new Translation2d(); + public static final MecanumDriveKinematicsProto proto = new MecanumDriveKinematicsProto(); + public static final MecanumDriveKinematicsStruct struct = new MecanumDriveKinematicsStruct(); + /** * Constructs a mecanum drive kinematics object. * @@ -207,4 +212,20 @@ public class MecanumDriveKinematics m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY()); m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY())); } + + public Translation2d getFrontLeft() { + return m_frontLeftWheelMeters; + } + + public Translation2d getFrontRight() { + return m_frontRightWheelMeters; + } + + public Translation2d getRearLeft() { + return m_rearLeftWheelMeters; + } + + public Translation2d getRearRight() { + return m_rearRightWheelMeters; + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java index 2ebb77a134..5515802fc4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java @@ -7,6 +7,8 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelPositionsProto; +import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelPositionsStruct; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import java.util.Objects; @@ -24,6 +26,10 @@ public class MecanumDriveWheelPositions implements WheelPositions { /** Angle of the module. */ public Rotation2d angle = Rotation2d.fromDegrees(0); + public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct(); + public static final SwerveModuleStateProto proto = new SwerveModuleStateProto(); + /** Constructs a SwerveModuleState with zeros for speed and angle. */ public SwerveModuleState() {} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java new file mode 100644 index 0000000000..b20d23bbc9 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java @@ -0,0 +1,39 @@ +// 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.kinematics.proto; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufChassisSpeeds; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class ChassisSpeedsProto implements Protobuf { + @Override + public Class getTypeClass() { + return ChassisSpeeds.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufChassisSpeeds.getDescriptor(); + } + + @Override + public ProtobufChassisSpeeds createMessage() { + return ProtobufChassisSpeeds.newInstance(); + } + + @Override + public ChassisSpeeds unpack(ProtobufChassisSpeeds msg) { + return new ChassisSpeeds(msg.getVx(), msg.getVy(), msg.getOmega()); + } + + @Override + public void pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) { + msg.setVx(value.vxMetersPerSecond); + msg.setVy(value.vyMetersPerSecond); + msg.setOmega(value.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java new file mode 100644 index 0000000000..d1a8969d16 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.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.kinematics.proto; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveKinematics; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveKinematicsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveKinematics.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveKinematics.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveKinematics createMessage() { + return ProtobufDifferentialDriveKinematics.newInstance(); + } + + @Override + public DifferentialDriveKinematics unpack(ProtobufDifferentialDriveKinematics msg) { + return new DifferentialDriveKinematics(msg.getTrackWidth()); + } + + @Override + public void pack(ProtobufDifferentialDriveKinematics msg, DifferentialDriveKinematics value) { + msg.setTrackWidth(value.trackWidthMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java new file mode 100644 index 0000000000..e30df56574 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java @@ -0,0 +1,39 @@ +// 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.kinematics.proto; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelSpeeds; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelSpeedsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelSpeeds.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveWheelSpeeds.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveWheelSpeeds createMessage() { + return ProtobufDifferentialDriveWheelSpeeds.newInstance(); + } + + @Override + public DifferentialDriveWheelSpeeds unpack(ProtobufDifferentialDriveWheelSpeeds msg) { + return new DifferentialDriveWheelSpeeds(msg.getLeft(), msg.getRight()); + } + + @Override + public void pack(ProtobufDifferentialDriveWheelSpeeds msg, DifferentialDriveWheelSpeeds value) { + msg.setLeft(value.leftMetersPerSecond); + msg.setRight(value.rightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java new file mode 100644 index 0000000000..f5b8f3bdfb --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java @@ -0,0 +1,51 @@ +// 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.kinematics.proto; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveKinematics; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MecanumDriveKinematicsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveKinematics.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveKinematics.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto}; + } + + @Override + public ProtobufMecanumDriveKinematics createMessage() { + return ProtobufMecanumDriveKinematics.newInstance(); + } + + @Override + public MecanumDriveKinematics unpack(ProtobufMecanumDriveKinematics msg) { + return new MecanumDriveKinematics( + Translation2d.proto.unpack(msg.getFrontLeft()), + Translation2d.proto.unpack(msg.getFrontRight()), + Translation2d.proto.unpack(msg.getRearLeft()), + Translation2d.proto.unpack(msg.getRearRight())); + } + + @Override + public void pack(ProtobufMecanumDriveKinematics msg, MecanumDriveKinematics value) { + Translation2d.proto.pack(msg.getMutableFrontLeft(), value.getFrontLeft()); + Translation2d.proto.pack(msg.getMutableFrontRight(), value.getFrontRight()); + Translation2d.proto.pack(msg.getMutableRearLeft(), value.getRearLeft()); + Translation2d.proto.pack(msg.getMutableRearRight(), value.getRearRight()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java new file mode 100644 index 0000000000..d4f0f5a3ce --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.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.kinematics.proto; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelPositions; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MecanumDriveWheelPositionsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveWheelPositions.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveWheelPositions.getDescriptor(); + } + + @Override + public ProtobufMecanumDriveWheelPositions createMessage() { + return ProtobufMecanumDriveWheelPositions.newInstance(); + } + + @Override + public MecanumDriveWheelPositions unpack(ProtobufMecanumDriveWheelPositions msg) { + return new MecanumDriveWheelPositions( + msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight()); + } + + @Override + public void pack(ProtobufMecanumDriveWheelPositions msg, MecanumDriveWheelPositions value) { + msg.setFrontLeft(value.frontLeftMeters); + msg.setFrontRight(value.frontRightMeters); + msg.setRearLeft(value.rearLeftMeters); + msg.setRearRight(value.rearRightMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java new file mode 100644 index 0000000000..85e8fd10a3 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.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.kinematics.proto; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelSpeeds; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MecanumDriveWheelSpeedsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveWheelSpeeds.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveWheelSpeeds.getDescriptor(); + } + + @Override + public ProtobufMecanumDriveWheelSpeeds createMessage() { + return ProtobufMecanumDriveWheelSpeeds.newInstance(); + } + + @Override + public MecanumDriveWheelSpeeds unpack(ProtobufMecanumDriveWheelSpeeds msg) { + return new MecanumDriveWheelSpeeds( + msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight()); + } + + @Override + public void pack(ProtobufMecanumDriveWheelSpeeds msg, MecanumDriveWheelSpeeds value) { + msg.setFrontLeft(value.frontLeftMetersPerSecond); + msg.setFrontRight(value.frontRightMetersPerSecond); + msg.setRearLeft(value.rearLeftMetersPerSecond); + msg.setRearRight(value.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java new file mode 100644 index 0000000000..fe5ec5fc66 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java @@ -0,0 +1,45 @@ +// 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.kinematics.proto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModulePosition; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class SwerveModulePositionProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SwerveModulePosition.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSwerveModulePosition.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Rotation2d.proto}; + } + + @Override + public ProtobufSwerveModulePosition createMessage() { + return ProtobufSwerveModulePosition.newInstance(); + } + + @Override + public SwerveModulePosition unpack(ProtobufSwerveModulePosition msg) { + return new SwerveModulePosition(msg.getDistance(), Rotation2d.proto.unpack(msg.getAngle())); + } + + @Override + public void pack(ProtobufSwerveModulePosition msg, SwerveModulePosition value) { + msg.setDistance(value.distanceMeters); + Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java new file mode 100644 index 0000000000..dc30f91117 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java @@ -0,0 +1,45 @@ +// 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.kinematics.proto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleState; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class SwerveModuleStateProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SwerveModuleState.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSwerveModuleState.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Rotation2d.proto}; + } + + @Override + public ProtobufSwerveModuleState createMessage() { + return ProtobufSwerveModuleState.newInstance(); + } + + @Override + public SwerveModuleState unpack(ProtobufSwerveModuleState msg) { + return new SwerveModuleState(msg.getSpeed(), Rotation2d.proto.unpack(msg.getAngle())); + } + + @Override + public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) { + msg.setSpeed(value.speedMetersPerSecond); + Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java new file mode 100644 index 0000000000..ea26f86e62 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java @@ -0,0 +1,46 @@ +// 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.kinematics.struct; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ChassisSpeedsStruct implements Struct { + @Override + public Class getTypeClass() { + return ChassisSpeeds.class; + } + + @Override + public String getTypeString() { + return "struct:ChassisSpeeds"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double vx;double vy;double omega"; + } + + @Override + public ChassisSpeeds unpack(ByteBuffer bb) { + double vx = bb.getDouble(); + double vy = bb.getDouble(); + double omega = bb.getDouble(); + return new ChassisSpeeds(vx, vy, omega); + } + + @Override + public void pack(ByteBuffer bb, ChassisSpeeds value) { + bb.putDouble(value.vxMetersPerSecond); + bb.putDouble(value.vyMetersPerSecond); + bb.putDouble(value.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java new file mode 100644 index 0000000000..1ddadaac1f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.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.kinematics.struct; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveKinematicsStruct implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveKinematics.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveKinematics"; + } + + @Override + public int getSize() { + return kSizeDouble; + } + + @Override + public String getSchema() { + return "double track_width"; + } + + @Override + public DifferentialDriveKinematics unpack(ByteBuffer bb) { + double trackWidth = bb.getDouble(); + return new DifferentialDriveKinematics(trackWidth); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveKinematics value) { + bb.putDouble(value.trackWidthMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java new file mode 100644 index 0000000000..c124247905 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java @@ -0,0 +1,44 @@ +// 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.kinematics.struct; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveWheelSpeedsStruct implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelSpeeds.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveWheelSpeeds"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double left;double right"; + } + + @Override + public DifferentialDriveWheelSpeeds unpack(ByteBuffer bb) { + double left = bb.getDouble(); + double right = bb.getDouble(); + return new DifferentialDriveWheelSpeeds(left, right); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveWheelSpeeds value) { + bb.putDouble(value.leftMetersPerSecond); + bb.putDouble(value.rightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java new file mode 100644 index 0000000000..227f80ed40 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java @@ -0,0 +1,55 @@ +// 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.kinematics.struct; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class MecanumDriveKinematicsStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveKinematics.class; + } + + @Override + public String getTypeString() { + return "struct:MecanumDriveKinematics"; + } + + @Override + public int getSize() { + return 4 * Translation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d front_left;Translation2d front_right;Translation2d rear_left;" + + "Translation2d rear_right"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct}; + } + + @Override + public MecanumDriveKinematics unpack(ByteBuffer bb) { + Translation2d frontLeft = Translation2d.struct.unpack(bb); + Translation2d frontRight = Translation2d.struct.unpack(bb); + Translation2d rearLeft = Translation2d.struct.unpack(bb); + Translation2d rearRight = Translation2d.struct.unpack(bb); + return new MecanumDriveKinematics(frontLeft, frontRight, rearLeft, rearRight); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveKinematics value) { + Translation2d.struct.pack(bb, value.getFrontLeft()); + Translation2d.struct.pack(bb, value.getFrontRight()); + Translation2d.struct.pack(bb, value.getRearLeft()); + Translation2d.struct.pack(bb, value.getRearRight()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java new file mode 100644 index 0000000000..9f8b182471 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.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.kinematics.struct; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class MecanumDriveWheelPositionsStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveWheelPositions.class; + } + + @Override + public String getTypeString() { + return "struct:MecanumDriveWheelPositions"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double front_left;double front_right;double rear_left;double rear_right"; + } + + @Override + public MecanumDriveWheelPositions unpack(ByteBuffer bb) { + double frontLeft = bb.getDouble(); + double frontRight = bb.getDouble(); + double rearLeft = bb.getDouble(); + double rearRight = bb.getDouble(); + return new MecanumDriveWheelPositions(frontLeft, frontRight, rearLeft, rearRight); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveWheelPositions value) { + bb.putDouble(value.frontLeftMeters); + bb.putDouble(value.frontRightMeters); + bb.putDouble(value.rearLeftMeters); + bb.putDouble(value.rearRightMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java new file mode 100644 index 0000000000..46d12763cd --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.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.kinematics.struct; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class MecanumDriveWheelSpeedsStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveWheelSpeeds.class; + } + + @Override + public String getTypeString() { + return "struct:MecanumDriveWheelSpeeds"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double front_left;double front_right;double rear_left;double rear_right"; + } + + @Override + public MecanumDriveWheelSpeeds unpack(ByteBuffer bb) { + double frontLeft = bb.getDouble(); + double frontRight = bb.getDouble(); + double rearLeft = bb.getDouble(); + double rearRight = bb.getDouble(); + return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveWheelSpeeds value) { + bb.putDouble(value.frontLeftMetersPerSecond); + bb.putDouble(value.frontRightMetersPerSecond); + bb.putDouble(value.rearLeftMetersPerSecond); + bb.putDouble(value.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java new file mode 100644 index 0000000000..21567328e9 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java @@ -0,0 +1,50 @@ +// 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.kinematics.struct; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class SwerveModulePositionStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveModulePosition.class; + } + + @Override + public String getTypeString() { + return "struct:SwerveModulePosition"; + } + + @Override + public int getSize() { + return kSizeDouble + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "double distance;Rotation2d angle"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Rotation2d.struct}; + } + + @Override + public SwerveModulePosition unpack(ByteBuffer bb) { + double distance = bb.getDouble(); + Rotation2d angle = Rotation2d.struct.unpack(bb); + return new SwerveModulePosition(distance, angle); + } + + @Override + public void pack(ByteBuffer bb, SwerveModulePosition value) { + bb.putDouble(value.distanceMeters); + Rotation2d.struct.pack(bb, value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java new file mode 100644 index 0000000000..51dd61f045 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java @@ -0,0 +1,50 @@ +// 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.kinematics.struct; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class SwerveModuleStateStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveModuleState.class; + } + + @Override + public String getTypeString() { + return "struct:SwerveModuleState"; + } + + @Override + public int getSize() { + return kSizeDouble + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "double speed;Rotation2d angle"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Rotation2d.struct}; + } + + @Override + public SwerveModuleState unpack(ByteBuffer bb) { + double speed = bb.getDouble(); + Rotation2d angle = Rotation2d.struct.unpack(bb); + return new SwerveModuleState(speed, angle); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleState value) { + bb.putDouble(value.speedMetersPerSecond); + Rotation2d.struct.pack(bb, value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java index 0c6e3346b2..9f9cafef7a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.system.plant; +import edu.wpi.first.math.system.plant.proto.DCMotorProto; +import edu.wpi.first.math.system.plant.struct.DCMotorStruct; import edu.wpi.first.math.util.Units; /** Holds the constants for a DC motor. */ @@ -17,6 +19,9 @@ public class DCMotor { public final double KvRadPerSecPerVolt; public final double KtNMPerAmp; + public static final DCMotorProto proto = new DCMotorProto(); + public static final DCMotorStruct struct = new DCMotorStruct(); + /** * Constructs a DC motor. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java new file mode 100644 index 0000000000..f9e4aa3957 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java @@ -0,0 +1,47 @@ +// 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.system.plant.proto; + +import edu.wpi.first.math.proto.Plant.ProtobufDCMotor; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DCMotorProto implements Protobuf { + @Override + public Class getTypeClass() { + return DCMotor.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDCMotor.getDescriptor(); + } + + @Override + public ProtobufDCMotor createMessage() { + return ProtobufDCMotor.newInstance(); + } + + @Override + public DCMotor unpack(ProtobufDCMotor msg) { + return new DCMotor( + msg.getNominalVoltage(), + msg.getStallTorque(), + msg.getStallCurrent(), + msg.getFreeCurrent(), + msg.getFreeSpeed(), + 1); + } + + @Override + public void pack(ProtobufDCMotor msg, DCMotor value) { + msg.setNominalVoltage(value.nominalVoltageVolts); + msg.setStallTorque(value.stallTorqueNewtonMeters); + msg.setStallCurrent(value.stallCurrentAmps); + msg.setFreeCurrent(value.freeCurrentAmps); + msg.setFreeSpeed(value.freeSpeedRadPerSec); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java new file mode 100644 index 0000000000..da159fc63a --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java @@ -0,0 +1,51 @@ +// 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.system.plant.struct; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DCMotorStruct implements Struct { + @Override + public Class getTypeClass() { + return DCMotor.class; + } + + @Override + public String getTypeString() { + return "struct:DCMotor"; + } + + @Override + public int getSize() { + return kSizeDouble * 5; + } + + @Override + public String getSchema() { + return "double nominal_voltage;double stall_torque;double stall_current;" + + "double free_current;double free_speed"; + } + + @Override + public DCMotor unpack(ByteBuffer bb) { + double nominalVoltage = bb.getDouble(); + double stallTorque = bb.getDouble(); + double stallCurrent = bb.getDouble(); + double freeCurrent = bb.getDouble(); + double freeSpeed = bb.getDouble(); + return new DCMotor(nominalVoltage, stallTorque, stallCurrent, freeCurrent, freeSpeed, 1); + } + + @Override + public void pack(ByteBuffer bb, DCMotor value) { + bb.putDouble(value.nominalVoltageVolts); + bb.putDouble(value.stallTorqueNewtonMeters); + bb.putDouble(value.stallCurrentAmps); + bb.putDouble(value.freeCurrentAmps); + bb.putDouble(value.freeSpeedRadPerSec); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java index f7120be7df..bc2dea3a4f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java @@ -7,6 +7,8 @@ package edu.wpi.first.math.trajectory; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.trajectory.proto.TrajectoryProto; +import edu.wpi.first.math.trajectory.proto.TrajectoryStateProto; import java.util.ArrayList; import java.util.List; import java.util.Objects; @@ -17,6 +19,8 @@ import java.util.stream.Collectors; * represent the pose, curvature, time elapsed, velocity, and acceleration at that point. */ public class Trajectory { + public static final TrajectoryProto proto = new TrajectoryProto(); + private final double m_totalTimeSeconds; private final List m_states; @@ -264,6 +268,8 @@ public class Trajectory { * represent the pose, curvature, time elapsed, velocity, and acceleration at that point. */ public static class State { + public static final TrajectoryStateProto proto = new TrajectoryStateProto(); + // The time elapsed since the beginning of the trajectory. @JsonProperty("time") public double timeSeconds; diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java new file mode 100644 index 0000000000..92e5ac7d2c --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java @@ -0,0 +1,52 @@ +// 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.trajectory.proto; + +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectory; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.util.protobuf.Protobuf; +import java.util.ArrayList; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class TrajectoryProto implements Protobuf { + @Override + public Class getTypeClass() { + return Trajectory.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTrajectory.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Trajectory.State.proto}; + } + + @Override + public ProtobufTrajectory createMessage() { + return ProtobufTrajectory.newInstance(); + } + + @Override + public Trajectory unpack(ProtobufTrajectory msg) { + ArrayList states = new ArrayList<>(msg.getStates().length()); + for (ProtobufTrajectoryState protoState : msg.getStates()) { + states.add(Trajectory.State.proto.unpack(protoState)); + } + + return new Trajectory(states); + } + + @Override + public void pack(ProtobufTrajectory msg, Trajectory value) { + var states = msg.getMutableStates().reserve(value.getStates().size()); + for (var item : value.getStates()) { + Trajectory.State.proto.pack(states.next(), item); + } + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java new file mode 100644 index 0000000000..e8900bfbd7 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java @@ -0,0 +1,52 @@ +// 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.trajectory.proto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class TrajectoryStateProto implements Protobuf { + @Override + public Class getTypeClass() { + return Trajectory.State.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTrajectoryState.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Pose2d.proto}; + } + + @Override + public ProtobufTrajectoryState createMessage() { + return ProtobufTrajectoryState.newInstance(); + } + + @Override + public Trajectory.State unpack(ProtobufTrajectoryState msg) { + return new Trajectory.State( + msg.getTime(), + msg.getVelocity(), + msg.getAcceleration(), + Pose2d.proto.unpack(msg.getPose()), + msg.getCurvature()); + } + + @Override + public void pack(ProtobufTrajectoryState msg, Trajectory.State value) { + msg.setTime(value.timeSeconds); + msg.setVelocity(value.velocityMetersPerSecond); + msg.setAcceleration(value.accelerationMetersPerSecondSq); + Pose2d.proto.pack(msg.getMutablePose(), value.poseMeters); + msg.setCurvature(value.curvatureRadPerMeter); + } +} diff --git a/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp b/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp new file mode 100644 index 0000000000..37a55cf631 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/controller/proto/ArmFeedforwardProto.h" + +#include "controller.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufArmFeedforward>(arena); +} + +frc::ArmFeedforward wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::ArmFeedforward{ + units::volt_t{m->ks()}, + units::volt_t{m->kg()}, + units::unit_t{m->kv()}, + units::unit_t{m->ka()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::ArmFeedforward& value) { + auto m = static_cast(msg); + m->set_ks(value.kS.value()); + m->set_kg(value.kG.value()); + m->set_kv(value.kV.value()); + m->set_ka(value.kA.value()); +} diff --git a/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp new file mode 100644 index 0000000000..b0b16b955d --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp @@ -0,0 +1,34 @@ +// 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. + +#include "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h" + +#include "controller.pb.h" + +google::protobuf::Message* wpi::Protobuf< + frc::DifferentialDriveWheelVoltages>::New(google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveWheelVoltages>(arena); +} + +frc::DifferentialDriveWheelVoltages +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + return frc::DifferentialDriveWheelVoltages{ + units::volt_t{m->left()}, + units::volt_t{m->right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveWheelVoltages& value) { + auto m = + static_cast(msg); + m->set_left(value.left.value()); + m->set_right(value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp b/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp new file mode 100644 index 0000000000..4b4bce2df6 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/controller/proto/ElevatorFeedforwardProto.h" + +#include "controller.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufElevatorFeedforward>(arena); +} + +frc::ElevatorFeedforward wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::ElevatorFeedforward{ + units::volt_t{m->ks()}, + units::volt_t{m->kg()}, + units::unit_t{m->kv()}, + units::unit_t{m->ka()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::ElevatorFeedforward& value) { + auto m = static_cast(msg); + m->set_ks(value.kS()); + m->set_kg(value.kG()); + m->set_kv(value.kV()); + m->set_ka(value.kA()); +} diff --git a/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp new file mode 100644 index 0000000000..0e6e8165ca --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/controller/struct/ArmFeedforwardStruct.h" + +namespace { +constexpr size_t kKsOff = 0; +constexpr size_t kKgOff = kKsOff + 8; +constexpr size_t kKvOff = kKgOff + 8; +constexpr size_t kKaOff = kKvOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::ArmFeedforward StructType::Unpack(std::span data) { + return frc::ArmFeedforward{ + units::volt_t{wpi::UnpackStruct(data)}, + units::volt_t{wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::ArmFeedforward& value) { + wpi::PackStruct(data, value.kS()); + wpi::PackStruct(data, value.kG()); + wpi::PackStruct(data, value.kV()); + wpi::PackStruct(data, value.kA()); +} diff --git a/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp new file mode 100644 index 0000000000..c056638eaf --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp @@ -0,0 +1,26 @@ +// 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. + +#include "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h" + +namespace { +constexpr size_t kLeftOff = 0; +constexpr size_t kRightOff = kLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveWheelVoltages StructType::Unpack( + std::span data) { + return frc::DifferentialDriveWheelVoltages{ + units::volt_t{wpi::UnpackStruct(data)}, + units::volt_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveWheelVoltages& value) { + wpi::PackStruct(data, value.left.value()); + wpi::PackStruct(data, value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp new file mode 100644 index 0000000000..0572181c7f --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp @@ -0,0 +1,34 @@ +// 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. + +#include "frc/controller/struct/ElevatorFeedforwardStruct.h" + +namespace { +constexpr size_t kKsOff = 0; +constexpr size_t kKgOff = kKsOff + 8; +constexpr size_t kKvOff = kKgOff + 8; +constexpr size_t kKaOff = kKvOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::ElevatorFeedforward StructType::Unpack( + std::span data) { + return frc::ElevatorFeedforward{ + units::volt_t{wpi::UnpackStruct(data)}, + units::volt_t{wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::ElevatorFeedforward& value) { + wpi::PackStruct(data, value.kS()); + wpi::PackStruct(data, value.kG()); + wpi::PackStruct(data, value.kV()); + wpi::PackStruct(data, value.kA()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp new file mode 100644 index 0000000000..097a2fba2e --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/kinematics/proto/ChassisSpeedsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufChassisSpeeds>(arena); +} + +frc::ChassisSpeeds wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::ChassisSpeeds{ + units::meters_per_second_t{m->vx()}, + units::meters_per_second_t{m->vy()}, + units::radians_per_second_t{m->omega()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::ChassisSpeeds& value) { + auto m = static_cast(msg); + m->set_vx(value.vx.value()); + m->set_vy(value.vy.value()); + m->set_omega(value.omega.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp new file mode 100644 index 0000000000..a79e565572 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp @@ -0,0 +1,30 @@ +// 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. + +#include "frc/kinematics/proto/DifferentialDriveKinematicsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveKinematics>(arena); +} + +frc::DifferentialDriveKinematics +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast(&msg); + return frc::DifferentialDriveKinematics{ + units::meter_t{m->track_width()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveKinematics& value) { + auto m = static_cast(msg); + m->set_track_width(value.trackWidth.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp new file mode 100644 index 0000000000..cb38b844c4 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf< + frc::DifferentialDriveWheelSpeeds>::New(google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveWheelSpeeds>(arena); +} + +frc::DifferentialDriveWheelSpeeds +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast( + &msg); + return frc::DifferentialDriveWheelSpeeds{ + units::meters_per_second_t{m->left()}, + units::meters_per_second_t{m->right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveWheelSpeeds& value) { + auto m = static_cast(msg); + m->set_left(value.left.value()); + m->set_right(value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp new file mode 100644 index 0000000000..5b3c027e6e --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/kinematics/proto/MecanumDriveKinematicsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufMecanumDriveKinematics>(arena); +} + +frc::MecanumDriveKinematics wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::MecanumDriveKinematics{ + wpi::UnpackProtobuf(m->front_left()), + wpi::UnpackProtobuf(m->front_right()), + wpi::UnpackProtobuf(m->rear_left()), + wpi::UnpackProtobuf(m->rear_right()), + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::MecanumDriveKinematics& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_front_left(), value.GetFrontLeftWheel()); + wpi::PackProtobuf(m->mutable_front_right(), value.GetFrontRightWheel()); + wpi::PackProtobuf(m->mutable_rear_left(), value.GetRearLeftWheel()); + wpi::PackProtobuf(m->mutable_rear_right(), value.GetRearRightWheel()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp new file mode 100644 index 0000000000..94ca982cfa --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp @@ -0,0 +1,36 @@ +// 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. + +#include "frc/kinematics/proto/MecanumDriveWheelPositionsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufMecanumDriveWheelPositions>(arena); +} + +frc::MecanumDriveWheelPositions +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast(&msg); + return frc::MecanumDriveWheelPositions{ + units::meter_t{m->front_left()}, + units::meter_t{m->front_right()}, + units::meter_t{m->rear_left()}, + units::meter_t{m->rear_right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::MecanumDriveWheelPositions& value) { + auto m = static_cast(msg); + m->set_front_left(value.frontLeft.value()); + m->set_front_right(value.frontRight.value()); + m->set_rear_left(value.rearLeft.value()); + m->set_rear_right(value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp new file mode 100644 index 0000000000..049a088eda --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufMecanumDriveWheelSpeeds>(arena); +} + +frc::MecanumDriveWheelSpeeds +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast(&msg); + return frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t{m->front_left()}, + units::meters_per_second_t{m->front_right()}, + units::meters_per_second_t{m->rear_left()}, + units::meters_per_second_t{m->rear_right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::MecanumDriveWheelSpeeds& value) { + auto m = static_cast(msg); + m->set_front_left(value.frontLeft.value()); + m->set_front_right(value.frontRight.value()); + m->set_rear_left(value.rearLeft.value()); + m->set_rear_right(value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp new file mode 100644 index 0000000000..4e85ec8eb4 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/kinematics/proto/SwerveModulePositionProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufSwerveModulePosition>(arena); +} + +frc::SwerveModulePosition wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::SwerveModulePosition{ + units::meter_t{m->distance()}, + wpi::UnpackProtobuf(m->angle()), + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::SwerveModulePosition& value) { + auto m = static_cast(msg); + m->set_distance(value.distance.value()); + wpi::PackProtobuf(m->mutable_angle(), value.angle); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp new file mode 100644 index 0000000000..f5d5a0fa1d --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/kinematics/proto/SwerveModuleStateProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufSwerveModuleState>(arena); +} + +frc::SwerveModuleState wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::SwerveModuleState{ + units::meters_per_second_t{m->speed()}, + wpi::UnpackProtobuf(m->angle()), + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::SwerveModuleState& value) { + auto m = static_cast(msg); + m->set_speed(value.speed.value()); + wpi::PackProtobuf(m->mutable_angle(), value.angle); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp new file mode 100644 index 0000000000..cb17a0c86d --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp @@ -0,0 +1,28 @@ +// 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. + +#include "frc/kinematics/struct/ChassisSpeedsStruct.h" + +namespace { +constexpr size_t kVxOff = 0; +constexpr size_t kVyOff = kVxOff + 8; +constexpr size_t kOmegaOff = kVyOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::ChassisSpeeds StructType::Unpack(std::span data) { + return frc::ChassisSpeeds{ + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::radians_per_second_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::ChassisSpeeds& value) { + wpi::PackStruct(data, value.vx.value()); + wpi::PackStruct(data, value.vy.value()); + wpi::PackStruct(data, value.omega.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp new file mode 100644 index 0000000000..9ce43860f1 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp @@ -0,0 +1,23 @@ +// 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. + +#include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h" + +namespace { +constexpr size_t kTrackWidthOff = 0; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveKinematics StructType::Unpack( + std::span data) { + return frc::DifferentialDriveKinematics{ + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveKinematics& value) { + wpi::PackStruct(data, value.trackWidth.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp new file mode 100644 index 0000000000..35c157c83d --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp @@ -0,0 +1,26 @@ +// 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. + +#include "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h" + +namespace { +constexpr size_t kLeftOff = 0; +constexpr size_t kRightOff = kLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveWheelSpeeds StructType::Unpack( + std::span data) { + return frc::DifferentialDriveWheelSpeeds{ + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::meters_per_second_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveWheelSpeeds& value) { + wpi::PackStruct(data, value.left.value()); + wpi::PackStruct(data, value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp new file mode 100644 index 0000000000..135618e5ad --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp @@ -0,0 +1,40 @@ +// 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. + +#include "frc/kinematics/struct/MecanumDriveKinematicsStruct.h" + +namespace { +constexpr size_t kFrontLeftOff = 0; +constexpr size_t kFrontRightOff = + kFrontLeftOff + wpi::Struct::kSize; +constexpr size_t kRearLeftOff = + kFrontRightOff + wpi::Struct::kSize; +constexpr size_t kRearRightOff = + kRearLeftOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::MecanumDriveKinematics StructType::Unpack( + std::span data) { + return frc::MecanumDriveKinematics{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::MecanumDriveKinematics& value) { + wpi::PackStruct(data, value.GetFrontLeftWheel()); + wpi::PackStruct(data, value.GetFrontRightWheel()); + wpi::PackStruct(data, value.GetRearLeftWheel()); + wpi::PackStruct(data, value.GetRearRightWheel()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp new file mode 100644 index 0000000000..d75df6e7dc --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h" + +namespace { +constexpr size_t kFrontLeftOff = 0; +constexpr size_t kFrontRightOff = kFrontLeftOff + 8; +constexpr size_t kRearLeftOff = kFrontRightOff + 8; +constexpr size_t kRearRightOff = kRearLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::MecanumDriveWheelPositions StructType::Unpack( + std::span data) { + return frc::MecanumDriveWheelPositions{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::MecanumDriveWheelPositions& value) { + wpi::PackStruct(data, value.frontLeft.value()); + wpi::PackStruct(data, value.frontRight.value()); + wpi::PackStruct(data, value.rearLeft.value()); + wpi::PackStruct(data, value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp new file mode 100644 index 0000000000..ef173f4f64 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h" + +namespace { +constexpr size_t kFrontLeftOff = 0; +constexpr size_t kFrontRightOff = kFrontLeftOff + 8; +constexpr size_t kRearLeftOff = kFrontRightOff + 8; +constexpr size_t kRearRightOff = kRearLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::MecanumDriveWheelSpeeds StructType::Unpack( + std::span data) { + return frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::meters_per_second_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::MecanumDriveWheelSpeeds& value) { + wpi::PackStruct(data, value.frontLeft.value()); + wpi::PackStruct(data, value.frontRight.value()); + wpi::PackStruct(data, value.rearLeft.value()); + wpi::PackStruct(data, value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp new file mode 100644 index 0000000000..56de1ed169 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/kinematics/struct/SwerveModulePositionStruct.h" + +namespace { +constexpr size_t kDistanceOff = 0; +constexpr size_t kAngleOff = kDistanceOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::SwerveModulePosition StructType::Unpack( + std::span data) { + return frc::SwerveModulePosition{ + units::meter_t{wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::SwerveModulePosition& value) { + wpi::PackStruct(data, value.distance.value()); + wpi::PackStruct(data, value.angle); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp new file mode 100644 index 0000000000..396b223f18 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/kinematics/struct/SwerveModuleStateStruct.h" + +namespace { +constexpr size_t kSpeedOff = 0; +constexpr size_t kAngleOff = kSpeedOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::SwerveModuleState StructType::Unpack( + std::span data) { + return frc::SwerveModuleState{ + units::meters_per_second_t{wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::SwerveModuleState& value) { + wpi::PackStruct(data, value.speed.value()); + wpi::PackStruct(data, value.angle); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp b/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp new file mode 100644 index 0000000000..8363270209 --- /dev/null +++ b/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/system/plant/proto/DCMotorProto.h" + +#include "plant.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::DCMotor wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::DCMotor{ + units::volt_t{m->nominal_voltage()}, + units::newton_meter_t{m->stall_torque()}, + units::ampere_t{m->stall_current()}, + units::ampere_t{m->free_current()}, + units::radians_per_second_t{m->free_speed()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::DCMotor& value) { + auto m = static_cast(msg); + m->set_nominal_voltage(value.nominalVoltage.value()); + m->set_stall_torque(value.stallTorque.value()); + m->set_stall_current(value.stallCurrent.value()); + m->set_free_current(value.freeCurrent.value()); + m->set_free_speed(value.freeSpeed.value()); +} diff --git a/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp b/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp new file mode 100644 index 0000000000..91438ca2c1 --- /dev/null +++ b/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/system/plant/struct/DCMotorStruct.h" + +namespace { +constexpr size_t kNominalVoltageOff = 0; +constexpr size_t kStallTorqueOff = kNominalVoltageOff + 8; +constexpr size_t kStallCurrentOff = kStallTorqueOff + 8; +constexpr size_t kFreeCurrentOff = kStallCurrentOff + 8; +constexpr size_t kFreeSpeedOff = kFreeCurrentOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DCMotor StructType::Unpack(std::span data) { + return frc::DCMotor{ + units::volt_t{wpi::UnpackStruct(data)}, + units::newton_meter_t{wpi::UnpackStruct(data)}, + units::ampere_t{wpi::UnpackStruct(data)}, + units::ampere_t{wpi::UnpackStruct(data)}, + units::radians_per_second_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DCMotor& value) { + wpi::PackStruct(data, value.nominalVoltage.value()); + wpi::PackStruct(data, value.stallTorque.value()); + wpi::PackStruct(data, value.stallCurrent.value()); + wpi::PackStruct(data, value.freeCurrent.value()); + wpi::PackStruct(data, value.freeSpeed.value()); +} diff --git a/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp new file mode 100644 index 0000000000..d5bbe2b3b5 --- /dev/null +++ b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/trajectory/proto/TrajectoryProto.h" + +#include "trajectory.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Trajectory wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + std::vector states; + states.reserve(m->states().size()); + for (const auto& protoState : m->states()) { + states.push_back(wpi::UnpackProtobuf(protoState)); + } + return frc::Trajectory{states}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Trajectory& value) { + auto m = static_cast(msg); + m->mutable_states()->Reserve(value.States().size()); + for (const auto& state : value.States()) { + wpi::PackProtobuf(m->add_states(), state); + } +} diff --git a/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp new file mode 100644 index 0000000000..f2b80abe8a --- /dev/null +++ b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/trajectory/proto/TrajectoryStateProto.h" + +#include "trajectory.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTrajectoryState>(arena); +} + +frc::Trajectory::State wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Trajectory::State{ + units::second_t{m->time()}, + units::meters_per_second_t{m->velocity()}, + units::meters_per_second_squared_t{m->acceleration()}, + wpi::UnpackProtobuf(m->pose()), + units::curvature_t{m->curvature()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::Trajectory::State& value) { + auto m = static_cast(msg); + m->set_time(value.t.value()); + m->set_velocity(value.velocity.value()); + m->set_acceleration(value.acceleration.value()); + wpi::PackProtobuf(m->mutable_pose(), value.pose); + m->set_curvature(value.curvature.value()); +} diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 5621803f22..39f4f90e59 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -169,3 +169,6 @@ class WPILIB_DLLEXPORT ArmFeedforward { units::unit_t kA{0}; }; } // namespace frc + +#include "frc/controller/proto/ArmFeedforwardProto.h" +#include "frc/controller/struct/ArmFeedforwardStruct.h" diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h index 48f341e6d6..235a95877e 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h @@ -17,3 +17,6 @@ struct DifferentialDriveWheelVoltages { }; } // namespace frc + +#include "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h" +#include "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h" diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index 62a7bad867..4d308f4ec7 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -176,3 +176,6 @@ class ElevatorFeedforward { units::unit_t kA{0}; }; } // namespace frc + +#include "frc/controller/proto/ElevatorFeedforwardProto.h" +#include "frc/controller/struct/ElevatorFeedforwardStruct.h" diff --git a/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h b/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h new file mode 100644 index 0000000000..bc893aa8cb --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h @@ -0,0 +1,18 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/controller/ArmFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::ArmFeedforward Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::ArmFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h b/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h new file mode 100644 index 0000000000..486fd177d5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveWheelVoltages Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveWheelVoltages& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h b/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h new file mode 100644 index 0000000000..377f62aaa3 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h @@ -0,0 +1,18 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/controller/ElevatorFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::ElevatorFeedforward Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::ElevatorFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h new file mode 100644 index 0000000000..3f427da9d8 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h @@ -0,0 +1,22 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/controller/ArmFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:ArmFeedforward"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double ks;double kg;double kv;double ka"; + + static frc::ArmFeedforward Unpack(std::span data); + static void Pack(std::span data, + const frc::ArmFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h b/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h new file mode 100644 index 0000000000..dae20ed489 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h @@ -0,0 +1,23 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:DifferentialDriveWheelVoltages"; + static constexpr size_t kSize = 16; + static constexpr std::string_view kSchema = "double left;double right"; + + static frc::DifferentialDriveWheelVoltages Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveWheelVoltages& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h new file mode 100644 index 0000000000..89c03f8fea --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h @@ -0,0 +1,22 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/controller/ElevatorFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:ElevatorFeedforward"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double ks;double kg;double kv;double ka"; + + static frc::ElevatorFeedforward Unpack(std::span data); + static void Pack(std::span data, + const frc::ElevatorFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 93c9044dca..ac117b63e3 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -251,3 +251,6 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { } }; } // namespace frc + +#include "frc/kinematics/proto/ChassisSpeedsProto.h" +#include "frc/kinematics/struct/ChassisSpeedsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 95f53fa3b9..3ea6ee92d3 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -91,3 +91,6 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics units::meter_t trackWidth; }; } // namespace frc + +#include "frc/kinematics/proto/DifferentialDriveKinematicsProto.h" +#include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h index 9d9e705772..676b8ad59c 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h @@ -112,3 +112,6 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds { } }; } // namespace frc + +#include "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h" +#include "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index fe6eb5120a..24f450abf7 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -138,6 +138,11 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics */ Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const; + const Translation2d GetFrontLeftWheel() const { return m_frontLeftWheel; } + const Translation2d GetFrontRightWheel() const { return m_frontRightWheel; } + const Translation2d GetRearLeftWheel() const { return m_rearLeftWheel; } + const Translation2d GetRearRightWheel() const { return m_rearRightWheel; } + private: mutable Matrixd<4, 3> m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; @@ -165,3 +170,6 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics }; } // namespace frc + +#include "frc/kinematics/proto/MecanumDriveKinematicsProto.h" +#include "frc/kinematics/struct/MecanumDriveKinematicsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h index 76c8b9dd96..77d79192b9 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h @@ -60,3 +60,6 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions { } }; } // namespace frc + +#include "frc/kinematics/proto/MecanumDriveWheelPositionsProto.h" +#include "frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h index 80e84607ed..5df198c801 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h @@ -120,3 +120,6 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds { } }; } // namespace frc + +#include "frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h" +#include "frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h index 93f7465a0a..576bfcf00f 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h @@ -42,3 +42,6 @@ struct WPILIB_DLLEXPORT SwerveModulePosition { } }; } // namespace frc + +#include "frc/kinematics/proto/SwerveModulePositionProto.h" +#include "frc/kinematics/struct/SwerveModulePositionStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h index 2f95d9b359..3060d35fc2 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h @@ -47,3 +47,6 @@ struct WPILIB_DLLEXPORT SwerveModuleState { const Rotation2d& currentAngle); }; } // namespace frc + +#include "frc/kinematics/proto/SwerveModuleStateProto.h" +#include "frc/kinematics/struct/SwerveModuleStateStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h new file mode 100644 index 0000000000..cefc83f8db --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h @@ -0,0 +1,18 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/ChassisSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::ChassisSpeeds Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::ChassisSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h new file mode 100644 index 0000000000..9108f33c9c --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveKinematics Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h new file mode 100644 index 0000000000..f310892538 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveWheelSpeeds Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h new file mode 100644 index 0000000000..16e828824f --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::MecanumDriveKinematics Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::MecanumDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h new file mode 100644 index 0000000000..a4b4bd6762 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::MecanumDriveWheelPositions Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::MecanumDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h new file mode 100644 index 0000000000..4d099d3de1 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::MecanumDriveWheelSpeeds Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::MecanumDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h new file mode 100644 index 0000000000..3e864cbd95 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h @@ -0,0 +1,18 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/SwerveModulePosition.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::SwerveModulePosition Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::SwerveModulePosition& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h new file mode 100644 index 0000000000..3e7b3ec6e4 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h @@ -0,0 +1,18 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/SwerveModuleState.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::SwerveModuleState Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::SwerveModuleState& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h new file mode 100644 index 0000000000..4e6547bd26 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h @@ -0,0 +1,22 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/ChassisSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:ChassisSpeeds"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = + "double vx;double vy;double omega"; + + static frc::ChassisSpeeds Unpack(std::span data); + static void Pack(std::span data, + const frc::ChassisSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h new file mode 100644 index 0000000000..ab1e2a8964 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h @@ -0,0 +1,23 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:DifferentialDriveKinematics"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "double track_width"; + + static frc::DifferentialDriveKinematics Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h new file mode 100644 index 0000000000..3810160385 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h @@ -0,0 +1,23 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:DifferentialDriveWheelSpeeds"; + static constexpr size_t kSize = 16; + static constexpr std::string_view kSchema = "double left;double right"; + + static frc::DifferentialDriveWheelSpeeds Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h new file mode 100644 index 0000000000..2974c6b1d6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h @@ -0,0 +1,29 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:MecanumDriveKinematics"; + static constexpr size_t kSize = 4 * wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d front_left;Translation2d front_right;Translation2d " + "rear_left;Translation2d rear_right"; + + static frc::MecanumDriveKinematics Unpack( + std::span data); + static void Pack(std::span data, + const frc::MecanumDriveKinematics& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h new file mode 100644 index 0000000000..8fbc3cb316 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h @@ -0,0 +1,24 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:MecanumDriveWheelPositions"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double front_left;double front_right;double rear_left;double rear_right"; + + static frc::MecanumDriveWheelPositions Unpack( + std::span data); + static void Pack(std::span data, + const frc::MecanumDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h new file mode 100644 index 0000000000..f4cfa0987e --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h @@ -0,0 +1,24 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:MecanumDriveWheelSpeeds"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double front_left;double front_right;double rear_left;double rear_right"; + + static frc::MecanumDriveWheelSpeeds Unpack( + std::span data); + static void Pack(std::span data, + const frc::MecanumDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h new file mode 100644 index 0000000000..20b1ff60cc --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h @@ -0,0 +1,26 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/SwerveModulePosition.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:SwerveModulePosition"; + static constexpr size_t kSize = 8 + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "double distance;Rotation2d angle"; + + static frc::SwerveModulePosition Unpack(std::span data); + static void Pack(std::span data, + const frc::SwerveModulePosition& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h new file mode 100644 index 0000000000..fd854cb54d --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h @@ -0,0 +1,25 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/SwerveModuleState.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:SwerveModuleState"; + static constexpr size_t kSize = 8 + wpi::Struct::kSize; + static constexpr std::string_view kSchema = "double speed;Rotation2d angle"; + + static frc::SwerveModuleState Unpack(std::span data); + static void Pack(std::span data, + const frc::SwerveModuleState& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h index ad711ee3cf..de9c3b1b54 100644 --- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h +++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h @@ -239,3 +239,6 @@ class WPILIB_DLLEXPORT DCMotor { }; } // namespace frc + +#include "frc/system/plant/proto/DCMotorProto.h" +#include "frc/system/plant/struct/DCMotorStruct.h" diff --git a/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h b/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h new file mode 100644 index 0000000000..acae1db6d3 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h @@ -0,0 +1,17 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/system/plant/DCMotor.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DCMotor Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::DCMotor& value); +}; diff --git a/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h b/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h new file mode 100644 index 0000000000..c16b65b4d6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h @@ -0,0 +1,22 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/system/plant/DCMotor.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:DCMotor"; + static constexpr size_t kSize = 40; + static constexpr std::string_view kSchema = + "double nominal_voltage;double stall_torque;double stall_current;double " + "free_current;double free_speed"; + + static frc::DCMotor Unpack(std::span data); + static void Pack(std::span data, const frc::DCMotor& value); +}; diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h index ca97593a1a..2ad972ea49 100644 --- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h @@ -145,3 +145,6 @@ WPILIB_DLLEXPORT void from_json(const wpi::json& json, Trajectory::State& state); } // namespace frc + +#include "frc/trajectory/proto/TrajectoryProto.h" +#include "frc/trajectory/proto/TrajectoryStateProto.h" diff --git a/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h new file mode 100644 index 0000000000..4b485b2a92 --- /dev/null +++ b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h @@ -0,0 +1,18 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/trajectory/Trajectory.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Trajectory Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Trajectory& value); +}; diff --git a/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h new file mode 100644 index 0000000000..150837b8d3 --- /dev/null +++ b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h @@ -0,0 +1,18 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/trajectory/Trajectory.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Trajectory::State Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Trajectory::State& value); +}; diff --git a/wpimath/src/main/proto/plant.proto b/wpimath/src/main/proto/plant.proto index d0d9eabc8a..86200d13cf 100644 --- a/wpimath/src/main/proto/plant.proto +++ b/wpimath/src/main/proto/plant.proto @@ -10,7 +10,4 @@ message ProtobufDCMotor { double stall_current = 3; double free_current = 4; double free_speed = 5; - double r = 6; - double kv = 7; - double kt = 8; } diff --git a/wpimath/src/main/proto/trajectory.proto b/wpimath/src/main/proto/trajectory.proto index a37a501d0d..03ece13af3 100644 --- a/wpimath/src/main/proto/trajectory.proto +++ b/wpimath/src/main/proto/trajectory.proto @@ -15,6 +15,5 @@ message ProtobufTrajectoryState { } message ProtobufTrajectory { - double total_time = 1; repeated ProtobufTrajectoryState states = 2; } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java new file mode 100644 index 0000000000..5c036d5e5f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java @@ -0,0 +1,27 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward; +import org.junit.jupiter.api.Test; + +class ArmFeedforwardProtoTest { + private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4); + + @Test + void testRoundtrip() { + ProtobufArmFeedforward proto = ArmFeedforward.proto.createMessage(); + ArmFeedforward.proto.pack(proto, DATA); + + ArmFeedforward data = ArmFeedforward.proto.unpack(proto); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java new file mode 100644 index 0000000000..c3bf58720f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java @@ -0,0 +1,27 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; +import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelVoltagesProtoTest { + private static final DifferentialDriveWheelVoltages DATA = + new DifferentialDriveWheelVoltages(0.174, 0.191); + + @Test + void testRoundtrip() { + ProtobufDifferentialDriveWheelVoltages proto = + DifferentialDriveWheelVoltages.proto.createMessage(); + DifferentialDriveWheelVoltages.proto.pack(proto, DATA); + + DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.proto.unpack(proto); + assertEquals(DATA.left, data.left); + assertEquals(DATA.right, data.right); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java new file mode 100644 index 0000000000..e624afff74 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java @@ -0,0 +1,27 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward; +import org.junit.jupiter.api.Test; + +class ElevatorFeedforwardProtoTest { + private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229); + + @Test + void testRoundtrip() { + ProtobufElevatorFeedforward proto = ElevatorFeedforward.proto.createMessage(); + ElevatorFeedforward.proto.pack(proto, DATA); + + ElevatorFeedforward data = ElevatorFeedforward.proto.unpack(proto); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java new file mode 100644 index 0000000000..42a452dbe7 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java @@ -0,0 +1,30 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ArmFeedforward; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class ArmFeedforwardStructTest { + private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(ArmFeedforward.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + ArmFeedforward.struct.pack(buffer, DATA); + buffer.rewind(); + + ArmFeedforward data = ArmFeedforward.struct.unpack(buffer); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java new file mode 100644 index 0000000000..0c6ea66a5f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java @@ -0,0 +1,29 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelVoltagesStructTest { + private static final DifferentialDriveWheelVoltages DATA = + new DifferentialDriveWheelVoltages(0.174, 0.191); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelVoltages.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveWheelVoltages.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.struct.unpack(buffer); + assertEquals(DATA.left, data.left); + assertEquals(DATA.right, data.right); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java new file mode 100644 index 0000000000..a5a83520de --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java @@ -0,0 +1,30 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class ElevatorFeedforwardStructTest { + private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(ElevatorFeedforward.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + ElevatorFeedforward.struct.pack(buffer, DATA); + buffer.rewind(); + + ElevatorFeedforward data = ElevatorFeedforward.struct.unpack(buffer); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java new file mode 100644 index 0000000000..055e8e7d5a --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java @@ -0,0 +1,26 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufChassisSpeeds; +import org.junit.jupiter.api.Test; + +class ChassisSpeedsProtoTest { + private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504); + + @Test + void testRoundtrip() { + ProtobufChassisSpeeds proto = ChassisSpeeds.proto.createMessage(); + ChassisSpeeds.proto.pack(proto, DATA); + + ChassisSpeeds data = ChassisSpeeds.proto.unpack(proto); + assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond); + assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond); + assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java new file mode 100644 index 0000000000..0d426ad240 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java @@ -0,0 +1,24 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveKinematics; +import org.junit.jupiter.api.Test; + +class DifferentialDriveKinematicsProtoTest { + private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74); + + @Test + void testRoundtrip() { + ProtobufDifferentialDriveKinematics proto = DifferentialDriveKinematics.proto.createMessage(); + DifferentialDriveKinematics.proto.pack(proto, DATA); + + DifferentialDriveKinematics data = DifferentialDriveKinematics.proto.unpack(proto); + assertEquals(DATA.trackWidthMeters, data.trackWidthMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java new file mode 100644 index 0000000000..1f0b237d3f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java @@ -0,0 +1,26 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelSpeeds; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelSpeedsProtoTest { + private static final DifferentialDriveWheelSpeeds DATA = + new DifferentialDriveWheelSpeeds(1.74, 35.04); + + @Test + void testRoundtrip() { + ProtobufDifferentialDriveWheelSpeeds proto = DifferentialDriveWheelSpeeds.proto.createMessage(); + DifferentialDriveWheelSpeeds.proto.pack(proto, DATA); + + DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.proto.unpack(proto); + assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond); + assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java new file mode 100644 index 0000000000..a94cfab933 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java @@ -0,0 +1,33 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveKinematics; +import org.junit.jupiter.api.Test; + +class MecanumDriveKinematicsProtoTest { + private static final MecanumDriveKinematics DATA = + new MecanumDriveKinematics( + new Translation2d(19.1, 2.2), + new Translation2d(35.04, 1.91), + new Translation2d(1.74, 3.504), + new Translation2d(3.504, 1.91)); + + @Test + void testRoundtrip() { + ProtobufMecanumDriveKinematics proto = MecanumDriveKinematics.proto.createMessage(); + MecanumDriveKinematics.proto.pack(proto, DATA); + + MecanumDriveKinematics data = MecanumDriveKinematics.proto.unpack(proto); + assertEquals(DATA.getFrontLeft(), data.getFrontLeft()); + assertEquals(DATA.getFrontRight(), data.getFrontRight()); + assertEquals(DATA.getRearLeft(), data.getRearLeft()); + assertEquals(DATA.getRearRight(), data.getRearRight()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java new file mode 100644 index 0000000000..3e4551d176 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java @@ -0,0 +1,28 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelPositions; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelPositionsProtoTest { + private static final MecanumDriveWheelPositions DATA = + new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74); + + @Test + void testRoundtrip() { + ProtobufMecanumDriveWheelPositions proto = MecanumDriveWheelPositions.proto.createMessage(); + MecanumDriveWheelPositions.proto.pack(proto, DATA); + + MecanumDriveWheelPositions data = MecanumDriveWheelPositions.proto.unpack(proto); + assertEquals(DATA.frontLeftMeters, data.frontLeftMeters); + assertEquals(DATA.frontRightMeters, data.frontRightMeters); + assertEquals(DATA.rearLeftMeters, data.rearLeftMeters); + assertEquals(DATA.rearRightMeters, data.rearRightMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java new file mode 100644 index 0000000000..3e1d589bcc --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java @@ -0,0 +1,28 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelSpeeds; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelSpeedsProtoTest { + private static final MecanumDriveWheelSpeeds DATA = + new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229); + + @Test + void testRoundtrip() { + ProtobufMecanumDriveWheelSpeeds proto = MecanumDriveWheelSpeeds.proto.createMessage(); + MecanumDriveWheelSpeeds.proto.pack(proto, DATA); + + MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.proto.unpack(proto); + assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond); + assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond); + assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond); + assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java new file mode 100644 index 0000000000..459d11958e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java @@ -0,0 +1,27 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModulePosition; +import org.junit.jupiter.api.Test; + +class SwerveModulePositionProtoTest { + private static final SwerveModulePosition DATA = + new SwerveModulePosition(3.504, new Rotation2d(17.4)); + + @Test + void testRoundtrip() { + ProtobufSwerveModulePosition proto = SwerveModulePosition.proto.createMessage(); + SwerveModulePosition.proto.pack(proto, DATA); + + SwerveModulePosition data = SwerveModulePosition.proto.unpack(proto); + assertEquals(DATA.distanceMeters, data.distanceMeters); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java new file mode 100644 index 0000000000..269b7b18b2 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java @@ -0,0 +1,26 @@ +// 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.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleState; +import org.junit.jupiter.api.Test; + +class SwerveModuleStateProtoTest { + private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3)); + + @Test + void testRoundtrip() { + ProtobufSwerveModuleState proto = SwerveModuleState.proto.createMessage(); + SwerveModuleState.proto.pack(proto, DATA); + + SwerveModuleState data = SwerveModuleState.proto.unpack(proto); + assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java new file mode 100644 index 0000000000..16ccdb9dfc --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java @@ -0,0 +1,29 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class ChassisSpeedsStructTest { + private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(ChassisSpeeds.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + ChassisSpeeds.struct.pack(buffer, DATA); + buffer.rewind(); + + ChassisSpeeds data = ChassisSpeeds.struct.unpack(buffer); + assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond); + assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond); + assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java new file mode 100644 index 0000000000..ffcf7e7c1f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java @@ -0,0 +1,27 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveKinematicsStructTest { + private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveKinematics.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveKinematics.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveKinematics data = DifferentialDriveKinematics.struct.unpack(buffer); + assertEquals(DATA.trackWidthMeters, data.trackWidthMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java new file mode 100644 index 0000000000..720f941c06 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java @@ -0,0 +1,29 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelSpeedsStructTest { + private static final DifferentialDriveWheelSpeeds DATA = + new DifferentialDriveWheelSpeeds(1.74, 35.04); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelSpeeds.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveWheelSpeeds.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.struct.unpack(buffer); + assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond); + assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java new file mode 100644 index 0000000000..a8d36f6a20 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java @@ -0,0 +1,36 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class MecanumDriveKinematicsStructTest { + private static final MecanumDriveKinematics DATA = + new MecanumDriveKinematics( + new Translation2d(19.1, 2.2), + new Translation2d(35.04, 1.91), + new Translation2d(1.74, 3.504), + new Translation2d(3.504, 1.91)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveKinematics.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + MecanumDriveKinematics.struct.pack(buffer, DATA); + buffer.rewind(); + + MecanumDriveKinematics data = MecanumDriveKinematics.struct.unpack(buffer); + assertEquals(DATA.getFrontLeft(), data.getFrontLeft()); + assertEquals(DATA.getFrontRight(), data.getFrontRight()); + assertEquals(DATA.getRearLeft(), data.getRearLeft()); + assertEquals(DATA.getRearRight(), data.getRearRight()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java new file mode 100644 index 0000000000..69f8e03401 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java @@ -0,0 +1,31 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelPositionsStructTest { + private static final MecanumDriveWheelPositions DATA = + new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelPositions.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + MecanumDriveWheelPositions.struct.pack(buffer, DATA); + buffer.rewind(); + + MecanumDriveWheelPositions data = MecanumDriveWheelPositions.struct.unpack(buffer); + assertEquals(DATA.frontLeftMeters, data.frontLeftMeters); + assertEquals(DATA.frontRightMeters, data.frontRightMeters); + assertEquals(DATA.rearLeftMeters, data.rearLeftMeters); + assertEquals(DATA.rearRightMeters, data.rearRightMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java new file mode 100644 index 0000000000..b962acd80a --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java @@ -0,0 +1,31 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelSpeedsStructTest { + private static final MecanumDriveWheelSpeeds DATA = + new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelSpeeds.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + MecanumDriveWheelSpeeds.struct.pack(buffer, DATA); + buffer.rewind(); + + MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.struct.unpack(buffer); + assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond); + assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond); + assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond); + assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java new file mode 100644 index 0000000000..819d98170c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java @@ -0,0 +1,30 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class SwerveModulePositionStructTest { + private static final SwerveModulePosition DATA = + new SwerveModulePosition(3.504, new Rotation2d(17.4)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(SwerveModulePosition.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + SwerveModulePosition.struct.pack(buffer, DATA); + buffer.rewind(); + + SwerveModulePosition data = SwerveModulePosition.struct.unpack(buffer); + assertEquals(DATA.distanceMeters, data.distanceMeters); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java new file mode 100644 index 0000000000..625514c04b --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java @@ -0,0 +1,29 @@ +// 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.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class SwerveModuleStateStructTest { + private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(SwerveModuleState.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + SwerveModuleState.struct.pack(buffer, DATA); + buffer.rewind(); + + SwerveModuleState data = SwerveModuleState.struct.unpack(buffer); + assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java new file mode 100644 index 0000000000..3a6e64a0db --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java @@ -0,0 +1,31 @@ +// 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.system.plant.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.proto.Plant.ProtobufDCMotor; +import edu.wpi.first.math.system.plant.DCMotor; +import org.junit.jupiter.api.Test; + +class DCMotorProtoTest { + private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); + + @Test + void testRoundtrip() { + ProtobufDCMotor proto = DCMotor.proto.createMessage(); + DCMotor.proto.pack(proto, DATA); + + DCMotor data = DCMotor.proto.unpack(proto); + assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); + assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); + assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); + assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); + assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); + assertEquals(DATA.rOhms, data.rOhms); + assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); + assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java new file mode 100644 index 0000000000..806254dc23 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java @@ -0,0 +1,34 @@ +// 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.system.plant.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.system.plant.DCMotor; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DCMotorStructTest { + private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DCMotor.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DCMotor.struct.pack(buffer, DATA); + buffer.rewind(); + + DCMotor data = DCMotor.struct.unpack(buffer); + assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); + assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); + assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); + assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); + assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); + assertEquals(DATA.rOhms, data.rOhms); + assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); + assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java new file mode 100644 index 0000000000..0477bb7dcf --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java @@ -0,0 +1,40 @@ +// 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.trajectory.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectory; +import edu.wpi.first.math.trajectory.Trajectory; +import java.util.List; +import org.junit.jupiter.api.Test; + +class TrajectoryProtoTest { + private static final Trajectory DATA = + new Trajectory( + List.of( + new Trajectory.State( + 1.1, 2.2, 3.3, new Pose2d(new Translation2d(1.1, 2.2), new Rotation2d(2.2)), 6.6), + new Trajectory.State( + 2.1, 2.2, 3.3, new Pose2d(new Translation2d(2.1, 2.2), new Rotation2d(2.2)), 6.6), + new Trajectory.State( + 3.1, + 2.2, + 3.3, + new Pose2d(new Translation2d(3.1, 2.2), new Rotation2d(2.2)), + 6.6))); + + @Test + void testRoundtrip() { + ProtobufTrajectory proto = Trajectory.proto.createMessage(); + Trajectory.proto.pack(proto, DATA); + + Trajectory data = Trajectory.proto.unpack(proto); + assertEquals(DATA.getStates(), data.getStates()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java new file mode 100644 index 0000000000..8a9fdae5ac --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java @@ -0,0 +1,33 @@ +// 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.trajectory.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState; +import edu.wpi.first.math.trajectory.Trajectory; +import org.junit.jupiter.api.Test; + +class TrajectoryStateProtoTest { + private static final Trajectory.State DATA = + new Trajectory.State( + 1.91, 4.4, 17.4, new Pose2d(new Translation2d(1.74, 19.1), new Rotation2d(22.9)), 0.174); + + @Test + void testRoundtrip() { + ProtobufTrajectoryState proto = Trajectory.State.proto.createMessage(); + Trajectory.State.proto.pack(proto, DATA); + + Trajectory.State data = Trajectory.State.proto.unpack(proto); + assertEquals(DATA.timeSeconds, data.timeSeconds); + assertEquals(DATA.velocityMetersPerSecond, data.velocityMetersPerSecond); + assertEquals(DATA.accelerationMetersPerSecondSq, data.accelerationMetersPerSecondSq); + assertEquals(DATA.poseMeters, data.poseMeters); + assertEquals(DATA.curvatureRadPerMeter, data.curvatureRadPerMeter); + } +} diff --git a/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp new file mode 100644 index 0000000000..b0e57c78d2 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp @@ -0,0 +1,33 @@ +// 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. + +#include + +#include "controller.pb.h" +#include "frc/controller/ArmFeedforward.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_rad; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad; +const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ArmFeedforwardProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + ArmFeedforward unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp new file mode 100644 index 0000000000..1adad90fd7 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "controller.pb.h" +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DifferentialDriveWheelVoltages kExpectedData = + DifferentialDriveWheelVoltages{0.174_V, 0.191_V}; +} // namespace + +TEST(DifferentialDriveWheelVoltagesProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DifferentialDriveWheelVoltages unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp new file mode 100644 index 0000000000..115a3dcf8b --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "controller.pb.h" +#include "frc/controller/ElevatorFeedforward.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_m; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m; + +constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ElevatorFeedforwardProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + ElevatorFeedforward unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp new file mode 100644 index 0000000000..453061c56e --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp @@ -0,0 +1,33 @@ +// 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. + +#include + +#include "frc/controller/ArmFeedforward.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_rad; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad; +const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ArmFeedforwardStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + ArmFeedforward unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp new file mode 100644 index 0000000000..2b47e5a538 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveWheelVoltages kExpectedData{ + DifferentialDriveWheelVoltages{0.174_V, 0.191_V}}; +} // namespace + +TEST(DifferentialDriveWheelVoltagesStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveWheelVoltages unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp new file mode 100644 index 0000000000..299d39a34b --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "frc/controller/ElevatorFeedforward.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_m; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m; + +constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ElevatorFeedforwardStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + ElevatorFeedforward unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp new file mode 100644 index 0000000000..375909a330 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/kinematics/ChassisSpeeds.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const ChassisSpeeds kExpectedData = + ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}; +} // namespace + +TEST(ChassisSpeedsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + ChassisSpeeds unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value()); + EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value()); + EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp new file mode 100644 index 0000000000..4d57108833 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DifferentialDriveKinematics kExpectedData = + DifferentialDriveKinematics{1.74_m}; +} // namespace + +TEST(DifferentialDriveKinematicsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DifferentialDriveKinematics unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp new file mode 100644 index 0000000000..83e317f206 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DifferentialDriveWheelSpeeds kExpectedData = + DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}; +} // namespace + +TEST(DifferentialDriveWheelSpeedsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DifferentialDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp new file mode 100644 index 0000000000..7051ca1a0c --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const MecanumDriveKinematics kExpectedData = MecanumDriveKinematics{ + Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m}, + Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}}; +} // namespace + +TEST(MecanumDriveKinematicsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + MecanumDriveKinematics unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.GetFrontLeftWheel(), + unpacked_data.GetFrontLeftWheel()); + EXPECT_EQ(kExpectedData.GetFrontRightWheel(), + unpacked_data.GetFrontRightWheel()); + EXPECT_EQ(kExpectedData.GetRearLeftWheel(), unpacked_data.GetRearLeftWheel()); + EXPECT_EQ(kExpectedData.GetRearRightWheel(), + unpacked_data.GetRearRightWheel()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp new file mode 100644 index 0000000000..e63ffbeadd --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp @@ -0,0 +1,30 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const MecanumDriveWheelPositions kExpectedData = + MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m}; +} // namespace + +TEST(MecanumDriveWheelPositionsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + MecanumDriveWheelPositions unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp new file mode 100644 index 0000000000..ae91c3a1eb --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp @@ -0,0 +1,30 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const MecanumDriveWheelSpeeds kExpectedData = + MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}; +} // namespace + +TEST(MecanumDriveWheelSpeedsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + MecanumDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp new file mode 100644 index 0000000000..c511b9f840 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModulePosition.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const SwerveModulePosition kExpectedData = + SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}}; +} // namespace + +TEST(SwerveModulePositionProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + SwerveModulePosition unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp new file mode 100644 index 0000000000..e210db9479 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModuleState.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const SwerveModuleState kExpectedData = + SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}; +} // namespace + +TEST(SwerveModuleStateProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + SwerveModuleState unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp new file mode 100644 index 0000000000..6729aa0b9d --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/ChassisSpeeds.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const ChassisSpeeds kExpectedData{ + ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}}; +} // namespace + +TEST(ChassisSpeedsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + ChassisSpeeds unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value()); + EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value()); + EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp new file mode 100644 index 0000000000..ecf10ad5f7 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp @@ -0,0 +1,26 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveKinematics kExpectedData{ + DifferentialDriveKinematics{1.74_m}}; +} // namespace + +TEST(DifferentialDriveKinematicsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveKinematics unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp new file mode 100644 index 0000000000..33499f5d4e --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveWheelSpeeds kExpectedData{ + DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}}; +} // namespace + +TEST(DifferentialDriveWheelSpeedsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp new file mode 100644 index 0000000000..8fe0817e27 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp @@ -0,0 +1,33 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const MecanumDriveKinematics kExpectedData{MecanumDriveKinematics{ + Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m}, + Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}}}; +} // namespace + +TEST(MecanumDriveKinematicsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + MecanumDriveKinematics unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.GetFrontLeftWheel(), + unpacked_data.GetFrontLeftWheel()); + EXPECT_EQ(kExpectedData.GetFrontRightWheel(), + unpacked_data.GetFrontRightWheel()); + EXPECT_EQ(kExpectedData.GetRearLeftWheel(), unpacked_data.GetRearLeftWheel()); + EXPECT_EQ(kExpectedData.GetRearRightWheel(), + unpacked_data.GetRearRightWheel()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp new file mode 100644 index 0000000000..39190a776f --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const MecanumDriveWheelPositions kExpectedData{ + MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m}}; +} // namespace + +TEST(MecanumDriveWheelPositionsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + MecanumDriveWheelPositions unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp new file mode 100644 index 0000000000..bbde80cdde --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const MecanumDriveWheelSpeeds kExpectedData{ + MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}}; +} // namespace + +TEST(MecanumDriveWheelSpeedsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + MecanumDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp new file mode 100644 index 0000000000..a942efe662 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModulePosition.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const SwerveModulePosition kExpectedData{ + SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}}}; +} // namespace + +TEST(SwerveModulePositionStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + SwerveModulePosition unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp new file mode 100644 index 0000000000..40ca305b8e --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModuleState.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const SwerveModuleState kExpectedData{ + SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}}; +} // namespace + +TEST(SwerveModuleStateStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + SwerveModuleState unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp b/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp new file mode 100644 index 0000000000..b664befd76 --- /dev/null +++ b/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp @@ -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. + +#include + +#include "frc/system/plant/DCMotor.h" +#include "plant.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DCMotor kExpectedData = DCMotor{units::volt_t{1.91}, + units::newton_meter_t{19.1}, + units::ampere_t{1.74}, + units::ampere_t{2.29}, + units::radians_per_second_t{2.2}, + 2}; +} // namespace + +TEST(DCMotorProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DCMotor unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.nominalVoltage.value(), + unpacked_data.nominalVoltage.value()); + EXPECT_EQ(kExpectedData.stallTorque.value(), + unpacked_data.stallTorque.value()); + EXPECT_EQ(kExpectedData.stallCurrent.value(), + unpacked_data.stallCurrent.value()); + EXPECT_EQ(kExpectedData.freeCurrent.value(), + unpacked_data.freeCurrent.value()); + EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value()); + EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value()); + EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value()); + EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value()); +} diff --git a/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp b/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp new file mode 100644 index 0000000000..30306d2d5f --- /dev/null +++ b/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp @@ -0,0 +1,41 @@ +// 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. + +#include + +#include "frc/system/plant/DCMotor.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DCMotor kExpectedData = DCMotor{units::volt_t{1.91}, + units::newton_meter_t{19.1}, + units::ampere_t{1.74}, + units::ampere_t{2.29}, + units::radians_per_second_t{2.2}, + 2}; +} // namespace + +TEST(DCMotorStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DCMotor unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.nominalVoltage.value(), + unpacked_data.nominalVoltage.value()); + EXPECT_EQ(kExpectedData.stallTorque.value(), + unpacked_data.stallTorque.value()); + EXPECT_EQ(kExpectedData.stallCurrent.value(), + unpacked_data.stallCurrent.value()); + EXPECT_EQ(kExpectedData.freeCurrent.value(), + unpacked_data.freeCurrent.value()); + EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value()); + EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value()); + EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value()); + EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value()); +} diff --git a/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp new file mode 100644 index 0000000000..400689bf84 --- /dev/null +++ b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp @@ -0,0 +1,35 @@ +// 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. + +#include + +#include "frc/trajectory/Trajectory.h" +#include "trajectory.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Trajectory kExpectedData = Trajectory{std::vector{ + Trajectory::State{1.1_s, 2.2_mps, 3.3_mps_sq, + Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)), + units::curvature_t{6.6}}, + Trajectory::State{2.1_s, 2.2_mps, 3.3_mps_sq, + Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)), + units::curvature_t{6.6}}, + Trajectory::State{3.1_s, 2.2_mps, 3.3_mps_sq, + Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)), + units::curvature_t{6.6}}}}; +} // namespace + +TEST(TrajectoryProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Trajectory unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.States(), unpacked_data.States()); +} diff --git a/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp new file mode 100644 index 0000000000..1d0b0497de --- /dev/null +++ b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "frc/trajectory/Trajectory.h" +#include "trajectory.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Trajectory::State kExpectedData = Trajectory::State{ + 1.91_s, 4.4_mps, 17.4_mps_sq, + Pose2d{Translation2d{1.74_m, 19.1_m}, Rotation2d{22.9_rad}}, + units::curvature_t{0.174}}; +} // namespace + +TEST(TrajectoryStateProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Trajectory::State unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.t.value(), unpacked_data.t.value()); + EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data.velocity.value()); + EXPECT_EQ(kExpectedData.acceleration.value(), + unpacked_data.acceleration.value()); + EXPECT_EQ(kExpectedData.pose, unpacked_data.pose); + EXPECT_EQ(kExpectedData.curvature.value(), unpacked_data.curvature.value()); +}