From 13757c8c57205eac7c924b0dccf1d96e391dfacc Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Sun, 31 Jan 2021 13:43:35 +0000
Subject: [PATCH] DIO2 replacement

Currently for writing signal pins during waveform.
---
 MotorDriver.cpp | 47 +++++++++++++++++++++++++++++------------------
 MotorDriver.h   |  9 +++++++++
 2 files changed, 38 insertions(+), 18 deletions(-)

diff --git a/MotorDriver.cpp b/MotorDriver.cpp
index c4ef893..aef9f6d 100644
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -21,24 +21,21 @@
 #include "AnalogReadFast.h"
 #include "DIAG.h"
 
-
-
-#if defined(ARDUINO_ARCH_SAMD) || defined(ARDUINO_ARCH_SAMC) || defined(ARDUINO_ARCH_MEGAAVR)
-    #define WritePin digitalWrite
-    #define ReadPin digitalRead
-#else
-    // use the DIO2 libraray for much faster pin access
-    #define GPIO2_PREFER_SPEED 1
-    #include <DIO2.h>  // use IDE menu Tools..Manage Libraries to locate and  install DIO2
-    #define WritePin digitalWrite2
-    #define ReadPin digitalRead2
-#endif
+#define setHIGH(fastpin)  *fastpin.out |= fastpin.maskHIGH
+#define setLOW(fastpin)   *fastpin.out &= fastpin.maskLOW
     
 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;
+  getFastPin(signalPin,fastSignalPin);
   signalPin2=signal_pin2;
+  if (signalPin2!=UNUSED_PIN) {
+    dualSignal=true;
+    getFastPin(signalPin2,fastSignalPin2);
+  }
+  else dualSignal=false; 
+  
   brakePin=brake_pin;
   currentPin=current_pin;
   senseFactor=sense_factor;
@@ -62,7 +59,7 @@ void MotorDriver::setPower(bool on) {
     setBrake(true);
     setBrake(false);
   }
-  WritePin(powerPin, on ? HIGH : LOW);
+  digitalWrite(powerPin, on ? HIGH : LOW);
 }
 
 // setBrake applies brake if on == true. So to get
@@ -80,18 +77,23 @@ void MotorDriver::setBrake(bool on) {
       pin=-pin;
       state=!state;
     }
-    WritePin(pin, state ? HIGH : LOW);
-    //DIAG(F("BrakePin: %d is %d\n"), pin, ReadPin(pin));
+    digitalWrite(pin, state ? HIGH : LOW);
 }
 
 void MotorDriver::setSignal( bool high) {
-  WritePin(signalPin, high ? HIGH : LOW);
-  if (signalPin2 != UNUSED_PIN) WritePin(signalPin2, high ? LOW : HIGH);
+   if (high) {
+      setHIGH(fastSignalPin);
+      if (dualSignal) setLOW(fastSignalPin2);
+   }
+   else {
+      setLOW(fastSignalPin);
+      if (dualSignal) setHIGH(fastSignalPin2);
+   }
 }
 
 
 int MotorDriver::getCurrentRaw() {
-  if (faultPin != UNUSED_PIN && ReadPin(faultPin) == LOW && ReadPin(powerPin) == HIGH)
+  if (faultPin != UNUSED_PIN && digitalRead(faultPin) == LOW && digitalRead(powerPin) == HIGH)
       return simulatedOverload;
   
   // IMPORTANT:  This function can be called in Interrupt() time within the 56uS timer
@@ -106,3 +108,12 @@ unsigned int MotorDriver::raw2mA( int raw) {
 int MotorDriver::mA2raw( unsigned int mA) {
   return (int)(mA / senseFactor);
 }
+
+void  MotorDriver::getFastPin(int pin, FASTPIN & result) {
+    DIAG(F("\nMotorDriver Pin=%d,"),pin);
+    uint8_t port = digitalPinToPort(pin);
+    result.out = portOutputRegister(port);
+    result.maskHIGH = digitalPinToBitMask(pin);
+    result.maskLOW = ~result.maskHIGH;
+    DIAG(F(" port=0x%x, out=0x%x, mask=0x%x\n"),port, result.out,result.maskHIGH);
+}
diff --git a/MotorDriver.h b/MotorDriver.h
index 077ab1d..61b36aa 100644
--- a/MotorDriver.h
+++ b/MotorDriver.h
@@ -24,6 +24,12 @@
 #define UNUSED_PIN 127 // inside int8_t
 #endif
 
+struct FASTPIN {
+  volatile uint8_t *out;
+  uint8_t maskHIGH;  
+  uint8_t maskLOW;  
+};
+
 class MotorDriver {
   public:
     MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin, byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin);
@@ -38,7 +44,10 @@ class MotorDriver {
     }
 
   private:
+    void  getFastPin(int pin, FASTPIN & result);
     byte powerPin, signalPin, signalPin2, currentPin, faultPin;
+    FASTPIN fastSignalPin, fastSignalPin2;
+    bool dualSignal;       // true to use signalPin2
     int8_t brakePin;       // negative means pin is inverted
     float senseFactor;
     unsigned int tripMilliamps;