Migration to TB6612 motor driver

src/Motor.cpp

@@ -5,31 +5,34 @@
 /**
  * Setup the Motor
  */
-Motor::Motor(uint8_t iPin1, uint8_t iPin2, bool iReversed = false, int iMin = 0, int iMax = 1023)
+Motor::Motor(uint8_t in1, uint8_t in2, uint8_t pwm, uint8_t offset, uint8_t standBy, int iMin = 0, int iMax = 255)
 {
-    pin1 = iPin1;
-    pin2 = iPin2;
-    reversed = iReversed;
-    min = iMin;
-    max = iMax;
-    pinMode(pin1, OUTPUT);
-    pinMode(pin2, OUTPUT);
+  motor = TB6612(in1, in2, pwm, offset, standBy);
+  min = iMin;
+  max = iMax;
 }
 
 void Motor::forward(int speed)
 {
-  analogWrite(pin1, map(speed, min, max, 0, 255));
-  digitalWrite(pin2, LOW);
+  motor.drive((map(speed, min, max, 0, 255)));
+}
+
+void Motor::forward(int speed, int duration)
+{
+  motor.drive((map(speed, min, max, 0, 255)), duration);
 }
 
 void Motor::backward(int speed)
 {
-  digitalWrite(pin1, LOW);
-  analogWrite(pin2, map(speed, min, max, 0, 255));
+  motor.drive(-(map(speed, min, max, 0, 255)));
+}
+
+void Motor::backward(int speed, int duration)
+{
+  motor.drive(-(map(speed, min, max, 0, 255)), duration);
 }
 
 void Motor::stop()
 {
-  digitalWrite(pin1, LOW);
-  digitalWrite(pin2, LOW);
+  motor.brake();
 }

src/Motor.h

@@ -1,15 +1,18 @@
 #include <Arduino.h>
 
+#include "TB6612.h"
+
 class Motor
 {
     public:
-        Motor(uint8_t iPin1, uint8_t iPin2, bool iReversed = false, int iMin = 0, int iMax = 1023);
+        Motor(uint8_t in1, uint8_t in2, uint8_t pwm, uint8_t offset, uint8_t standBy, int iMin = 0, int iMax = 255);
         void forward(int speed);
+        void forward(int speed, int duration);
         void backward(int speed);
+        void backward(int speed, int duration);
         void stop();
-        uint8_t pin1;
-        uint8_t pin2;
         int min = 0;
-        int max = 1023;
-        bool reversed = false;
+        int max = 255;
+        TB6612 motor = TB6612(0, 0, 0, 0, 0);
+        // bool reversed = false;
 };

src/TB6612.cpp

@@ -0,0 +1,57 @@
+#include "TB6612.h"
+#include <Arduino.h>
+
+TB6612::TB6612(int In1pin, int In2pin, int PWMpin, int offset, int STBYpin)
+{
+    In1 = In1pin;
+    In2 = In2pin;
+    PWM = PWMpin;
+    Standby = STBYpin;
+    Offset = offset;
+
+    pinMode(In1, OUTPUT);
+    pinMode(In2, OUTPUT);
+    pinMode(PWM, OUTPUT);
+    pinMode(Standby, OUTPUT);
+}
+
+void TB6612::drive(int speed)
+{
+    digitalWrite(Standby, HIGH);
+    speed = speed * Offset;
+    if (speed >= 0)
+        fwd(speed);
+    else
+        rev(-speed);
+}
+void TB6612::drive(int speed, int duration)
+{
+    drive(speed);
+    delay(duration);
+}
+
+void TB6612::fwd(int speed)
+{
+    digitalWrite(In1, HIGH);
+    digitalWrite(In2, LOW);
+    analogWrite(PWM, speed);
+}
+
+void TB6612::rev(int speed)
+{
+    digitalWrite(In1, LOW);
+    digitalWrite(In2, HIGH);
+    analogWrite(PWM, speed);
+}
+
+void TB6612::brake()
+{
+    digitalWrite(In1, HIGH);
+    digitalWrite(In2, HIGH);
+    analogWrite(PWM, 0);
+}
+
+void TB6612::standby()
+{
+    digitalWrite(Standby, LOW);
+}

src/TB6612.h

@@ -0,0 +1,17 @@
+#include <Arduino.h>
+
+#define DEFAULTSPEED 255
+
+class TB6612
+{
+    public:
+        TB6612(int In1pin, int In2pin, int PWMpin, int offset, int STBYpin);
+        void drive(int speed);
+        void drive(int speed, int duration);
+        void brake();
+        void standby();
+    private:
+        int In1, In2, PWM, Offset, Standby;
+        void fwd(int speed);
+        void rev(int speed);
+};