From 64a7e57fe0585cc3648f2528ec26953fca415c34 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Thu, 1 Feb 2018 21:17:04 -0800
Subject: [PATCH] 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  | 9 +++++++++
 .../edu/wpi/first/wpilibj/drive/DifferentialDrive.java   | 7 +++++++
 2 files changed, 16 insertions(+)

diff --git a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp
index 4f8802dfd6..b8cab7ed83 100644
--- a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp
+++ b/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);
 
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
index 74ff519c4e..79b011f69c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -261,6 +261,13 @@ public class DifferentialDrive extends RobotDriveBase {
       }
     }
 
+    // Normalize the wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput));
+    if (maxMagnitude > 1.0) {
+      leftMotorOutput /= maxMagnitude;
+      rightMotorOutput /= maxMagnitude;
+    }
+
     m_leftMotor.set(leftMotorOutput * m_maxOutput);
     m_rightMotor.set(-rightMotorOutput * m_maxOutput);