From 4dae747343e5d817ae4d4b75fb435572508d9c09 Mon Sep 17 00:00:00 2001
From: Austin Shalit <austinshalit@gmail.com>
Date: Sun, 7 May 2017 09:40:48 -0700
Subject: [PATCH] Refactor RobotDrive squaring (#390)

---
 wpilibc/athena/include/RobotDrive.h           |  2 +-
 wpilibc/athena/src/RobotDrive.cpp             | 41 ++++++-------------
 .../edu/wpi/first/wpilibj/RobotDrive.java     | 39 ++++++------------
 3 files changed, 27 insertions(+), 55 deletions(-)

diff --git a/wpilibc/athena/include/RobotDrive.h b/wpilibc/athena/include/RobotDrive.h
index 8e23e625ea..219ef9e1e2 100644
--- a/wpilibc/athena/include/RobotDrive.h
+++ b/wpilibc/athena/include/RobotDrive.h
@@ -98,7 +98,7 @@ class RobotDrive : public MotorSafety, public ErrorBase {
 
  protected:
   void InitRobotDrive();
-  double Limit(double num);
+  double Limit(double number);
   void Normalize(double* wheelSpeeds);
   void RotateVector(double& x, double& y, double angle);
 
diff --git a/wpilibc/athena/src/RobotDrive.cpp b/wpilibc/athena/src/RobotDrive.cpp
index 00d50ec8d5..e5c06d1238 100644
--- a/wpilibc/athena/src/RobotDrive.cpp
+++ b/wpilibc/athena/src/RobotDrive.cpp
@@ -312,21 +312,14 @@ void RobotDrive::TankDrive(double leftValue, double rightValue,
     reported = true;
   }
 
-  // square the inputs (while preserving the sign) to increase fine control
-  // while permitting full power
   leftValue = Limit(leftValue);
   rightValue = Limit(rightValue);
+
+  // square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power
   if (squaredInputs) {
-    if (leftValue >= 0.0) {
-      leftValue = (leftValue * leftValue);
-    } else {
-      leftValue = -(leftValue * leftValue);
-    }
-    if (rightValue >= 0.0) {
-      rightValue = (rightValue * rightValue);
-    } else {
-      rightValue = -(rightValue * rightValue);
-    }
+    leftValue = std::copysign(leftValue * leftValue, leftValue);
+    rightValue = std::copysign(rightValue * rightValue, rightValue);
   }
 
   SetLeftRightMotorOutputs(leftValue, rightValue);
@@ -443,19 +436,11 @@ void RobotDrive::ArcadeDrive(double moveValue, double rotateValue,
   moveValue = Limit(moveValue);
   rotateValue = Limit(rotateValue);
 
+  // square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power
   if (squaredInputs) {
-    // square the inputs (while preserving the sign) to increase fine control
-    // while permitting full power
-    if (moveValue >= 0.0) {
-      moveValue = (moveValue * moveValue);
-    } else {
-      moveValue = -(moveValue * moveValue);
-    }
-    if (rotateValue >= 0.0) {
-      rotateValue = (rotateValue * rotateValue);
-    } else {
-      rotateValue = -(rotateValue * rotateValue);
-    }
+    moveValue = std::copysign(moveValue * moveValue, moveValue);
+    rotateValue = std::copysign(rotateValue * rotateValue, rotateValue);
   }
 
   if (moveValue > 0.0) {
@@ -625,14 +610,14 @@ void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
 /**
  * Limit motor values to the -1.0 to +1.0 range.
  */
-double RobotDrive::Limit(double num) {
-  if (num > 1.0) {
+double RobotDrive::Limit(double number) {
+  if (number > 1.0) {
     return 1.0;
   }
-  if (num < -1.0) {
+  if (number < -1.0) {
     return -1.0;
   }
-  return num;
+  return number;
 }
 
 /**
diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java
index 51b55a3294..9f24da1310 100644
--- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java
+++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java
@@ -276,21 +276,14 @@ public class RobotDrive implements MotorSafety {
       kTank_Reported = true;
     }
 
-    // square the inputs (while preserving the sign) to increase fine control
-    // while permitting full power
     leftValue = limit(leftValue);
     rightValue = limit(rightValue);
+
+    // square the inputs (while preserving the sign) to increase fine control
+    // while permitting full power
     if (squaredInputs) {
-      if (leftValue >= 0.0) {
-        leftValue = leftValue * leftValue;
-      } else {
-        leftValue = -(leftValue * leftValue);
-      }
-      if (rightValue >= 0.0) {
-        rightValue = rightValue * rightValue;
-      } else {
-        rightValue = -(rightValue * rightValue);
-      }
+      leftValue = Math.copySign(leftValue * leftValue, leftValue);
+      rightValue = Math.copySign(rightValue * rightValue, rightValue);
     }
     setLeftRightMotorOutputs(leftValue, rightValue);
   }
@@ -391,19 +384,13 @@ public class RobotDrive implements MotorSafety {
     moveValue = limit(moveValue);
     rotateValue = limit(rotateValue);
 
+    // square the inputs (while preserving the sign) to increase fine control
+    // while permitting full power
     if (squaredInputs) {
       // square the inputs (while preserving the sign) to increase fine control
       // while permitting full power
-      if (moveValue >= 0.0) {
-        moveValue = moveValue * moveValue;
-      } else {
-        moveValue = -(moveValue * moveValue);
-      }
-      if (rotateValue >= 0.0) {
-        rotateValue = rotateValue * rotateValue;
-      } else {
-        rotateValue = -(rotateValue * rotateValue);
-      }
+      moveValue = Math.copySign(moveValue * moveValue, moveValue);
+      rotateValue = Math.copySign(rotateValue * rotateValue, rotateValue);
     }
 
     if (moveValue > 0.0) {
@@ -580,14 +567,14 @@ public class RobotDrive implements MotorSafety {
   /**
    * Limit motor values to the -1.0 to +1.0 range.
    */
-  protected static double limit(double num) {
-    if (num > 1.0) {
+  protected static double limit(double number) {
+    if (number > 1.0) {
       return 1.0;
     }
-    if (num < -1.0) {
+    if (number < -1.0) {
       return -1.0;
     }
-    return num;
+    return number;
   }
 
   /**