Added output normalization to DifferentialDrive::CurvatureDrive() (#924)

This normalizes within -1..1 to avoid clipping and maintain the ratio between
wheel speeds, since that ratio determines the center of rotation.

Fixes #923.

wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp

@@ -7,6 +7,7 @@
 
 #include "Drive/DifferentialDrive.h"
 
+#include <algorithm>
 #include <cmath>
 
 #include <HAL/HAL.h>
@@ -171,6 +172,14 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
     }
   }
 
+  // Normalize the wheel speeds
+  double maxMagnitude =
+      std::max(std::abs(leftMotorOutput), std::abs(rightMotorOutput));
+  if (maxMagnitude > 1.0) {
+    leftMotorOutput /= maxMagnitude;
+    rightMotorOutput /= maxMagnitude;
+  }
+
   m_leftMotor.Set(leftMotorOutput * m_maxOutput);
   m_rightMotor.Set(-rightMotorOutput * m_maxOutput);