From 023c088290b3a35c0cdd5ef8ada4fc5dac90816c Mon Sep 17 00:00:00 2001
From: Prateek Machiraju <prateek.machiraju@gmail.com>
Date: Tue, 10 Dec 2019 00:35:43 -0500
Subject: [PATCH] Add toString() to relevant kinematics classes (#2160)

---
 .../edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java    | 6 ++++++
 .../wpilibj/kinematics/DifferentialDriveWheelSpeeds.java   | 6 ++++++
 .../wpilibj/kinematics/MecanumDriveMotorVoltages.java      | 7 +++++++
 .../first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java  | 7 +++++++
 .../wpi/first/wpilibj/kinematics/SwerveModuleState.java    | 6 ++++++
 5 files changed, 32 insertions(+)

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
index e16f0a0d64..aa98a477c0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
@@ -82,4 +82,10 @@ public class ChassisSpeeds {
         omegaRadiansPerSecond
     );
   }
+
+  @Override
+  public String toString() {
+    return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
+        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+  }
 }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
index 32c5ade93c..ac4a9fa0d9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
@@ -59,4 +59,10 @@ public class DifferentialDriveWheelSpeeds {
           * attainableMaxSpeedMetersPerSecond;
     }
   }
+
+  @Override
+  public String toString() {
+    return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
+        leftMetersPerSecond, rightMetersPerSecond);
+  }
 }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
index af4a7f1d8d..f467d2033a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
@@ -55,4 +55,11 @@ public class MecanumDriveMotorVoltages {
     this.rearLeftVoltage = rearLeftVoltage;
     this.rearRightVoltage = rearRightVoltage;
   }
+
+  @Override
+  public String toString() {
+    return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+            + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
+        rearLeftVoltage, rearRightVoltage);
+  }
 }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
index 54ae521098..0f0dd7113b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
@@ -81,4 +81,11 @@ public class MecanumDriveWheelSpeeds {
           * attainableMaxSpeedMetersPerSecond;
     }
   }
+
+  @Override
+  public String toString() {
+    return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
+        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
+  }
 }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
index 528776f5cc..e3119e5fd0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
@@ -54,4 +54,10 @@ public class SwerveModuleState implements Comparable<SwerveModuleState> {
   public int compareTo(SwerveModuleState o) {
     return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
   }
+
+  @Override
+  public String toString() {
+    return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
+        angle);
+  }
 }