From f7c01d41c5e4f1aec86e4e872470031a059c8bb9 Mon Sep 17 00:00:00 2001
From: Patrick Goebel <patrick@pirobot.org>
Date: Wed, 27 Jan 2016 06:26:10 -0800
Subject: [PATCH] Added support for Arduino Motor Shield R3

---
 .../libraries/ROSArduinoBridge/motor_driver.h |  1 +
 .../ROSArduinoBridge/motor_driver.ino         | 62 ++++++++++++++++++-
 2 files changed, 62 insertions(+), 1 deletion(-)

diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
index 0e3248c..2c91e25 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
@@ -5,3 +5,4 @@
 void initMotorController();
 void setMotorSpeed(int i, int spd);
 void setMotorSpeeds(int leftSpeed, int rightSpeed);
+
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino
index 27c76bc..e59606f 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino
@@ -55,8 +55,68 @@
     setMotorSpeed(LEFT, leftSpeed);
     setMotorSpeed(RIGHT, rightSpeed);
   }
+#elif defined ARDUINO_MOTOR_SHIELD_R3
+  /* Include the pin definitions for the shield */
+  #include "arduino_motor_shield_r3.h"
+  
+  /* Wrap the motor driver initialization */
+  void initMotorController() {
+    // Set up Channel A - LEFT
+    pinMode(LEFT_MOTOR_PIN_DIR, OUTPUT); // Initiates Motor Channel A pin
+    pinMode(LEFT_MOTOR_PIN_BRAKE, OUTPUT); // Initiates Brake Channel A pin
+
+    // Set up Channel B - RIGHT
+    pinMode(RIGHT_MOTOR_PIN_DIR, OUTPUT); // Initiates Motor Channel B pin
+    pinMode(RIGHT_MOTOR_PIN_BRAKE, OUTPUT);  // Initiates Brake Channel B pin
+  }
+
+  /* Wrap the drive motor set speed function */
+  void setMotorSpeed(int i, int spd) {
+    if (i == LEFT) {
+      if (spd < 0) digitalWrite(LEFT_MOTOR_PIN_DIR, LOW);
+      else digitalWrite(LEFT_MOTOR_PIN_DIR, HIGH);
+
+      if (spd == 0) digitalWrite(LEFT_MOTOR_PIN_BRAKE, HIGH);
+      else digitalWrite(LEFT_MOTOR_PIN_BRAKE, LOW);
+
+      analogWrite(LEFT_MOTOR_PIN_SPEED, spd);
+    }
+    else {
+      if (spd < 0) digitalWrite(RIGHT_MOTOR_PIN_DIR, LOW);
+      else digitalWrite(RIGHT_MOTOR_PIN_DIR, HIGH);
+      
+      if (spd == 0) digitalWrite(RIGHT_MOTOR_PIN_BRAKE, HIGH);
+      else digitalWrite(RIGHT_MOTOR_PIN_BRAKE, LOW);
+      
+      analogWrite(RIGHT_MOTOR_PIN_SPEED, spd);
+    }
+  }
+
+  /* A convenience function for setting both motor speeds */
+  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
+    setMotorSpeed(LEFT, leftSpeed);
+    setMotorSpeed(RIGHT, rightSpeed);
+  }
+
+/* For testing only! */
+#elif defined(NO_MOTOR_CONTROLLER)
+  /* Wrap the motor driver initialization */
+  void initMotorController() { }
+  
+  /* A convenience function for setting both motor speeds */
+  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
+    setMotorSpeed(LEFT, leftSpeed);
+    setMotorSpeed(RIGHT, rightSpeed);
+  }
+
+  /* Wrap the drive motor set speed function */
+  void setMotorSpeed(int i, int spd) {
+    if (i == LEFT) {}
+    else {}
+  }
 #else
-  #error A motor driver must be selected!
+  //#error A motor driver must be selected!
 #endif
 
 #endif
+