From 8523223ca629a2e9648f3874b071e2922e11f60f Mon Sep 17 00:00:00 2001
From: Anas I Gouda <anas.ibrahim006@gmail.com>
Date: Sat, 19 Aug 2017 04:00:43 +0200
Subject: [PATCH] added L298 Motor Driver

---
 .../ROSArduinoBridge/ROSArduinoBridge.ino     |  3 ++
 .../libraries/ROSArduinoBridge/motor_driver.h |  9 ++++++
 .../ROSArduinoBridge/motor_driver.ino         | 31 +++++++++++++++++++
 3 files changed, 43 insertions(+)

diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
index 9eb8e21..3348af5 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
@@ -61,6 +61,9 @@
    
    /* Encoders directly attached to Arduino board */
    //#define ARDUINO_ENC_COUNTER
+
+   /* L298 Motor driver*/
+   //#define L298_MOTOR_DRIVER
 #endif
 
 #define USE_SERVOS  // Enable use of PWM servos as defined in servos.h
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
index 0e3248c..9bf3f16 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
@@ -2,6 +2,15 @@
    Motor driver function definitions - by James Nugen
    *************************************************************/
 
+#ifdef L298_MOTOR_DRIVER
+  #define RIGHT_MOTOR_BACKWARD 5
+  #define LEFT_MOTOR_BACKWARD  6
+  #define RIGHT_MOTOR_FORWARD  9
+  #define LEFT_MOTOR_FORWARD   10
+  #define RIGHT_MOTOR_ENABLE 12
+  #define LEFT_MOTOR_ENABLE 13
+#endif
+
 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..facb8a8 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino
@@ -51,6 +51,37 @@
   }
 
   // A convenience function for setting both motor speeds
+  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
+    setMotorSpeed(LEFT, leftSpeed);
+    setMotorSpeed(RIGHT, rightSpeed);
+  }
+#elif defined L298_MOTOR_DRIVER
+  void initMotorController() {
+    digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
+    digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
+  }
+  
+  void setMotorSpeed(int i, int spd) {
+    unsigned char reverse = 0;
+  
+    if (spd < 0)
+    {
+      spd = -spd;
+      reverse = 1;
+    }
+    if (spd > 255)
+      spd = 255;
+    
+    if (i == LEFT) { 
+      if      (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
+      else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
+    }
+    else /*if (i == RIGHT) //no need for condition*/ {
+      if      (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
+      else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
+    }
+  }
+  
   void setMotorSpeeds(int leftSpeed, int rightSpeed) {
     setMotorSpeed(LEFT, leftSpeed);
     setMotorSpeed(RIGHT, rightSpeed);