From 7a5b84ef18b90bcafdc117472418651ad80e26e8 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sat, 31 Oct 2020 11:26:23 +0100
Subject: [PATCH] handle motorboards with invereted brake pin (like pololu)

---
 MotorDriver.cpp | 25 +++++++++++++++++++++----
 MotorDriver.h   |  5 +++--
 MotorDrivers.h  | 10 +++++++---
 3 files changed, 31 insertions(+), 9 deletions(-)

diff --git a/MotorDriver.cpp b/MotorDriver.cpp
index 5420488..c80048e 100644
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -34,7 +34,7 @@
     #define ReadPin digitalRead2
 #endif
     
-MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, byte brake_pin, 
+MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin,
                          byte current_pin, float sense_factor, unsigned int trip_milliamps, byte fault_pin) {
    powerPin=power_pin;
    signalPin=signal_pin;
@@ -46,7 +46,8 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, byte
    tripMilliamps=trip_milliamps;
    rawCurrentTripValue=(int)(trip_milliamps / sense_factor);
   pinMode(powerPin, OUTPUT);
-  pinMode(brakePin, OUTPUT);
+  pinMode(brakePin < 0 ? -brakePin : brakePin, OUTPUT);
+  setBrake(false);
   pinMode(signalPin, OUTPUT);
   if (signalPin2 != UNUSED_PIN) pinMode(signalPin2, OUTPUT);
   pinMode(currentPin, INPUT);
@@ -56,8 +57,24 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, byte
 void MotorDriver::setPower(bool on) {
   WritePin(powerPin, on ? HIGH : LOW);
 }
-void MotorDriver::setBrake( bool on) {
-  WritePin(brakePin, on ? HIGH : LOW);
+
+// setBrake applies brake if on == true. So to get
+// voltage from the motor bride one needs to do a
+// setBrake(false).
+// If the brakePin is negative that means the sense
+// of the brake pin on the motor bridge is inverted
+// (HIGH == release brake) and setBrake does
+// compensate for that.
+//
+void MotorDriver::setBrake(bool on) {
+    bool state = on;
+    byte pin = brakePin;
+    if (brakePin < 0) {
+      pin=-pin;
+      state=!state;
+    }
+    WritePin(pin, state ? HIGH : LOW);
+    //DIAG(F("BrakePin: %d is %d\n"), pin, ReadPin(pin));
 }
 
 void MotorDriver::setSignal( bool high) {
diff --git a/MotorDriver.h b/MotorDriver.h
index cbc09cc..2655605 100644
--- a/MotorDriver.h
+++ b/MotorDriver.h
@@ -22,7 +22,7 @@
 
 class MotorDriver {
   public:
-    MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, byte brake_pin, byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin);
+    MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin, byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin);
     virtual void setPower( bool on);
     virtual void setSignal( bool high);
     virtual void setBrake( bool on);
@@ -34,7 +34,8 @@ class MotorDriver {
     }
 
   private:
-    byte powerPin, signalPin, signalPin2, brakePin,currentPin,faultPin;
+    byte powerPin, signalPin, signalPin2, currentPin, faultPin;
+    int8_t brakePin;
     float senseFactor;
     unsigned int tripMilliamps;
     int rawCurrentTripValue;
diff --git a/MotorDrivers.h b/MotorDrivers.h
index 3695e0a..809d69b 100644
--- a/MotorDrivers.h
+++ b/MotorDrivers.h
@@ -15,8 +15,12 @@
 
 const byte UNUSED_PIN = 255;
 
-// MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, byte brake_pin, byte current_pin,
+// MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin, byte current_pin,
 //             float senseFactor, unsigned int tripMilliamps, byte faultPin);
+//
+// If the brakePin is negative that means the sense
+// of the brake pin on the motor bridge is inverted
+// (HIGH == release brake)
 
 // Arduino standard Motor Shield
 #define STANDARD_MOTOR_SHIELD F("STANDARD_MOTOR_SHIELD"),                                                 \
@@ -25,8 +29,8 @@ const byte UNUSED_PIN = 255;
 
 // Pololu Motor Shield
 #define POLOLU_MOTOR_SHIELD F("POLOLU_MOTOR_SHIELD"),                               \
-                            new MotorDriver(4, 7, UNUSED_PIN, 9, A0, 18, 3000, 12), \
-                            new MotorDriver(2, 8, UNUSED_PIN, 10, A1, 18, 3000, UNUSED_PIN)
+                            new MotorDriver(4, 7, UNUSED_PIN, -9, A0, 18, 3000, 12), \
+                            new MotorDriver(2, 8, UNUSED_PIN, -10, A1, 18, 3000, UNUSED_PIN)
 
 // Firebox Mk1
 #define FIREBOX_MK1 F("FIREBOX_MK1"),                                                  \