Add toString() to relevant kinematics classes (#2160)

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);
+  }
 }

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);
+  }
 }

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);
+  }
 }

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);
+  }
 }

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);
+  }
 }