From 0d2947803b42799a6bc72fe579b38d1e09f86c4b Mon Sep 17 00:00:00 2001
From: Derek Lindahl <derek.lindahl@gmail.com>
Date: Mon, 18 Jul 2016 22:15:33 -0700
Subject: [PATCH] Add motor control translation

This takes the differential steering values and maps them to motor commands
---
 lib/motor_control.js | 44 ++++++++++++++++++++++++++++++++++++++++++++
 motor_control.js     |  1 +
 2 files changed, 45 insertions(+)
 create mode 100644 lib/motor_control.js
 create mode 100644 motor_control.js

diff --git a/lib/motor_control.js b/lib/motor_control.js
new file mode 100644
index 0000000..58135c9
--- /dev/null
+++ b/lib/motor_control.js
@@ -0,0 +1,44 @@
+var diffSteer = require('./diff_steer');
+
+function explicate(motorSpeed) {
+  leftMotorDirection = motorSpeed > 0 ? motorControl.revMethod : motorControl.fwdMethod;
+  leftMotorSpeed = Math.abs(motorSpeed);
+  return [leftMotorDirection, leftMotorSpeed];
+}
+
+function motorControl(wheels, leftRightAxis, upDownAxis, maxAxis, minAxis, maxSpeed, axisFlip) {
+  var leftMotor;
+  var leftMotorDirection;
+  var leftMotorSpeed = 0;
+  var motorSpeeds;
+  var rightMotor;
+  var rightMotorDirection;
+  var rightMotorSpeed = 0;
+
+  // Map differential steering values to motor commands
+  motorSpeeds = diffSteer(leftRightAxis, upDownAxis, maxAxis, minAxis, maxSpeed, axisFlip);
+  leftMotor = explicate(motorSpeeds[0]);
+  leftMotorDirection = leftMotor[0];
+  leftMotorSpeed = leftMotor[1];
+  rightMotor = explicate(motorSpeeds[1]);
+  rightMotorDirection = rightMotor[0];
+  rightMotorSpeed = rightMotor[1];
+
+  wheels[0][leftMotorDirection](leftMotorSpeed);
+  wheels[1][rightMotorDirection](rightMotorSpeed);
+
+  return [
+    {
+      direction: leftMotorDirection,
+      speed: leftMotorSpeed
+    },
+    {
+      direction: rightMotorDirection,
+      speed: rightMotorSpeed
+    }
+  ];
+}
+motorControl.fwdMethod = 'fwd';
+motorControl.revMethod = 'rev';
+
+module.exports = motorControl;
diff --git a/motor_control.js b/motor_control.js
new file mode 100644
index 0000000..feb2f28
--- /dev/null
+++ b/motor_control.js
@@ -0,0 +1 @@
+module.exports = require('./lib/motor_control');