diff --git a/src/wpimath/Geometry/Pose2d.cs b/src/wpimath/Geometry/Pose2d.cs index f96217be..d08c9cec 100644 --- a/src/wpimath/Geometry/Pose2d.cs +++ b/src/wpimath/Geometry/Pose2d.cs @@ -102,7 +102,11 @@ public Pose2d(Length x, Length y, Rotation2d rotation) public Pose2d TransformBy(Transform2d other) { - return new Pose2d(Translation + other.Translation.RotateBy(Rotation), other.Rotation + Rotation); + Translation2d rotated = other.Translation.RotateBy(Rotation); + Translation2d newTranslation = Translation + rotated; + Rotation2d newRotation = other.Rotation + Rotation; + Pose2d newPose = new Pose2d(newTranslation, newRotation); + return newPose; } public Pose2d RotateBy(Rotation2d other) @@ -240,4 +244,6 @@ public override int GetHashCode() { return HashCode.Combine(Translation, Rotation); } + + public override string ToString() => $"Pose2d({Translation}, {Rotation})"; } diff --git a/src/wpimath/Geometry/Rotation2d.cs b/src/wpimath/Geometry/Rotation2d.cs index 0935a33d..102a4924 100644 --- a/src/wpimath/Geometry/Rotation2d.cs +++ b/src/wpimath/Geometry/Rotation2d.cs @@ -69,6 +69,11 @@ public partial class Rotation2dJsonContext : JsonSerializerContext public static IStruct Struct { get; } = new Rotation2dStruct(); public static IProtobuf Proto { get; } = new Rotation2dProto(); + public Rotation2d() : this(0.Radians()) + { + + } + public Rotation2d(Angle angle) { Angle = angle; @@ -100,11 +105,6 @@ internal Rotation2d(double radians) : this(radians.Radians()) [JsonPropertyName("radians")] internal double Radians => Angle.Radians; - public Rotation2d() : this(0.Radians()) - { - - } - [JsonIgnore] public Angle Angle { get; } [JsonIgnore] @@ -181,4 +181,5 @@ public Rotation2d Interpolate(Rotation2d endValue, double t) return MathExtras.Lerp(this, endValue, t); } + public override string ToString() => $"Rotation2d(Angle: {Angle})"; } diff --git a/src/wpimath/Geometry/Translation2d.cs b/src/wpimath/Geometry/Translation2d.cs index 7bde38fc..6aee02cf 100644 --- a/src/wpimath/Geometry/Translation2d.cs +++ b/src/wpimath/Geometry/Translation2d.cs @@ -183,4 +183,6 @@ public override int GetHashCode() { return HashCode.Combine(X, Y); } + + public override string ToString() => $"Translation2d(X: {X}, Y: {Y})"; } diff --git a/src/wpimath/Kinematics/ChassisSpeeds.cs b/src/wpimath/Kinematics/ChassisSpeeds.cs new file mode 100644 index 00000000..8350d7c2 --- /dev/null +++ b/src/wpimath/Kinematics/ChassisSpeeds.cs @@ -0,0 +1,263 @@ +using System.Numerics; +using Google.Protobuf.Reflection; +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToRotationalSpeed; +using UnitsNet.NumberExtensions.NumberToSpeed; +using WPIMath.Geometry; +using WPIMath.Proto; +using WPIUtil.Serialization.Protobuf; +using WPIUtil.Serialization.Struct; + +namespace WPIMath; + +public readonly struct ChassisSpeeds : IAdditionOperators, + ISubtractionOperators, + IMultiplyOperators, + IDivisionOperators, + IUnaryNegationOperators, + IEquatable +{ + /// + /// Chassis velocity in the X-axis (Forward is positive). + /// + public Speed Vx { get; } + + /// + /// Chassis velocity in the Y-axis (Left is positive). + /// + public Speed Vy { get; } + + /// + /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). + /// + public RotationalSpeed Omega { get; } + + public static IProtobuf Proto { get; } = new ChassisSpeedsProto(); + public static IStruct Struct { get; } = new ChassisSpeedsStruct(); + + /// + /// Constructs a new ChassisSpeeds object with zero velocity on all axes. + /// + public ChassisSpeeds() { } + + /// + /// Constructs a new ChassisSpeeds object with the given velocities. + /// + /// Chassis velocity in the X-axis (Forward is positive). + /// Chassis velocity in the Y-axis (Left is positive). + /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive). + public ChassisSpeeds(Speed vx, Speed vy, RotationalSpeed omega) + { + Vx = vx; + Vy = vy; + Omega = omega; + } + + /// + /// Constructs a new ChassisSpeeds object with the given velocities. + /// + /// Chassis velocity in the X-axis (Forward is positive) in meters per second. + /// Chassis velocity in the Y-axis (Left is positive) in meters per second. + /// Chassis Angular velocity (Z-axis or theta, Counter-Clockwise is positive) in radians per second. + public ChassisSpeeds(double vx, double vy, double omega) + { + Vx = vx.MetersPerSecond(); + Vy = vy.MetersPerSecond(); + Omega = omega.RadiansPerSecond(); + } + + /// + /// Discretizes a continuous-time chassis speed. + /// + /// + /// This function converts a continuous-time chassis speed into a discrete-time one such that + /// when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + /// velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + /// along the y-axis, and omega * dt around the z-axis). + /// + /// + /// This is useful for compensating for translational skew when translating and rotating a + /// swerve drivetrain. + /// + /// + /// Forward velocity. + /// Sideways velocity (left is positive). + /// Angular velocity (couter-clockwise is positive). + /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). + /// Discretized ChassisSpeeds. + public static ChassisSpeeds Discretize(Speed vx, Speed vy, RotationalSpeed omega, TimeSpan dt) + { + Pose2d desiredDeltaPose = new Pose2d(vx * dt, vy * dt, new Rotation2d(omega * dt)); + var twist = new Pose2d().Log(desiredDeltaPose); + return new(twist.Dx / dt, twist.Dy / dt, twist.Dtheta / dt); + } + + /// + /// Discretizes a continuous-time chassis speed. + /// + /// + /// This function converts a continuous-time chassis speed into a discrete-time one such that + /// when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + /// velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + /// along the y-axis, and omega * dt around the z-axis). + /// + /// + /// This is useful for compensating for translational skew when translating and rotating a + /// swerve drivetrain. + /// + /// + /// ChassisSpeeds to discretize. + /// Timestep the speed should be applied for (this should be the same as the robot loop duration for most users). + /// Discretized ChassisSpeeds. + public static ChassisSpeeds Discretize(ChassisSpeeds continuousSpeeds, TimeSpan dt) => + Discretize(continuousSpeeds.Vx, continuousSpeeds.Vy, continuousSpeeds.Omega, dt); + + /// + /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object. + /// + /// The component of speed in the x direction relative to the field. Positive x is away + /// from your alliance wall. + /// The component of speed in the y direction relative to the field. Positive y is to + /// your left when standing behind the alliance wall. + /// The angular velocity of the robot. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the robot's frame of reference. + public static ChassisSpeeds FromFieldRelativeSpeeds(Speed vx, Speed vy, RotationalSpeed omega, Rotation2d robotAngle) + { + var oneSecond = TimeSpan.FromSeconds(1); + // clockwise rotation into robot frame, so negate the angle + var chassisFrameVelocity = new Translation2d(vx * oneSecond, vy * oneSecond).RotateBy(-robotAngle); + return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, omega); + } + + /// + /// Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object. + /// + /// Field-relative ChassisSpeeds. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the robot's frame of reference. + public static ChassisSpeeds FromFieldRelativeSpeeds(ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) => + FromFieldRelativeSpeeds(fieldRelativeSpeeds.Vx, fieldRelativeSpeeds.Vy, fieldRelativeSpeeds.Omega, robotAngle); + + /// + /// Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds object. + /// + /// The component of speed in the x direction relative to the robot. Positive x is towards the + /// robot's front. + /// The component of speed in the y direction relative to the robot. Positive y is towards the + /// robot's left. + /// The angular velocity of the robot. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the field's frame of reference. + public static ChassisSpeeds FromRobotRelativeSpeeds(Speed vx, Speed vy, RotationalSpeed omega, Rotation2d robotAngle) + { + var oneSecond = TimeSpan.FromSeconds(1); + // counter-clockwise rotation out of robot frame + var chassisFrameVelocity = new Translation2d(vx * oneSecond, vy * oneSecond).RotateBy(robotAngle); + return new(chassisFrameVelocity.X / oneSecond, chassisFrameVelocity.Y / oneSecond, omega); + } + + /// + /// Converts a user provided robot-relative ChassisSpeeds object into a field-relative ChassisSpeeds object. + /// + /// The ChassisSpeeds object representing the speeds in the robot frame + /// of reference. Positive x is towards the robot's front. Positive y is towards the robot's left. + /// The angle of the robot as measured by a gyroscope. The robot's angle is + /// considered to be zero when it is facing directly away from your alliance station wall. + /// Remember that this should be CCW positive. + /// ChassisSpeeds object representing the speeds in the field's frame of reference. + public static ChassisSpeeds FromRobotRelativeSpeeds(ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) => + FromRobotRelativeSpeeds(robotRelativeSpeeds.Vx, robotRelativeSpeeds.Vy, robotRelativeSpeeds.Omega, robotAngle); + + public static ChassisSpeeds operator +(ChassisSpeeds left, ChassisSpeeds right) => + new(left.Vx + right.Vx, left.Vy + right.Vy, left.Omega + right.Omega); + + public static ChassisSpeeds operator -(ChassisSpeeds left, ChassisSpeeds right) => + new(left.Vx - right.Vx, left.Vy - right.Vy, left.Omega - right.Omega); + + public static ChassisSpeeds operator -(ChassisSpeeds value) => + new(-value.Vx, -value.Vy, -value.Omega); + + public static ChassisSpeeds operator *(ChassisSpeeds speed, double scalar) => + new(speed.Vx * scalar, speed.Vy * scalar, speed.Omega * scalar); + + public static ChassisSpeeds operator /(ChassisSpeeds speeds, double scalar) => + new(speeds.Vx / scalar, speeds.Vy / scalar, speeds.Omega / scalar); + + public override string ToString() => + $"ChassisSpeeds(Vx: {Vx:F2} {Vx.Unit}, Vy: {Vy:F2} {Vy.Unit}, Omega: {Omega:F2} {Omega.Unit})"; + + public bool Equals(ChassisSpeeds other) + { + return + Math.Abs(Vx.MetersPerSecond - other.Vx.MetersPerSecond) < 1E-9 && + Math.Abs(Vy.MetersPerSecond - other.Vy.MetersPerSecond) < 1E-9 && + Math.Abs(Omega.RadiansPerSecond - other.Omega.RadiansPerSecond) < 1E-9; + } + + public override bool Equals(object? obj) + { + return obj is ChassisSpeeds speeds && Equals(speeds); + } + + public static bool operator ==(ChassisSpeeds left, ChassisSpeeds right) + { + return left.Equals(right); + } + + public static bool operator !=(ChassisSpeeds left, ChassisSpeeds right) + { + return !(left == right); + } + + public override int GetHashCode() => HashCode.Combine(Vx, Vy, Omega); +} + +public class ChassisSpeedsProto : IProtobuf +{ + public MessageDescriptor Descriptor => ProtobufChassisSpeeds.Descriptor; + + public ProtobufChassisSpeeds CreateMessage() => new(); + + public void Pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) + { + msg.Vx = value.Vx.MetersPerSecond; + msg.Vy = value.Vy.MetersPerSecond; + msg.Omega = value.Omega.RadiansPerSecond; + } + + public ChassisSpeeds Unpack(ProtobufChassisSpeeds msg) + { + return new(Speed.FromMetersPerSecond(msg.Vx), Speed.FromMetersPerSecond(msg.Vy), RotationalSpeed.FromRadiansPerSecond(msg.Omega)); + } +} + +public class ChassisSpeedsStruct : IStruct +{ + public string TypeString => "struct:ChassisSpeeds"; + + public int Size => sizeof(double) * 3; + + public string Schema => "double vx;double vy;double omega"; + + public void Pack(ref StructPacker buffer, ChassisSpeeds value) + { + buffer.WriteDouble(value.Vx.MetersPerSecond); + buffer.WriteDouble(value.Vy.MetersPerSecond); + buffer.WriteDouble(value.Omega.RadiansPerSecond); + } + + public ChassisSpeeds Unpack(ref StructUnpacker buffer) + { + var vx = Speed.FromMetersPerSecond(buffer.ReadDouble()); + var vy = Speed.FromMetersPerSecond(buffer.ReadDouble()); + var omega = RotationalSpeed.FromRadiansPerSecond(buffer.ReadDouble()); + return new(vx, vy, omega); + } +} diff --git a/test/wpimath.test/Geometry/Pose2dTest.cs b/test/wpimath.test/Geometry/Pose2dTest.cs new file mode 100644 index 00000000..dfbe88e2 --- /dev/null +++ b/test/wpimath.test/Geometry/Pose2dTest.cs @@ -0,0 +1,22 @@ +using UnitsNet.NumberExtensions.NumberToAngle; +using UnitsNet.NumberExtensions.NumberToLength; +using WPIMath.Geometry; +using Xunit; + +namespace WPIMath.Test.Geometry +{ + public class Pose2dTest + { + [Fact] + public void TestPose2dTransformBy() + { + Pose2d start = new(); + + Transform2d transform = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Pose2d end = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Assert.Equal(end, start.TransformBy(transform)); + } + } +} diff --git a/test/wpimath.test/Geometry/Rotation2dTest.cs b/test/wpimath.test/Geometry/Rotation2dTest.cs index ecd8cbff..b59ca665 100644 --- a/test/wpimath.test/Geometry/Rotation2dTest.cs +++ b/test/wpimath.test/Geometry/Rotation2dTest.cs @@ -66,6 +66,15 @@ public void TestMinus() Assert.Equal(40.0, (rot1 - rot2).Angle.Degrees, Epsilon); } + [Fact] + public void TestPlus() + { + Rotation2d rot1 = 70.Degrees(); + Rotation2d rot2 = 30.Degrees(); + + Assert.Equal(100.0, (rot1 + rot2).Angle.Degrees, Epsilon); + } + [Fact] public void TestUrnaryMinus() { diff --git a/test/wpimath.test/Geometry/Translation2dTest.cs b/test/wpimath.test/Geometry/Translation2dTest.cs new file mode 100644 index 00000000..963a80a2 --- /dev/null +++ b/test/wpimath.test/Geometry/Translation2dTest.cs @@ -0,0 +1,30 @@ +using UnitsNet.NumberExtensions.NumberToAngle; +using UnitsNet.NumberExtensions.NumberToLength; +using WPIMath.Geometry; +using Xunit; + +namespace WPIMath.Test.Geometry +{ + public class Translation2dTest + { + [Fact] + public void TestTranslation2dRotateBy() + { + Translation2d start = new(5.Meters(), 0.Meters()); + Rotation2d rotation = new(90.Degrees()); + var end = start.RotateBy(rotation); + + Assert.Equal(new Translation2d(0.Meters(), 5.Meters()), end); + } + + [Fact] + public void TestTranslation2dAddition() + { + Translation2d start = new(5.Meters(), 0.Meters()); + Translation2d offset = new(2.Meters(), 3.Meters()); + Translation2d end = new(7.Meters(), 3.Meters()); + + Assert.Equal(end, start + offset); + } + } +} diff --git a/test/wpimath.test/Geometry/Twist2dTest.cs b/test/wpimath.test/Geometry/Twist2dTest.cs new file mode 100644 index 00000000..7f046b4b --- /dev/null +++ b/test/wpimath.test/Geometry/Twist2dTest.cs @@ -0,0 +1,27 @@ +using UnitsNet.NumberExtensions.NumberToAngle; +using UnitsNet.NumberExtensions.NumberToLength; +using WPIMath.Geometry; +using Xunit; + +namespace WPIMath.Test.Geometry +{ + public class Twist2dTest + { + [Fact] + public void TestPose2dLog() + { + Pose2d start = new(); + Pose2d end = new(new Translation2d(5.Meters(), 5.Meters()), new Rotation2d(90.Degrees())); + + Twist2d twist = start.Log(end); + + Twist2d expected = new((5.0 / 2.0 * Math.PI).Meters(), 0.Meters(), (Math.PI / 2.0).Radians()); + + Assert.Equal(expected, twist); + + // ensure the twist gives us the real end pose + Pose2d applied = start.Exp(twist); + Assert.Equal(end, applied); + } + } +} diff --git a/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs b/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs new file mode 100644 index 00000000..7993a114 --- /dev/null +++ b/test/wpimath.test/Kinematics/ChassisSpeedsTest.cs @@ -0,0 +1,117 @@ +using UnitsNet; +using UnitsNet.NumberExtensions.NumberToAngle; +using UnitsNet.NumberExtensions.NumberToSpeed; +using WPIMath.Geometry; +using Xunit; + +namespace WPIMath.Test.Kinematics; + +public class ChassisSpeedsTest +{ + + private static readonly double Epsilon = 1e-9; + + [Fact] + public void TestAddition() + { + // Given + ChassisSpeeds first = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + ChassisSpeeds second = new(4.MetersPerSecond(), 5.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(6)); + // When + ChassisSpeeds sum = first + second; + // Then + Assert.Equal(5.0, sum.Vx.MetersPerSecond, Epsilon); + Assert.Equal(7.0, sum.Vy.MetersPerSecond, Epsilon); + Assert.Equal(9.0, sum.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestMinus() + { + // Given + ChassisSpeeds first = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + ChassisSpeeds second = new(4.MetersPerSecond(), 5.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(6)); + // When + ChassisSpeeds difference = first - second; + // Then + Assert.Equal(-3.0, difference.Vx.MetersPerSecond, Epsilon); + Assert.Equal(-3.0, difference.Vy.MetersPerSecond, Epsilon); + Assert.Equal(-3.0, difference.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestMultiply() + { + // Given + ChassisSpeeds speed = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + double scalar = 3.0; + // When + ChassisSpeeds product = speed * scalar; + // Then + Assert.Equal(3.0, product.Vx.MetersPerSecond, Epsilon); + Assert.Equal(6.0, product.Vy.MetersPerSecond, Epsilon); + Assert.Equal(9.0, product.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestDivide() + { + // Given + ChassisSpeeds speed = new(1.MetersPerSecond(), 2.MetersPerSecond(), RotationalSpeed.FromRadiansPerSecond(3)); + double scalar = 5.0; + // When + ChassisSpeeds quotient = speed / scalar; + // Then + Assert.Equal(0.2, quotient.Vx.MetersPerSecond, Epsilon); + Assert.Equal(0.4, quotient.Vy.MetersPerSecond, Epsilon); + Assert.Equal(0.6, quotient.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestFromFieldRelative() + { + + ChassisSpeeds fieldSpeeds = new(1.0, 0.0, 0.5); + ChassisSpeeds robotSpeeds = ChassisSpeeds.FromFieldRelativeSpeeds(fieldSpeeds, new Rotation2d(-90.Degrees())); + + Assert.Equal(0.0, robotSpeeds.Vx.MetersPerSecond, Epsilon); + Assert.Equal(1.0, robotSpeeds.Vy.MetersPerSecond, Epsilon); + Assert.Equal(0.5, robotSpeeds.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestFromRobotRelative() + { + ChassisSpeeds robotSpeeds = new(1.0, 0.0, 0.5); + ChassisSpeeds fieldSpeeds = ChassisSpeeds.FromRobotRelativeSpeeds(robotSpeeds, new Rotation2d(45.Degrees())); + + Assert.Equal(1.0 / Math.Sqrt(2.0), fieldSpeeds.Vx.MetersPerSecond, Epsilon); + Assert.Equal(1.0 / Math.Sqrt(2.0), fieldSpeeds.Vy.MetersPerSecond, Epsilon); + Assert.Equal(0.5, fieldSpeeds.Omega.RadiansPerSecond, Epsilon); + } + + [Fact] + public void TestDiscretize() + { + ChassisSpeeds target = new(1, 0.0, 0.5); + TimeSpan duration = TimeSpan.FromSeconds(1); + TimeSpan dt = TimeSpan.FromSeconds(0.01); + + ChassisSpeeds speeds = ChassisSpeeds.Discretize(target, duration); + Twist2d twist = new( + speeds.Vx * dt, + speeds.Vy * dt, + speeds.Omega * dt + ); + + Pose2d pose = new(); + for (double time = 0; time < duration.TotalSeconds; time += dt.TotalSeconds) + { + pose = pose.Exp(twist); + } + + Assert.Equal((target.Vx * duration).Meters, pose.X.Meters, Epsilon); + Assert.Equal((target.Vy * duration).Meters, pose.Y.Meters, Epsilon); + Assert.Equal((target.Omega * duration).Radians, pose.Rotation.Angle.Radians, Epsilon); + } +} diff --git a/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs b/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs new file mode 100644 index 00000000..bc6f9fac --- /dev/null +++ b/test/wpimath.test/Kinematics/Proto/ChassisSpeedProtoTest.cs @@ -0,0 +1,23 @@ +using UnitsNet; +using WPIMath.Proto; +using Xunit; + +namespace WPIMath.Test.Kinematics.Proto +{ + public class ChassisSpeedsProtoTest + { + readonly ChassisSpeeds data = new(Speed.FromMetersPerSecond(1.0), Speed.FromMetersPerSecond(2.0), RotationalSpeed.FromRadiansPerSecond(3.0)); + + [Fact] + public void TestRoundtrip() + { + ProtobufChassisSpeeds proto = ChassisSpeeds.Proto.CreateMessage(); + ChassisSpeeds.Proto.Pack(proto, data); + ChassisSpeeds unpacked = ChassisSpeeds.Proto.Unpack(proto); + + Assert.Equal(data.Vx, unpacked.Vx); + Assert.Equal(data.Vy, unpacked.Vy); + Assert.Equal(data.Omega, unpacked.Omega); + } + } +} diff --git a/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs b/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs new file mode 100644 index 00000000..25871d86 --- /dev/null +++ b/test/wpimath.test/Kinematics/Struct/ChassisSpeedsStructTest.cs @@ -0,0 +1,23 @@ +using UnitsNet; +using WPIUtil.Serialization.Struct; +using Xunit; + +namespace WPIMath.Test.Kinematics.Struct +{ + public class ChassisSpeedsStructTest + { + readonly ChassisSpeeds data = new(Speed.FromMetersPerSecond(1.0), Speed.FromMetersPerSecond(2.0), RotationalSpeed.FromRadiansPerSecond(3.0)); + + [Fact] + public void TestRoundtrip() + { + StructPacker packer = new(new byte[ChassisSpeeds.Struct.Size]); + ChassisSpeeds.Struct.Pack(ref packer, data); + + StructUnpacker unpacker = new StructUnpacker(packer.Filled); + ChassisSpeeds unpackedData = ChassisSpeeds.Struct.Unpack(ref unpacker); + + Assert.Equal(data, unpackedData); + } + } +} diff --git a/test/wpimath.test/wpimath.test.csproj b/test/wpimath.test/wpimath.test.csproj index 4309094e..6c4078aa 100644 --- a/test/wpimath.test/wpimath.test.csproj +++ b/test/wpimath.test/wpimath.test.csproj @@ -5,7 +5,4 @@ - - -