Add docs and prepare ex-rail for archiving

If an Arduino pin was used as an input (e.g. by EXRAIL) without previously configuring it, the default pullup wouldn't be set up.  Now, on first call to the _read() method the pullup will be enabled.

CommandStation-EX.ino

@@ -109,7 +109,7 @@ void setup()
       LCN::init(LCN_SERIAL);
   #endif
 
-  LCD(1,F("Ready")); 
+  LCD(3,F("Ready")); 
 }
 
 void loop()
@@ -149,6 +149,6 @@ void loop()
   if (freeNow < ramLowWatermark)
   {
     ramLowWatermark = freeNow;
-    LCD(2,F("Free RAM=%5db"), ramLowWatermark);
+    LCD(3,F("Free RAM=%5db"), ramLowWatermark);
   }
 }

DCC.cpp

@@ -17,13 +17,6 @@
  *  You should have received a copy of the GNU General Public License
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
-
-#if __has_include ( "config.h")
-  #include "config.h"
-#else
-  #include "config.example.h"
-#endif
-
 #include "DIAG.h"
 #include "DCC.h"
 #include "DCCWaveform.h"
@@ -148,7 +141,7 @@ uint8_t DCC::getThrottleSpeed(int cab) {
 
 bool DCC::getThrottleDirection(int cab) {
   int reg=lookupSpeedTable(cab);
-  if (reg<0) return false ;
+  if (reg<0) return true;
   return (speedTable[reg].speedCode & 0x80) !=0;
 }
 
@@ -246,6 +239,9 @@ void DCC::updateGroupflags(byte & flags, int16_t functionNumber) {
 }
 
 void DCC::setAccessory(int address, byte number, bool activate) {
+  #ifdef DIAG_IO
+  DIAG(F("DCC::setAccessory(%d,%d,%d)"), address, number, activate);
+  #endif
   // use masks to detect wrong values and do nothing
   if(address != (address & 511))
     return;
@@ -253,15 +249,8 @@ void DCC::setAccessory(int address, byte number, bool activate) {
     return;
   byte b[2];
 
-  // first byte is of the form 10AAAAAA, where AAAAAA represent 6 least signifcant bits of accessory address
-  b[0] = address % 64 + 128;
-  // second byte is of the form 1AAACDDD, where C should be 1, and the least significant D represent activate/deactivate
-  // if we follow RCN-213, activate has to be reversed because in DCC++/DCC-EX activate=1 is "thrown, diverging",
-  // but in RCN-213, 1 means "closed, straight" and 0 "thrown, diverging"
-#ifdef TURNOUTS_RCN_213
-  activate = !activate;
-#endif
-  b[1] = ((((address / 64) % 8) << 4) + (number % 4 << 1) + activate % 2) ^ 0xF8;
+  b[0] = address % 64 + 128;                                     // first byte is of the form 10AAAAAA, where AAAAAA represent 6 least signifcant bits of accessory address
+  b[1] = ((((address / 64) % 8) << 4) + (number % 4 << 1) + activate % 2) ^ 0xF8; // second byte is of the form 1AAACDDD, where C should be 1, and the least significant D represent activate/deactivate
 
   DCCWaveform::mainTrack.schedulePacket(b, 2, 4);      // Repeat the packet four times
 }
@@ -368,8 +357,9 @@ const ackOp FLASH WRITE_BYTE_PROG[] = {
       
 const ackOp FLASH VERIFY_BYTE_PROG[] = {
       BASELINE,
+      BIV,         // ackManagerByte initial value
       VB,WACK,     // validate byte 
-      ITCB,       // if ok callback value
+      ITCB,       // if ok callback value	
       STARTMERGE,    //clear bit and byte values ready for merge pass
       // each bit is validated against 0 and the result inverted in MERGE
       // this is because there tend to be more zeros in cv values than ones.  
@@ -387,7 +377,7 @@ const ackOp FLASH VERIFY_BYTE_PROG[] = {
       V0, WACK, MERGE,
       V0, WACK, MERGE,
       V0, WACK, MERGE,
-      VB, WACK, ITCB,  // verify merged byte and return it if acked ok 
+      VB, WACK, ITCBV,  // verify merged byte and return it if acked ok - with retry report
       FAIL };
       
       
@@ -697,9 +687,15 @@ int DCC::nextLoco = 0;
 
 //ACK MANAGER
 ackOp  const *  DCC::ackManagerProg;
+ackOp  const *  DCC::ackManagerProgStart;
 byte   DCC::ackManagerByte;
+byte   DCC::ackManagerByteVerify;
 byte   DCC::ackManagerStash;
 int    DCC::ackManagerWord;
+byte   DCC::ackManagerRetry;
+byte   DCC::ackRetry = 2;
+int16_t  DCC::ackRetrySum;
+int16_t  DCC::ackRetryPSum;
 int    DCC::ackManagerCv;
 byte   DCC::ackManagerBitNum;
 bool   DCC::ackReceived;
@@ -732,7 +728,10 @@ void  DCC::ackManagerSetup(int cv, byte byteValueOrBitnum, ackOp const program[]
 
   ackManagerCv = cv;
   ackManagerProg = program;
+  ackManagerProgStart = program;
+  ackManagerRetry = ackRetry;
   ackManagerByte = byteValueOrBitnum;
+  ackManagerByteVerify = byteValueOrBitnum;
   ackManagerBitNum=byteValueOrBitnum;
   ackManagerCallback = callback;
 }
@@ -758,6 +757,7 @@ void DCC::ackManagerLoop() {
     // (typically waiting for a reset counter or ACK waiting, or when all finished.)
     switch (opcode) {
       case BASELINE:
+          if (DCCWaveform::progTrack.getPowerMode()==POWERMODE::OVERLOAD) return;
       	  if (checkResets(DCCWaveform::progTrack.autoPowerOff || ackManagerRejoin  ? 20 : 3)) return;
           DCCWaveform::progTrack.setAckBaseline();
           callbackState=READY;
@@ -831,7 +831,18 @@ void DCC::ackManagerLoop() {
             return;
           }
         break;
-
+		    
+      case ITCBV:   // If True callback(byte) - Verify
+          if (ackReceived) {
+            if (ackManagerByte == ackManagerByteVerify) {
+               ackRetrySum ++;
+               LCD(1, F("v %d %d Sum=%d"), ackManagerCv, ackManagerByte, ackRetrySum);
+            }
+            callback(ackManagerByte);
+            return;
+          }
+        break;
+		    
       case ITCB7:   // If True callback(byte & 0x7F)
           if (ackReceived) {
             callback(ackManagerByte & 0x7F);
@@ -849,7 +860,11 @@ void DCC::ackManagerLoop() {
       case FAIL:  // callback(-1)
            callback(-1);
            return;
-           
+
+      case BIV:     // ackManagerByte initial value
+           ackManagerByte = ackManagerByteVerify;
+           break;
+
       case STARTMERGE:
            ackManagerBitNum=7;
            ackManagerByte=0;     
@@ -915,6 +930,15 @@ void DCC::ackManagerLoop() {
 }
 
 void DCC::callback(int value) {
+    // check for automatic retry
+    if (value == -1 && ackManagerRetry > 0) {
+      ackRetrySum ++;
+      LCD(0, F("Retry %d %d Sum=%d"), ackManagerCv, ackManagerRetry, ackRetrySum);
+      ackManagerRetry --;
+      ackManagerProg = ackManagerProgStart;
+      return;
+    }
+
     static unsigned long callbackStart;
     // We are about to leave programming mode
     // Rule 1: If we have written to a decoder we must maintain power for 100mS

DCC.h

@@ -38,9 +38,11 @@ enum ackOp : byte
   ITC1,             // If True Callback(1)  (if prevous WACK got an ACK)
   ITC0,             // If True callback(0);
   ITCB,             // If True callback(byte)
+  ITCBV,            // If True callback(byte) - end of Verify Byte
   ITCB7,            // If True callback(byte &0x7F)
   NAKFAIL,          // if false callback(-1)
   FAIL,             // callback(-1)
+  BIV,              // Set ackManagerByte to initial value for Verify retry
   STARTMERGE,       // Clear bit and byte settings ready for merge pass
   MERGE,            // Merge previous wack response with byte value and decrement bit number (use for readimng CV bytes)
   SETBIT,           // sets bit number to next prog byte
@@ -64,8 +66,10 @@ enum   CALLBACK_STATE : byte {
 
 // Allocations with memory implications..!
 // Base system takes approx 900 bytes + 8 per loco. Turnouts, Sensors etc are dynamically created
-#ifdef ARDUINO_AVR_UNO
+#if defined(ARDUINO_AVR_UNO)
 const byte MAX_LOCOS = 20;
+#elif defined(ARDUINO_AVR_NANO)
+const byte MAX_LOCOS = 30;
 #else
 const byte MAX_LOCOS = 50;
 #endif
@@ -113,6 +117,12 @@ public:
   static inline void setGlobalSpeedsteps(byte s) {
     globalSpeedsteps = s;
   };
+  static inline int16_t setAckRetry(byte retry) {
+    ackRetry = retry;
+    ackRetryPSum = ackRetrySum;
+    ackRetrySum = 0;  // reset running total
+    return ackRetryPSum;
+  };
 
 private:
   struct LOCO
@@ -141,9 +151,15 @@ private:
 
   // ACK MANAGER
   static ackOp const *ackManagerProg;
+  static ackOp const *ackManagerProgStart;
   static byte ackManagerByte;
+  static byte ackManagerByteVerify;
   static byte ackManagerBitNum;
   static int ackManagerCv;
+  static byte ackManagerRetry;
+  static byte ackRetry;
+  static int16_t ackRetrySum;
+  static int16_t ackRetryPSum;
   static int ackManagerWord;
   static byte ackManagerStash;
   static bool ackReceived;

DCCEXParser.cpp

@@ -27,11 +27,22 @@
 #include "freeMemory.h"
 #include "GITHUB_SHA.h"
 #include "version.h"
+#include "defines.h"
 
 #include "EEStore.h"
 #include "DIAG.h"
 #include <avr/wdt.h>
 
+////////////////////////////////////////////////////////////////////////////////
+//
+// Figure out if we have enough memory for advanced features
+//
+#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_NANO)
+// nope
+#else
+#define HAS_ENOUGH_MEMORY
+#endif
+
 // These keywords are used in the <1> command. The number is what you get if you use the keyword as a parameter.
 // To discover new keyword numbers , use the <$ YOURKEYWORD> command
 const int16_t HASH_KEYWORD_PROG = -29718;
@@ -40,8 +51,6 @@ const int16_t HASH_KEYWORD_JOIN = -30750;
 const int16_t HASH_KEYWORD_CABS = -11981;
 const int16_t HASH_KEYWORD_RAM = 25982;
 const int16_t HASH_KEYWORD_CMD = 9962;
-const int16_t HASH_KEYWORD_WIT = 31594;
-const int16_t HASH_KEYWORD_WIFI = -5583;
 const int16_t HASH_KEYWORD_ACK = 3113;
 const int16_t HASH_KEYWORD_ON = 2657;
 const int16_t HASH_KEYWORD_DCC = 6436;
@@ -49,14 +58,26 @@ const int16_t HASH_KEYWORD_SLOW = -17209;
 const int16_t HASH_KEYWORD_PROGBOOST = -6353;
 const int16_t HASH_KEYWORD_EEPROM = -7168;
 const int16_t HASH_KEYWORD_LIMIT = 27413;
-const int16_t HASH_KEYWORD_ETHERNET = -30767;    
 const int16_t HASH_KEYWORD_MAX = 16244;
 const int16_t HASH_KEYWORD_MIN = 15978;
-const int16_t HASH_KEYWORD_LCN = 15137;   
 const int16_t HASH_KEYWORD_RESET = 26133;
+const int16_t HASH_KEYWORD_RETRY = 25704;
 const int16_t HASH_KEYWORD_SPEED28 = -17064;
 const int16_t HASH_KEYWORD_SPEED128 = 25816;
-const int16_t HASH_KEYWORD_SERVO = 27709;
+const int16_t HASH_KEYWORD_SERVO=27709;
+const int16_t HASH_KEYWORD_VPIN=-415;
+const int16_t HASH_KEYWORD_C=67;
+const int16_t HASH_KEYWORD_T=84;
+const int16_t HASH_KEYWORD_LCN = 15137;
+const int16_t HASH_KEYWORD_HAL = 10853;
+const int16_t HASH_KEYWORD_SHOW = -21309;
+const int16_t HASH_KEYWORD_ANIN = -10424;
+const int16_t HASH_KEYWORD_ANOUT = -26399;
+#ifdef HAS_ENOUGH_MEMORY
+const int16_t HASH_KEYWORD_WIFI = -5583;
+const int16_t HASH_KEYWORD_ETHERNET = -30767;
+const int16_t HASH_KEYWORD_WIT = 31594;
+#endif
 
 int16_t DCCEXParser::stashP[MAX_COMMAND_PARAMS];
 bool DCCEXParser::stashBusy;
@@ -254,6 +275,7 @@ void DCCEXParser::parse(const FSH * cmd) {
 }
 
 // See documentation on DCC class for info on this section
+
 void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
 {
     (void)EEPROM; // tell compiler not to warn this is unused
@@ -345,8 +367,12 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
           || ((subaddress & 0x03) != subaddress)  // invalid subaddress (limit 2 bits ) 
           || ((p[activep]  & 0x01) != p[activep]) // invalid activate 0|1
           ) break; 
-          // TODO: Trigger configurable range of addresses on local VPins.
+          // Honour the configuration option (config.h) which allows the <a> command to be reversed
+#ifdef DCC_ACCESSORY_RCN_213
+          DCC::setAccessory(address, subaddress,p[activep]==0);
+#else
           DCC::setAccessory(address, subaddress,p[activep]==1);
+#endif
         }
         return;
      
@@ -452,6 +478,7 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
 		if (mode == POWERMODE::OFF)
 		  DCC::setProgTrackBoost(false);  // Prog track boost mode will not outlive prog track off
                 StringFormatter::send(stream, F("<p%c>\n"), opcode);
+		        LCD(2, F("p%c"), opcode);
                 return;
             }
             switch (p[0])
@@ -459,6 +486,7 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
             case HASH_KEYWORD_MAIN:
                 DCCWaveform::mainTrack.setPowerMode(mode);
                 StringFormatter::send(stream, F("<p%c MAIN>\n"), opcode);
+	            LCD(2, F("p%c MAIN"), opcode);
                 return;
 
             case HASH_KEYWORD_PROG:
@@ -466,6 +494,7 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
 		if (mode == POWERMODE::OFF)
 		  DCC::setProgTrackBoost(false);  // Prog track boost mode will not outlive prog track off
                 StringFormatter::send(stream, F("<p%c PROG>\n"), opcode);
+		        LCD(2, F("p%c PROG"), opcode);
                 return;
             case HASH_KEYWORD_JOIN:
                 DCCWaveform::mainTrack.setPowerMode(mode);
@@ -474,9 +503,13 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
                 {
                     DCC::setProgTrackSyncMain(true);
                     StringFormatter::send(stream, F("<p1 JOIN>\n"), opcode);
+		            LCD(2, F("p1 JOIN"));
                 }
                 else
+		        {
                     StringFormatter::send(stream, F("<p0>\n"));
+		            LCD(2, F("p0"));
+	    	    }
                 return;
             }
             break;
@@ -658,7 +691,7 @@ bool DCCEXParser::parseT(Print *stream, int16_t params, int16_t p[])
     case 0: // <T>  list turnout definitions
     {
         bool gotOne = false;
-        for (Turnout *tt = Turnout::firstTurnout; tt != NULL; tt = tt->nextTurnout)
+        for (Turnout *tt = Turnout::first(); tt != NULL; tt = tt->next())
         {
             gotOne = true;
             tt->print(stream);
@@ -672,17 +705,62 @@ bool DCCEXParser::parseT(Print *stream, int16_t params, int16_t p[])
         StringFormatter::send(stream, F("<O>\n"));
         return true;
 
-    case 2: // <T id 0|1>  turnout 0=CLOSE,1=THROW
-        if (p[1]>1 || p[1]<0 ) return false;
-        if (!Turnout::setClosed(p[0],p[1]==0)) return false;
-        StringFormatter::send(stream, F("<H %d %d>\n"), p[0], p[1]);
-        return true;
+    case 2: // <T id 0|1|T|C> 
+        {
+          bool state = false;
+          switch (p[1]) {
+            // Turnout messages use 1=throw, 0=close.
+            case 0:
+            case HASH_KEYWORD_C:
+              state = true;
+              break;
+            case 1:
+            case HASH_KEYWORD_T:
+              state= false;
+              break;
+            default:
+              return false;  // Invalid parameter
+          }
+          if (!Turnout::setClosed(p[0], state)) return false;
 
-    default: // Anything else is handled by Turnout class.
-        if (!Turnout::create(p[0], params-1, &p[1]))
-            return false;
-        StringFormatter::send(stream, F("<O>\n"));
-        return true;
+          // Send acknowledgement to caller if the command was not received over Serial
+          // (acknowledgement messages on Serial are sent by the Turnout class).
+          if (stream != &Serial) Turnout::printState(p[0], stream);
+          return true;
+        }
+
+    default: // Anything else is some kind of turnout create function.
+      if (params == 6 && p[1] == HASH_KEYWORD_SERVO) { // <T id SERVO n n n n>
+        if (!ServoTurnout::create(p[0], (VPIN)p[2], (uint16_t)p[3], (uint16_t)p[4], (uint8_t)p[5]))
+          return false;
+      } else 
+      if (params == 3 && p[1] == HASH_KEYWORD_VPIN) { // <T id VPIN n>
+        if (!VpinTurnout::create(p[0], p[2])) return false;
+      } else 
+      if (params >= 3 && p[1] == HASH_KEYWORD_DCC) {
+        // <T id DCC addr subadd>   0<=addr<=511, 0<=subadd<=3 (like <a> command).<T>
+        if (params==4 && p[2]>=0 && p[2]<512 && p[3]>=0 && p[3]<4) { // <T id DCC n m>
+          if (!DCCTurnout::create(p[0], p[2], p[3])) return false;
+        } else if (params==3 && p[2]>0 && p[2]<=512*4) { // <T id DCC nn>, 1<=nn<=2048
+          // Linearaddress 1 maps onto decoder address 1/0 (not 0/0!).
+          if (!DCCTurnout::create(p[0], (p[2]-1)/4+1, (p[2]-1)%4)) return false;
+        } else
+          return false;
+      } else 
+      if (params==3) { // legacy <T id addr subadd> for DCC accessory
+        if (p[1]>=0 && p[1]<512 && p[2]>=0 && p[2]<4) {
+          if (!DCCTurnout::create(p[0], p[1], p[2])) return false;
+        } else
+          return false;
+      } 
+      else 
+      if (params==4) { // legacy <T id n n n> for Servo
+        if (!ServoTurnout::create(p[0], (VPIN)p[1], (uint16_t)p[2], (uint16_t)p[3], 1)) return false;
+      } else
+        return false;
+
+      StringFormatter::send(stream, F("<O>\n"));
+      return true;
     }
 }
 
@@ -733,17 +811,20 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
         StringFormatter::send(stream, F("Free memory=%d\n"), minimumFreeMemory());
         break;
 
-    case HASH_KEYWORD_ACK: // <D ACK ON/OFF> <D ACK [LIMIT|MIN|MAX] Value>
+    case HASH_KEYWORD_ACK: // <D ACK ON/OFF> <D ACK [LIMIT|MIN|MAX|RETRY] Value>
 	if (params >= 3) {
 	    if (p[1] == HASH_KEYWORD_LIMIT) {
 	      DCCWaveform::progTrack.setAckLimit(p[2]);
-	      StringFormatter::send(stream, F("Ack limit=%dmA\n"), p[2]);
+	      LCD(1, F("Ack Limit=%dmA"), p[2]);  // <D ACK LIMIT 42>
 	    } else if (p[1] == HASH_KEYWORD_MIN) {
 	      DCCWaveform::progTrack.setMinAckPulseDuration(p[2]);
-	      StringFormatter::send(stream, F("Ack min=%dus\n"), p[2]);
+	      LCD(0, F("Ack Min=%dus"), p[2]);  //   <D ACK MIN 1500>
 	    } else if (p[1] == HASH_KEYWORD_MAX) {
 	      DCCWaveform::progTrack.setMaxAckPulseDuration(p[2]);
-	      StringFormatter::send(stream, F("Ack max=%dus\n"), p[2]);
+	      LCD(0, F("Ack Max=%dus"), p[2]);  //   <D ACK MAX 9000>
+	    } else if (p[1] == HASH_KEYWORD_RETRY) {
+	      if (p[2] >255) p[2]=3;
+	      LCD(0, F("Ack Retry=%d Sum=%d"), p[2], DCC::setAckRetry(p[2]));  //   <D ACK RETRY 2>
 	    }
 	} else {
 	  StringFormatter::send(stream, F("Ack diag %S\n"), onOff ? F("on") : F("off"));
@@ -755,21 +836,23 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
         Diag::CMD = onOff;
         return true;
 
+#ifdef HAS_ENOUGH_MEMORY
     case HASH_KEYWORD_WIFI: // <D WIFI ON/OFF>
         Diag::WIFI = onOff;
         return true;
 
-   case HASH_KEYWORD_ETHERNET: // <D ETHERNET ON/OFF>
+    case HASH_KEYWORD_ETHERNET: // <D ETHERNET ON/OFF>
         Diag::ETHERNET = onOff;
         return true;
 
     case HASH_KEYWORD_WIT: // <D WIT ON/OFF>
         Diag::WITHROTTLE = onOff;
         return true;
-  
+
     case HASH_KEYWORD_LCN: // <D LCN ON/OFF>
         Diag::LCN = onOff;
         return true;
+#endif
 
     case HASH_KEYWORD_PROGBOOST:
         DCC::setProgTrackBoost(true);
@@ -797,10 +880,22 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
 	StringFormatter::send(stream, F("128 Speedsteps"));
         return true;
 
-    case HASH_KEYWORD_SERVO:
+    case HASH_KEYWORD_SERVO:  // <D SERVO vpin position [profile]>
+    case HASH_KEYWORD_ANOUT:  // <D ANOUT vpin position [profile]>
         IODevice::writeAnalogue(p[1], p[2], params>3 ? p[3] : 0);
         break;
 
+    case HASH_KEYWORD_ANIN:   // <D ANIN vpin>  Display analogue input value
+        DIAG(F("VPIN=%d value=%d"), p[1], IODevice::readAnalogue(p[1]));
+        break;
+
+#if !defined(IO_MINIMAL_HAL)
+    case HASH_KEYWORD_HAL: 
+        if (p[1] == HASH_KEYWORD_SHOW) 
+          IODevice::DumpAll();
+        break;
+#endif
+
     default: // invalid/unknown
         break;
     }

DCCWaveform.cpp

@@ -17,7 +17,7 @@
  *  You should have received a copy of the GNU General Public License
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
- #pragma GCC optimize ("-O3")
+
 #include <Arduino.h>
 
 #include "DCCWaveform.h"
@@ -46,10 +46,8 @@ void DCCWaveform::begin(MotorDriver * mainDriver, MotorDriver * progDriver) {
 				 && (mainDriver->getFaultPin() != UNUSED_PIN));
   // Only use PWM if both pins are PWM capable. Otherwise JOIN does not work
   MotorDriver::usePWM= mainDriver->isPWMCapable() && progDriver->isPWMCapable();
-  if (MotorDriver::usePWM)
-    DIAG(F("Signal pin config: high accuracy waveform"));
-  else
-    DIAG(F("Signal pin config: normal accuracy waveform"));
+  DIAG(F("Signal pin config: %S accuracy waveform"),
+	 MotorDriver::usePWM ? F("high") : F("normal") );
   DCCTimer::begin(DCCWaveform::interruptHandler);     
 }
 
@@ -58,6 +56,8 @@ void DCCWaveform::loop(bool ackManagerActive) {
   progTrack.checkPowerOverload(ackManagerActive);
 }
 
+#pragma GCC push_options
+#pragma GCC optimize ("-O3")
 void DCCWaveform::interruptHandler() {
   // call the timer edge sensitive actions for progtrack and maintrack
   // member functions would be cleaner but have more overhead
@@ -79,7 +79,7 @@ void DCCWaveform::interruptHandler() {
   else if (progTrack.ackPending) progTrack.checkAck();
 
 }
-
+#pragma GCC push_options
 
 // An instance of this class handles the DCC transmissions for one track. (main or prog)
 // Interrupts are marshalled via the statics.
@@ -114,6 +114,7 @@ void DCCWaveform::setPowerMode(POWERMODE mode) {
   powerMode = mode;
   bool ison = (mode == POWERMODE::ON);
   motorDriver->setPower( ison);
+  sentResetsSincePacket=0; 
 }
 
 
@@ -124,6 +125,8 @@ void DCCWaveform::checkPowerOverload(bool ackManagerActive) {
   if (!isMainTrack && !ackManagerActive && !progTrackSyncMain && !progTrackBoosted)
     tripValue=progTripValue;
   
+  // Trackname for diag messages later
+  const FSH*trackname = isMainTrack ? F("MAIN") : F("PROG");
   switch (powerMode) {
     case POWERMODE::OFF:
       sampleDelay = POWER_SAMPLE_OFF_WAIT;
@@ -141,9 +144,9 @@ void DCCWaveform::checkPowerOverload(bool ackManagerActive) {
 	      }
 	      // Write this after the fact as we want to turn on as fast as possible
 	      // because we don't know which output actually triggered the fault pin
-	      DIAG(F("*** COMMON FAULT PIN ACTIVE - TOGGLED POWER on %S ***"), isMainTrack ? F("MAIN") : F("PROG"));
+	      DIAG(F("COMMON FAULT PIN ACTIVE - TOGGLED POWER on %S"), trackname);
 	  } else {
-	      DIAG(F("*** %S FAULT PIN ACTIVE - OVERLOAD ***"), isMainTrack ? F("MAIN") : F("PROG"));
+	    DIAG(F("%S FAULT PIN ACTIVE - OVERLOAD"), trackname);
 	      if (lastCurrent < tripValue) {
 		  lastCurrent = tripValue; // exaggerate
 	      }
@@ -161,7 +164,7 @@ void DCCWaveform::checkPowerOverload(bool ackManagerActive) {
         unsigned int maxmA=motorDriver->raw2mA(tripValue);
 	power_good_counter=0;
         sampleDelay = power_sample_overload_wait;
-        DIAG(F("*** %S TRACK POWER OVERLOAD current=%d max=%d  offtime=%d ***"), isMainTrack ? F("MAIN") : F("PROG"), mA, maxmA, sampleDelay);
+        DIAG(F("%S TRACK POWER OVERLOAD current=%d max=%d offtime=%d"), trackname, mA, maxmA, sampleDelay);
 	if (power_sample_overload_wait >= 10000)
 	    power_sample_overload_wait = 10000;
 	else
@@ -173,7 +176,7 @@ void DCCWaveform::checkPowerOverload(bool ackManagerActive) {
       setPowerMode(POWERMODE::ON);
       sampleDelay = POWER_SAMPLE_ON_WAIT;
       // Debug code....
-      DIAG(F("*** %S TRACK POWER RESET delay=%d ***"), isMainTrack ? F("MAIN") : F("PROG"), sampleDelay);
+      DIAG(F("%S TRACK POWER RESET delay=%d"), trackname, sampleDelay);
       break;
     default:
       sampleDelay = 999; // cant get here..meaningless statement to avoid compiler warning.
@@ -197,6 +200,8 @@ const bool DCCWaveform::signalTransform[]={
    /* WAVE_LOW_0   -> */ LOW,
    /* WAVE_PENDING (should not happen) -> */ LOW};
         
+#pragma GCC push_options
+#pragma GCC optimize ("-O3")
 void DCCWaveform::interrupt2() {
   // calculate the next bit to be sent:
   // set state WAVE_MID_1  for a 1=bit
@@ -252,7 +257,7 @@ void DCCWaveform::interrupt2() {
     }
   }  
 }
-
+#pragma GCC pop_options
 
 
 // Wait until there is no packet pending, then make this pending
@@ -306,6 +311,8 @@ byte DCCWaveform::getAck() {
       return(0);  // pending set off but not detected means no ACK.   
 }
 
+#pragma GCC push_options
+#pragma GCC optimize ("-O3")
 void DCCWaveform::checkAck() {
     // This function operates in interrupt() time so must be fast and can't DIAG 
     if (sentResetsSincePacket > 6) {  //ACK timeout
@@ -355,3 +362,4 @@ void DCCWaveform::checkAck() {
     }      
     ackPulseStart=0;  // We have detected a too-short or too-long pulse so ignore and wait for next leading edge 
 }
+#pragma GCC pop_options

EEStore.cpp

@@ -19,89 +19,87 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
 #include "EEStore.h"
-#include "Turnouts.h"
-#include "Sensors.h"
-#include "Outputs.h"
+
 #include "DIAG.h"
+#include "Outputs.h"
+#include "Sensors.h"
+#include "Turnouts.h"
 
 #if defined(ARDUINO_ARCH_SAMD)
 ExternalEEPROM EEPROM;
 #endif
 
-void EEStore::init(){
+void EEStore::init() {
 #if defined(ARDUINO_ARCH_SAMD)
-    EEPROM.begin(0x50);     // Address for Microchip 24-series EEPROM with all three A pins grounded (0b1010000 = 0x50)
+  EEPROM.begin(0x50);  // Address for Microchip 24-series EEPROM with all three
+                       // A pins grounded (0b1010000 = 0x50)
 #endif
 
-    eeStore=(EEStore *)calloc(1,sizeof(EEStore));
-    
-    EEPROM.get(0,eeStore->data);                                       // get eeStore data
+  eeStore = (EEStore *)calloc(1, sizeof(EEStore));
 
-    if(strncmp(eeStore->data.id,EESTORE_ID,sizeof(EESTORE_ID))!=0){    // check to see that eeStore contains valid DCC++ ID
-        sprintf(eeStore->data.id,EESTORE_ID);                           // if not, create blank eeStore structure (no turnouts, no sensors) and save it back to EEPROM
-        eeStore->data.nTurnouts=0;
-        eeStore->data.nSensors=0;
-        eeStore->data.nOutputs=0;
-        EEPROM.put(0,eeStore->data);
-    }
+  EEPROM.get(0, eeStore->data);  // get eeStore data
 
-    reset();            // set memory pointer to first free EEPROM space
-    Turnout::load();    // load turnout definitions
-    Sensor::load();     // load sensor definitions
-    Output::load();     // load output definitions
+  // check to see that eeStore contains valid DCC++ ID
+  if (strncmp(eeStore->data.id, EESTORE_ID, sizeof(EESTORE_ID)) != 0) {  
+    // if not, create blank eeStore structure (no
+    // turnouts, no sensors) and save it back to EEPROM  
+    strncpy(eeStore->data.id, EESTORE_ID, sizeof(EESTORE_ID));  
+    eeStore->data.nTurnouts = 0;
+    eeStore->data.nSensors = 0;
+    eeStore->data.nOutputs = 0;
+    EEPROM.put(0, eeStore->data);
+  }
 
+  reset();          // set memory pointer to first free EEPROM space
+  Turnout::load();  // load turnout definitions
+  Sensor::load();   // load sensor definitions
+  Output::load();   // load output definitions
 }
 
 ///////////////////////////////////////////////////////////////////////////////
 
-void EEStore::clear(){
-
-    sprintf(eeStore->data.id,EESTORE_ID);                           // create blank eeStore structure (no turnouts, no sensors) and save it back to EEPROM
-    eeStore->data.nTurnouts=0;
-    eeStore->data.nSensors=0;
-    eeStore->data.nOutputs=0;
-    EEPROM.put(0,eeStore->data);
-
+void EEStore::clear() {
+  sprintf(eeStore->data.id,
+          EESTORE_ID);  // create blank eeStore structure (no turnouts, no
+                        // sensors) and save it back to EEPROM
+  eeStore->data.nTurnouts = 0;
+  eeStore->data.nSensors = 0;
+  eeStore->data.nOutputs = 0;
+  EEPROM.put(0, eeStore->data);
 }
 
 ///////////////////////////////////////////////////////////////////////////////
 
-void EEStore::store(){
-    reset();
-    Turnout::store();
-    Sensor::store();
-    Output::store();
-    EEPROM.put(0,eeStore->data);
-    DIAG(F("EEPROM used: %d bytes"), EEStore::pointer());
+void EEStore::store() {
+  reset();
+  Turnout::store();
+  Sensor::store();
+  Output::store();
+  EEPROM.put(0, eeStore->data);
+  DIAG(F("EEPROM used: %d/%d bytes"), EEStore::pointer(), EEPROM.length());
 }
 
 ///////////////////////////////////////////////////////////////////////////////
 
-void EEStore::advance(int n){
-    eeAddress+=n;
-}
+void EEStore::advance(int n) { eeAddress += n; }
 
 ///////////////////////////////////////////////////////////////////////////////
 
-void EEStore::reset(){
-    eeAddress=sizeof(EEStore);
-}
+void EEStore::reset() { eeAddress = sizeof(EEStore); }
 ///////////////////////////////////////////////////////////////////////////////
 
-int EEStore::pointer(){
-    return(eeAddress);
-}
+int EEStore::pointer() { return (eeAddress); }
 ///////////////////////////////////////////////////////////////////////////////
 
 void EEStore::dump(int num) {
-    byte b;
-    DIAG(F("Addr  0x  char"));
-    for (int n=0 ; n<num; n++) {
-	EEPROM.get(n, b);
-	DIAG(F("%d     %x    %c"),n,b,isprint(b) ? b : ' ');
-    }
+  byte b;
+  DIAG(F("Addr  0x  char"));
+  for (int n = 0; n < num; n++) {
+    EEPROM.get(n, b);
+    DIAG(F("%d     %x    %c"), n, b, isprint(b) ? b : ' ');
+  }
 }
 ///////////////////////////////////////////////////////////////////////////////
 
-EEStore *EEStore::eeStore=NULL;
-int EEStore::eeAddress=0;
+EEStore *EEStore::eeStore = NULL;
+int EEStore::eeAddress = 0;

EEStore.h

@@ -29,7 +29,7 @@ extern ExternalEEPROM EEPROM;
 #include <EEPROM.h>
 #endif
 
-#define EESTORE_ID "DCC++0"
+#define EESTORE_ID "DCC++1"
 
 struct EEStoreData{
   char id[sizeof(EESTORE_ID)];

EthernetInterface.cpp

@@ -17,12 +17,6 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  * 
  */
-#if __has_include ( "config.h")
-  #include "config.h"
-#else
-  #warning config.h not found. Using defaults from config.example.h 
-  #include "config.example.h"
-#endif
 #include "defines.h" 
 #if ETHERNET_ON == true
 #include "EthernetInterface.h"

EthernetInterface.h

@@ -22,12 +22,8 @@
 
 #ifndef EthernetInterface_h
 #define EthernetInterface_h
-#if __has_include ( "config.h")
-  #include "config.h"
-#else
-  #warning config.h not found. Using defaults from config.example.h 
-  #include "config.example.h"
-#endif
+
+#include "defines.h")
 #include "DCCEXParser.h"
 #include <Arduino.h>
 #include <avr/pgmspace.h>

I2CManager.cpp

@@ -147,6 +147,25 @@ uint8_t I2CManagerClass::finishRB(I2CRB *rb, uint8_t status) {
   return status;
 }
 
+/***************************************************************************
+ * Get a message corresponding to the error status
+ ***************************************************************************/
+const FSH *I2CManagerClass::getErrorMessage(uint8_t status) {
+  switch (status) {
+    case I2C_STATUS_OK: return F("OK");
+    case I2C_STATUS_TRUNCATED: return F("Transmission truncated");
+    case I2C_STATUS_NEGATIVE_ACKNOWLEDGE: return F("No response from device (address NAK)");
+    case I2C_STATUS_TRANSMIT_ERROR: return F("Transmit error (data NAK)");
+    case I2C_STATUS_OTHER_TWI_ERROR: return F("Other Wire/TWI error");
+    case I2C_STATUS_TIMEOUT: return F("Timeout");
+    case I2C_STATUS_ARBITRATION_LOST: return F("Arbitration lost");
+    case I2C_STATUS_BUS_ERROR: return F("I2C bus error");
+    case I2C_STATUS_UNEXPECTED_ERROR: return F("Unexpected error");
+    case I2C_STATUS_PENDING: return F("Request pending");
+    default: return F("Error code not recognised");
+  }
+}
+
 /***************************************************************************
  *  Declare singleton class instance.
  ***************************************************************************/
@@ -158,12 +177,25 @@ I2CManagerClass I2CManager = I2CManagerClass();
 /////////////////////////////////////////////////////////////////////////////
 
 /***************************************************************************
- *  Block waiting for request block to complete, and return completion status
+ *  Block waiting for request block to complete, and return completion status.
+ *  Since such a loop could potentially last for ever if the RB status doesn't
+ *  change, we set a high limit (1sec, 1000ms) on the wait time and, if it
+ *  hasn't changed by that time we assume it's not going to, and just return
+ *  a timeout status.  This means that CS will not lock up.
  ***************************************************************************/
 uint8_t I2CRB::wait() {
-  do
+  unsigned long waitStart = millis();
+  do {
     I2CManager.loop();
-  while (status==I2C_STATUS_PENDING);
+    // Rather than looping indefinitely, let's set a very high timeout (1s).
+    if ((millis() - waitStart) > 1000UL) { 
+      DIAG(F("I2C TIMEOUT I2C:x%x I2CRB:x%x"), i2cAddress, this);
+      status = I2C_STATUS_TIMEOUT;
+      // Note that, although the timeout is posted, the request may yet complete.
+      // TODO: Ideally we would like to cancel the request.
+      return status;
+    }
+  } while (status==I2C_STATUS_PENDING);
   return status;
 }
 

I2CManager.h

@@ -118,12 +118,15 @@
 
 // Status codes for I2CRB structures.
 enum : uint8_t {
+  // Codes used by Wire and by native drivers
   I2C_STATUS_OK=0,
   I2C_STATUS_TRUNCATED=1,
-  I2C_STATUS_DEVICE_NOT_PRESENT=2,
+  I2C_STATUS_NEGATIVE_ACKNOWLEDGE=2,
   I2C_STATUS_TRANSMIT_ERROR=3,
-  I2C_STATUS_NEGATIVE_ACKNOWLEDGE=4,
   I2C_STATUS_TIMEOUT=5,
+  // Code used by Wire only
+  I2C_STATUS_OTHER_TWI_ERROR=4, // catch-all error
+  // Codes used by native drivers only
   I2C_STATUS_ARBITRATION_LOST=6,
   I2C_STATUS_BUS_ERROR=7,
   I2C_STATUS_UNEXPECTED_ERROR=8,
@@ -151,14 +154,16 @@ typedef enum : uint8_t
 #define I2C_FREQ    400000L
 #endif
 
-// Struct defining a request context for an I2C operation.
-struct I2CRB {
+// Class defining a request context for an I2C operation.
+class I2CRB {
+public:
   volatile uint8_t status; // Completion status, or pending flag (updated from IRC)
   volatile uint8_t nBytes; // Number of bytes read (updated from IRC)
 
+  inline I2CRB() { status = I2C_STATUS_OK; };
   uint8_t wait();
   bool isBusy();
-  inline void init() { status = I2C_STATUS_OK; };
+
   void setReadParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen);
   void setRequestParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen, const uint8_t *writeBuffer, uint8_t writeLen);
   void setWriteParams(uint8_t i2cAddress, const uint8_t *writeBuffer, uint8_t writeLen);
@@ -213,6 +218,10 @@ public:
   // Loop method
   void loop();
 
+  // Expand error codes into text.  Note that they are in flash so 
+  // need to be printed using FSH.
+  static const FSH *getErrorMessage(uint8_t status);
+
 private:
   bool _beginCompleted = false;
   bool _clockSpeedFixed = false;
@@ -235,7 +244,7 @@ private:
     // Mark volatile as they are updated by IRC and read/written elsewhere.
     static I2CRB * volatile queueHead;
     static I2CRB * volatile queueTail;
-    static volatile uint8_t status;
+    static volatile uint8_t state;
 
     static I2CRB * volatile currentRequest;
     static volatile uint8_t txCount;
@@ -258,7 +267,9 @@ private:
     static void I2C_close();
     
   public:
-    void setTimeout(unsigned long value) { timeout = value;};
+    // setTimeout sets the timout value for I2C transactions.
+    // TODO: Get I2C timeout working before uncommenting the code below.
+    void setTimeout(unsigned long value) { (void)value; /* timeout = value; */ };
 
     // handleInterrupt needs to be public to be called from the ISR function!
     static void handleInterrupt();

I2CManager_AVR.h

@@ -62,7 +62,18 @@
  *  Set I2C clock speed register.
  ***************************************************************************/
 void I2CManagerClass::I2C_setClock(unsigned long i2cClockSpeed) {
-  TWBR = ((F_CPU / i2cClockSpeed) - 16) / 2;
+  unsigned long temp = ((F_CPU / i2cClockSpeed) - 16) / 2;
+  for (uint8_t preScaler = 0; preScaler<=3; preScaler++) {
+    if (temp <= 255) {
+      TWBR = temp;
+      TWSR = (TWSR & 0xfc) | preScaler;
+      return;
+    } else 
+      temp /= 4;
+  }
+  // Set slowest speed ~= 500 bits/sec
+  TWBR = 255;
+  TWSR |= 0x03;
 }
 
 /***************************************************************************
@@ -136,7 +147,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
       } else {  // Nothing left to send or receive
         TWDR = 0xff;  // Default condition = SDA released
         TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-        status = I2C_STATUS_OK;
+        state = I2C_STATUS_OK;
       }
       break;
     case TWI_MRX_DATA_ACK:      // Data byte has been received and ACK transmitted
@@ -159,7 +170,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
         bytesToReceive--;
       }
       TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-      status = I2C_STATUS_OK;
+      state = I2C_STATUS_OK;
       break;
     case TWI_START:             // START has been transmitted  
     case TWI_REP_START:         // Repeated START has been transmitted
@@ -175,7 +186,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
     case TWI_MTX_DATA_NACK:     // Data byte has been transmitted and NACK received
       TWDR = 0xff;  // Default condition = SDA released
       TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-      status = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      state = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
       break;
     case TWI_ARB_LOST:          // Arbitration lost
       // Restart transaction from start.
@@ -185,7 +196,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
     default:
       TWDR = 0xff;  // Default condition = SDA released
       TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-      status = I2C_STATUS_TRANSMIT_ERROR;
+      state = I2C_STATUS_TRANSMIT_ERROR;
   }
 }
 

I2CManager_Mega4809.h

@@ -105,14 +105,14 @@ void I2CManagerClass::I2C_handleInterrupt() {
     I2C_sendStart();   // Reinitiate request
   } else if (currentStatus & TWI_BUSERR_bm) {
     // Bus error
-    status = I2C_STATUS_BUS_ERROR;
+    state = I2C_STATUS_BUS_ERROR;
     TWI0.MSTATUS = currentStatus; // clear all flags
   } else if (currentStatus & TWI_WIF_bm) {
     // Master write completed
     if (currentStatus & TWI_RXACK_bm) {
       // Nacked, send stop.
       TWI0.MCTRLB = TWI_MCMD_STOP_gc;
-      status = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      state = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
     } else if (bytesToSend) {
       // Acked, so send next byte
       if (currentRequest->operation == OPERATION_SEND_P)
@@ -126,7 +126,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
     } else {
       // No more data to send/receive. Initiate a STOP condition.
       TWI0.MCTRLB = TWI_MCMD_STOP_gc;
-      status = I2C_STATUS_OK;  // Done
+      state = I2C_STATUS_OK;  // Done
     }
   } else if (currentStatus & TWI_RIF_bm) {
     // Master read completed without errors
@@ -136,7 +136,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
     } else { 
       // Buffer full, issue nack/stop
       TWI0.MCTRLB = TWI_ACKACT_bm | TWI_MCMD_STOP_gc;
-      status = I2C_STATUS_OK;
+      state = I2C_STATUS_OK;
     }
     if (bytesToReceive) {
       // More bytes to receive, issue ack and start another read
@@ -144,7 +144,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
     } else {
       // Transaction finished, issue NACK and STOP.
       TWI0.MCTRLB = TWI_ACKACT_bm | TWI_MCMD_STOP_gc;
-      status = I2C_STATUS_OK;
+      state = I2C_STATUS_OK;
     }
   }
 }

I2CManager_NonBlocking.h

@@ -41,7 +41,7 @@
 void I2CManagerClass::_initialise()
 {
   queueHead = queueTail = NULL;
-  status = I2C_STATE_FREE;
+  state = I2C_STATE_FREE;
   I2C_init();
 }
 
@@ -59,10 +59,9 @@ void I2CManagerClass::_setClock(unsigned long i2cClockSpeed) {
  ***************************************************************************/
 void I2CManagerClass::startTransaction() { 
   ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-    I2CRB *t = queueHead;
-    if ((status == I2C_STATE_FREE) && (t != NULL)) {
-      status = I2C_STATE_ACTIVE;
-      currentRequest = t;
+    if ((state == I2C_STATE_FREE) && (queueHead != NULL)) {
+      state = I2C_STATE_ACTIVE;
+      currentRequest = queueHead;
       rxCount = txCount = 0;
       // Copy key fields to static data for speed.
       operation = currentRequest->operation;
@@ -85,9 +84,9 @@ void I2CManagerClass::queueRequest(I2CRB *req) {
       queueHead = queueTail = req;  // Only item on queue
     else
       queueTail = queueTail->nextRequest = req; // Add to end
+    startTransaction();
   }
 
-  startTransaction();
 }
 
 /***************************************************************************
@@ -135,7 +134,7 @@ void I2CManagerClass::checkForTimeout() {
   unsigned long currentMicros = micros();
   ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
     I2CRB *t = queueHead;
-    if (t && timeout > 0) {
+    if (state==I2C_STATE_ACTIVE && t!=0 && t==currentRequest && timeout > 0) {
       // Check for timeout
       if (currentMicros - startTime > timeout) { 
         // Excessive time. Dequeue request
@@ -148,9 +147,9 @@ void I2CManagerClass::checkForTimeout() {
         // Try close and init, not entirely satisfactory but sort of works...
         I2C_close();  // Shutdown and restart twi interface
         I2C_init();
-        status = I2C_STATE_FREE;
+        state = I2C_STATE_FREE;
         
-        // Initiate next queued request
+        // Initiate next queued request if any.
         startTransaction();
       }
     }
@@ -164,8 +163,6 @@ void I2CManagerClass::loop() {
 #if !defined(I2C_USE_INTERRUPTS)
   handleInterrupt();
 #endif
-  // If free, initiate next transaction
-  startTransaction();
   checkForTimeout();
 }
 
@@ -175,27 +172,29 @@ void I2CManagerClass::loop() {
  ***************************************************************************/
 void I2CManagerClass::handleInterrupt() {
 
+  // Update hardware state machine
   I2C_handleInterrupt();
 
-  // Experimental -- perform the post processing with interrupts enabled.
-  //interrupts();
-
-  if (status!=I2C_STATUS_PENDING) {
+  // Check if current request has completed.  If there's a current request
+  // and state isn't active then state contains the completion status of the request.
+  if (state != I2C_STATE_ACTIVE && currentRequest != NULL) {
     // Remove completed request from head of queue
-    I2CRB * t;
     ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-      t = queueHead;
-      if (t != NULL) {
+      I2CRB * t = queueHead;
+      if (t == queueHead) {
         queueHead = t->nextRequest;
         if (!queueHead) queueTail = queueHead;
         t->nBytes = rxCount;
-        t->status = status;
+        t->status = state;
+        
+        // I2C state machine is now free for next request
+        currentRequest = NULL;
+        state = I2C_STATE_FREE;
+
+        // Start next request (if any)
+        I2CManager.startTransaction();
       }
-      // I2C state machine is now free for next request
-      status = I2C_STATE_FREE;
     }
-    // Start next request (if any)
-    I2CManager.startTransaction();
   }
 }
 
@@ -203,7 +202,7 @@ void I2CManagerClass::handleInterrupt() {
 I2CRB * volatile I2CManagerClass::queueHead = NULL;
 I2CRB * volatile I2CManagerClass::queueTail = NULL;
 I2CRB * volatile I2CManagerClass::currentRequest = NULL;
-volatile uint8_t I2CManagerClass::status = I2C_STATE_FREE;
+volatile uint8_t I2CManagerClass::state = I2C_STATE_FREE;
 volatile uint8_t I2CManagerClass::txCount;
 volatile uint8_t I2CManagerClass::rxCount;
 volatile uint8_t I2CManagerClass::operation;

IODevice.cpp

@@ -67,32 +67,54 @@ void IODevice::begin() {
 // doesn't need to invoke it.
 void IODevice::loop() {
   unsigned long currentMicros = micros();
-  // Call every device's loop function in turn, one per entry.
-  if (!_nextLoopDevice) _nextLoopDevice = _firstDevice;
-  _nextLoopDevice->_loop(currentMicros);
-  _nextLoopDevice = _nextLoopDevice->_nextDevice;
+  
+  IODevice *lastLoopDevice = _nextLoopDevice;  // So we know when to stop...
+  // Loop through devices until we find one ready to be serviced.
+  do {
+    if (!_nextLoopDevice) _nextLoopDevice = _firstDevice;
+    if (_nextLoopDevice) {
+      if (_nextLoopDevice->_deviceState != DEVSTATE_FAILED 
+            && ((long)(currentMicros - _nextLoopDevice->_nextEntryTime)) >= 0) {
+        // Found one ready to run, so invoke its _loop method.
+        _nextLoopDevice->_nextEntryTime = currentMicros;
+        _nextLoopDevice->_loop(currentMicros);
+        _nextLoopDevice = _nextLoopDevice->_nextDevice;
+        break;
+      }
+      // Not this one, move to next one
+      _nextLoopDevice = _nextLoopDevice->_nextDevice;
+    }
+  } while (_nextLoopDevice != lastLoopDevice); // Stop looking when we've done all.
   
   // Report loop time if diags enabled
 #if defined(DIAG_LOOPTIMES)
   static unsigned long lastMicros = 0;
-  static unsigned long maxElapsed = 0;
+  // Measure time since loop() method started.
+  unsigned long halElapsed = micros() - currentMicros;
+  // Measure time between loop() method entries.
+  unsigned long elapsed = currentMicros - lastMicros;
+  static unsigned long maxElapsed = 0, maxHalElapsed = 0;
   static unsigned long lastOutputTime = 0;
+  static unsigned long halTotal = 0, total = 0;
   static unsigned long count = 0;
   const unsigned long interval = (unsigned long)5 * 1000 * 1000; // 5 seconds in microsec
-  unsigned long elapsed = currentMicros - lastMicros;
+
   // Ignore long loop counts while message is still outputting
   if (currentMicros - lastOutputTime > 3000UL) {
     if (elapsed > maxElapsed) maxElapsed = elapsed;
+    if (halElapsed > maxHalElapsed) maxHalElapsed = halElapsed;
+    halTotal += halElapsed;
+    total += elapsed;
+    count++;
   }
-  count++;
   if (currentMicros - lastOutputTime > interval) {
     if (lastOutputTime > 0) 
-      LCD(1,F("Loop=%lus,%lus max"), interval/count, maxElapsed);
-    maxElapsed = 0;
-    count = 0;
+      DIAG(F("Loop Total:%lus (%lus max) HAL:%lus (%lus max)"), 
+        total/count, maxElapsed, halTotal/count, maxHalElapsed);
+    maxElapsed = maxHalElapsed = total = halTotal = count = 0;
     lastOutputTime = currentMicros;
   }
-  lastMicros = micros();
+  lastMicros = currentMicros;
 #endif
 }
 
@@ -112,12 +134,13 @@ bool IODevice::exists(VPIN vpin) {
 bool IODevice::hasCallback(VPIN vpin) {
   IODevice *dev = findDevice(vpin);
   if (!dev) return false;
-  return dev->_hasCallback(vpin);
+  return dev->_hasCallback;
 }
 
 // Display (to diagnostics) details of the device.
 void IODevice::_display() {
-  DIAG(F("Unknown device Vpins:%d-%d"), (int)_firstVpin, (int)_firstVpin+_nPins-1);
+  DIAG(F("Unknown device Vpins:%d-%d %S"), 
+    (int)_firstVpin, (int)_firstVpin+_nPins-1, _deviceState==DEVSTATE_FAILED ? F("OFFLINE") : F(""));
 }
 
 // Find device associated with nominated Vpin and pass configuration values on to it.
@@ -141,12 +164,18 @@ void IODevice::write(VPIN vpin, int value) {
 #endif
 }
 
-// Write analogue value to virtual pin(s).  If multiple devices are allocated the same pin
-//  then only the first one found will be used.
-void IODevice::writeAnalogue(VPIN vpin, int value, int profile) {
+// Write analogue value to virtual pin(s).  If multiple devices are allocated
+// the same pin then only the first one found will be used.
+//
+// The significance of param1 and param2 may vary from device to device.
+// For servo controllers, param1 is the profile of the transition and param2
+// the duration, i.e. the time that the operation is to be animated over
+// in deciseconds (0-3276 sec)
+//
+void IODevice::writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) {
   IODevice *dev = findDevice(vpin);
   if (dev) {
-    dev->_writeAnalogue(vpin, value, profile);
+    dev->_writeAnalogue(vpin, value, param1, param2);
     return;
   }
 #ifdef DIAG_IO
@@ -154,12 +183,13 @@ void IODevice::writeAnalogue(VPIN vpin, int value, int profile) {
 #endif
 }
 
-// isActive returns true if the device is currently in an animation of some sort, e.g. is changing
-//  the output over a period of time.
-bool IODevice::isActive(VPIN vpin) {
+// isBusy, when called for a device pin is always a digital output or analogue output,
+//  returns input feedback state of the pin, i.e. whether the pin is busy performing
+//  an animation or fade over a period of time.
+bool IODevice::isBusy(VPIN vpin) {
   IODevice *dev = findDevice(vpin);
   if (dev) 
-    return dev->_isActive(vpin);
+    return dev->_read(vpin);
   else
     return false;
 }
@@ -191,10 +221,12 @@ void IODevice::addDevice(IODevice *newDevice) {
     newDevice->_begin();
 }
 
-// Private helper function to locate a device by VPIN.  Returns NULL if not found
+// Private helper function to locate a device by VPIN.  Returns NULL if not found.
+//  This is performance-critical, so minimises the calculation and function calls necessary.
 IODevice *IODevice::findDevice(VPIN vpin) { 
   for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice) {
-    if (dev->owns(vpin)) 
+    VPIN firstVpin = dev->_firstVpin;
+    if (vpin >= firstVpin && vpin < firstVpin+dev->_nPins)
       return dev;
   }
   return NULL;
@@ -238,42 +270,49 @@ int IODevice::read(VPIN vpin) {
   return false;
 }
 
+// Read analogue value from virtual pin.
+int IODevice::readAnalogue(VPIN vpin) {
+  for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice) {
+    if (dev->owns(vpin)) 
+      return dev->_readAnalogue(vpin);
+  }
+#ifdef DIAG_IO
+  DIAG(F("IODevice::readAnalogue(): Vpin %d not found!"), (int)vpin);
+#endif
+  return false;
+}
+
 
 #else // !defined(IO_NO_HAL)
 
 // Minimal implementations of public HAL interface, to support Arduino pin I/O and nothing more.
 
 void IODevice::begin() { DIAG(F("NO HAL CONFIGURED!")); }
-bool IODevice::configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) {
-  (void)vpin; (void)paramCount; (void)params; // Avoid compiler warnings
-  if (configType == CONFIGURE_INPUT || configType == CONFIGURE_OUTPUT) 
-    return true;
-  else
-    return false;
-}
+bool IODevice::configure(VPIN, ConfigTypeEnum, int, int []) { return true; }
 void IODevice::write(VPIN vpin, int value) {
   digitalWrite(vpin, value);
   pinMode(vpin, OUTPUT);
 }
-void IODevice::writeAnalogue(VPIN vpin, int value, int profile) {
-  (void)vpin; (void)value; (void)profile; // Avoid compiler warnings
-}
-bool IODevice::hasCallback(VPIN vpin) { 
-  (void)vpin;  // Avoid compiler warnings
-  return false; 
-}
+void IODevice::writeAnalogue(VPIN, int, uint8_t, uint16_t) {}
+bool IODevice::isBusy(VPIN) { return false; }
+bool IODevice::hasCallback(VPIN) { return false; }
 int IODevice::read(VPIN vpin) { 
   pinMode(vpin, INPUT_PULLUP);
   return !digitalRead(vpin);  // Return inverted state (5v=0, 0v=1)
 }
+int IODevice::readAnalogue(VPIN vpin) {
+  pinMode(vpin, INPUT);
+  noInterrupts();
+  int value = analogRead(vpin);
+  interrupts();
+  return value;
+}
 void IODevice::loop() {}
 void IODevice::DumpAll() {
   DIAG(F("NO HAL CONFIGURED!"));
 }
-bool IODevice::exists(VPIN vpin) { return (vpin > 2 && vpin < 49); }
-void IODevice::setGPIOInterruptPin(int16_t pinNumber) {
-  (void) pinNumber; // Avoid compiler warning
-}
+bool IODevice::exists(VPIN vpin) { return (vpin > 2 && vpin < NUM_DIGITAL_PINS); }
+void IODevice::setGPIOInterruptPin(int16_t) {}
 
 // Chain of callback blocks (identifying registered callback functions for state changes)
 // Not used in IO_NO_HAL but must be declared.
@@ -288,12 +327,14 @@ IONotifyCallback *IONotifyCallback::first = 0;
 ArduinoPins::ArduinoPins(VPIN firstVpin, int nPins) {
   _firstVpin = firstVpin;
   _nPins = nPins;
-  uint8_t arrayLen = (_nPins+7)/8;
-  _pinPullups = (uint8_t *)calloc(2, arrayLen);
+  int arrayLen = (_nPins+7)/8;
+  _pinPullups = (uint8_t *)calloc(3, arrayLen);
   _pinModes = (&_pinPullups[0]) + arrayLen;
+  _pinInUse = (&_pinPullups[0]) + 2*arrayLen;
   for (int i=0; i<arrayLen; i++) {
-    _pinPullups[i] = 0;
+    _pinPullups[i] = 0xff;  // default to pullup on, for inputs
     _pinModes[i] = 0;
+    _pinInUse[i] = 0;
   }
 }
 
@@ -318,6 +359,7 @@ bool ArduinoPins::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCoun
     _pinPullups[index] &= ~mask;
     pinMode(pin, INPUT);
   }
+  _pinInUse[index] |= mask;
   return true;
 }
 
@@ -336,11 +378,35 @@ void ArduinoPins::_write(VPIN vpin, int value) {
     _pinModes[index] |= mask;
     // Since mode changes should be infrequent, use standard pinMode function
     pinMode(pin, OUTPUT);
+    _pinInUse[index] |= mask;
   }
 }
 
-// Device-specific read function.
+// Device-specific read function (digital input).
 int ArduinoPins::_read(VPIN vpin) {
+  int pin = vpin;
+  uint8_t mask = 1 << ((pin-_firstVpin) % 8);
+  uint8_t index = (pin-_firstVpin) / 8;
+  if ((_pinModes[index] | ~_pinInUse[index]) & mask) {
+    // Currently in write mode or not initialised, change to read mode
+    _pinModes[index] &= ~mask;
+    // Since mode changes should be infrequent, use standard pinMode function
+    if (_pinPullups[index] & mask) 
+      pinMode(pin, INPUT_PULLUP);
+    else
+      pinMode(pin, INPUT);
+    _pinInUse[index] |= mask;
+  }
+  int value = !fastReadDigital(pin); // Invert (5v=0, 0v=1)
+
+  #ifdef DIAG_IO
+  //DIAG(F("Arduino Read Pin:%d Value:%d"), pin, value);
+  #endif
+  return value;
+}
+
+// Device-specific readAnalogue function (analogue input)
+int ArduinoPins::_readAnalogue(VPIN vpin) {
   int pin = vpin;
   uint8_t mask = 1 << ((pin-_firstVpin) % 8);
   uint8_t index = (pin-_firstVpin) / 8;
@@ -353,7 +419,19 @@ int ArduinoPins::_read(VPIN vpin) {
     else
       pinMode(pin, INPUT);
   }
-  int value = !fastReadDigital(pin); // Invert (5v=0, 0v=1)
+
+  // Since AnalogRead is also called from interrupt code, disable interrupts 
+  // while we're using it.  There's only one ADC shared by all analogue inputs 
+  // on the Arduino, so we don't want interruptions.
+  //******************************************************************************
+  // NOTE: If the HAL is running on a computer without the DCC signal generator,
+  // then interrupts needn't be disabled.  Also, the DCC signal generator puts
+  // the ADC into fast mode, so if it isn't present, analogueRead calls will be much
+  // slower!!
+  //******************************************************************************
+  noInterrupts();
+  int value = analogRead(pin);
+  interrupts();
 
   #ifdef DIAG_IO
   //DIAG(F("Arduino Read Pin:%d Value:%d"), pin, value);

IODevice.h

@@ -42,6 +42,7 @@
 #include "DIAG.h"
 #include "FSH.h"
 #include "I2CManager.h"
+#include "inttypes.h"
 
 typedef uint16_t VPIN;
 // Limit VPIN number to max 32767.  Above this number, printing often gives negative values.
@@ -112,15 +113,27 @@ public:
   // configure is used invoke an IODevice instance's _configure method
   static bool configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]);
 
+  // User-friendly function for configuring an input pin.
+  inline static bool configureInput(VPIN vpin, bool pullupEnable) {
+    int params[] = {pullupEnable};
+    return IODevice::configure(vpin, CONFIGURE_INPUT, 1, params);
+  }
+
+  // User-friendly function for configuring a servo pin.
+  inline static bool configureServo(VPIN vpin, uint16_t activePosition, uint16_t inactivePosition, uint8_t profile=0, uint16_t duration=0, uint8_t initialState=0) {
+    int params[] = {(int)activePosition, (int)inactivePosition, profile, (int)duration, initialState};
+    return IODevice::configure(vpin, CONFIGURE_SERVO, 5, params);
+  }
+
   // write invokes the IODevice instance's _write method.
   static void write(VPIN vpin, int value);
 
   // write invokes the IODevice instance's _writeAnalogue method (not applicable for digital outputs)
-  static void writeAnalogue(VPIN vpin, int value, int profile);
+  static void writeAnalogue(VPIN vpin, int value, uint8_t profile=0, uint16_t duration=0);
 
-  // isActive returns true if the device is currently in an animation of some sort, e.g. is changing
+  // isBusy returns true if the device is currently in an animation of some sort, e.g. is changing
   //  the output over a period of time.
-  static bool isActive(VPIN vpin);
+  static bool isBusy(VPIN vpin);
 
   // check whether the pin supports notification.  If so, then regular _read calls are not required.
   static bool hasCallback(VPIN vpin);
@@ -128,6 +141,9 @@ public:
   // read invokes the IODevice instance's _read method.
   static int read(VPIN vpin);
 
+  // read invokes the IODevice instance's _readAnalogue method.
+  static int readAnalogue(VPIN vpin);
+
   // loop invokes the IODevice instance's _loop method.
   static void loop();
 
@@ -147,7 +163,14 @@ public:
   
 protected:
   
-  // Method to perform initialisation of the device (optionally implemented within device class)
+  // Constructor
+  IODevice(VPIN firstVpin=0, int nPins=0) {
+    _firstVpin = firstVpin;
+    _nPins = nPins;
+    _nextEntryTime = 0;
+  }
+
+ // Method to perform initialisation of the device (optionally implemented within device class)
   virtual void _begin() {}
 
   // Method to configure device (optionally implemented within device class)
@@ -161,41 +184,26 @@ protected:
     (void)vpin; (void)value;
   };
 
-  // Method to write an analogue value (optionally implemented within device class)
-  virtual  void _writeAnalogue(VPIN vpin, int value, int profile) {
-    (void)vpin; (void)value; (void) profile;
+  // Method to write an 'analogue' value (optionally implemented within device class)
+  virtual void _writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) {
+    (void)vpin; (void)value; (void) param1; (void)param2;
   };
 
-  // Method called from within a filter device to trigger its output (which may
-  // have the same VPIN id as the input to the filter).  It works through the 
-  // later devices in the chain only.
-  void writeDownstream(VPIN vpin, int value);
-
-  // Function called to check whether callback notification is supported by this pin.
-  //  Defaults to no, if not overridden by the device.
-  //  The same value should be returned by all pins on the device, so only one need
-  //  be checked.
-  virtual bool _hasCallback(VPIN vpin) { 
-    (void) vpin;
-    return false; 
-  }
-
-  // Method to read pin state (optionally implemented within device class)
+  // Method to read digital pin state (optionally implemented within device class)
   virtual int _read(VPIN vpin) { 
     (void)vpin; 
     return 0;
   };
 
-  // _isActive returns true if the device is currently in an animation of some sort, e.g. is changing
-  //  the output over a period of time.  Returns false unless overridden in sub class.
-  virtual bool _isActive(VPIN vpin) {
-      (void)vpin;
-      return false;
-  }
+  // Method to read analogue pin state (optionally implemented within device class)
+  virtual int _readAnalogue(VPIN vpin) { 
+    (void)vpin; 
+    return 0;
+  };
 
   // Method to perform updates on an ongoing basis (optionally implemented within device class)
   virtual void _loop(unsigned long currentMicros) {
-    (void)currentMicros; // Suppress compiler warning.
+    delayUntil(currentMicros + 0x7fffffff); // Largest time in the future!  Effectively disable _loop calls.
   };
 
   // Method for displaying info on DIAG output (optionally implemented within device class)
@@ -203,11 +211,19 @@ protected:
 
   // Destructor
   virtual ~IODevice() {};
+
+  // Non-virtual function
+  void delayUntil(unsigned long futureMicrosCount) {
+    _nextEntryTime = futureMicrosCount;
+  }
   
   // Common object fields.
   VPIN _firstVpin;
   int _nPins;
 
+  // Flag whether the device supports callbacks.
+  bool _hasCallback = false;
+
   // Pin number of interrupt pin for GPIO extender devices.  The extender module will pull this
   //  pin low if an input changes state.
   int16_t _gpioInterruptPin = -1;
@@ -225,6 +241,7 @@ private:
   static IODevice *findDevice(VPIN vpin);
 
   IODevice *_nextDevice = 0;
+  unsigned long _nextEntryTime;
   static IODevice *_firstDevice;
 
   static IODevice *_nextLoopDevice;
@@ -242,12 +259,14 @@ public:
   static void create(VPIN vpin, int nPins, uint8_t I2CAddress);
   // Constructor
   PCA9685(VPIN vpin, int nPins, uint8_t I2CAddress);
-  enum ProfileType {
-    Instant = 0,  // Moves immediately between positions
+  enum ProfileType : uint8_t {
+    Instant = 0,  // Moves immediately between positions (if duration not specified)
+    UseDuration = 0, // Use specified duration
     Fast = 1,     // Takes around 500ms end-to-end
     Medium = 2,   // 1 second end-to-end
     Slow = 3,     // 2 seconds end-to-end
-    Bounce = 4    // For semaphores/turnouts with a bit of bounce!!
+    Bounce = 4,   // For semaphores/turnouts with a bit of bounce!!
+    NoPowerOff = 0x80, // Flag to be ORed in to suppress power off after move.
   };
 
 private:
@@ -256,8 +275,8 @@ private:
   bool _configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) override;
   // Device-specific write functions.
   void _write(VPIN vpin, int value) override;
-  void _writeAnalogue(VPIN vpin, int value, int profile) override;
-  bool _isActive(VPIN vpin) override;
+  void _writeAnalogue(VPIN vpin, int value, uint8_t profile, uint16_t duration) override;
+  int _read(VPIN vpin) override; // returns the digital state or busy status of the device
   void _loop(unsigned long currentMicros) override;
   void updatePosition(uint8_t pin);
   void writeDevice(uint8_t pin, int value);
@@ -272,21 +291,18 @@ private:
     uint16_t fromPosition : 12;
     uint16_t toPosition : 12; 
     uint8_t profile;  // Config parameter
-    uint8_t stepNumber; // Index of current step (starting from 0)
-    uint8_t numSteps;  // Number of steps in animation, or 0 if none in progress.
-    int8_t state;
-  }; // 12 bytes per element, i.e. per pin in use
+    uint16_t stepNumber; // Index of current step (starting from 0)
+    uint16_t numSteps;  // Number of steps in animation, or 0 if none in progress.
+    uint8_t currentProfile; // profile being used for current animation.
+    uint16_t duration; // time (tenths of a second) for animation to complete.
+  }; // 14 bytes per element, i.e. per pin in use
   
   struct ServoData *_servoData [16];
 
-  static const uint16_t _defaultActivePosition = 410;
-  static const uint16_t _defaultInactivePosition = 205;
-
   static const uint8_t _catchupSteps = 5; // number of steps to wait before switching servo off
   static const byte FLASH _bounceProfile[30];
 
   const unsigned int refreshInterval = 50; // refresh every 50ms
-  unsigned long _lastRefreshTime; // last seen value of micros() count
 
   // structures for setting up non-blocking writes to servo controller
   I2CRB requestBlock;
@@ -335,13 +351,15 @@ private:
   bool _configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) override;
   // Device-specific write function.
   void _write(VPIN vpin, int value) override;
-  // Device-specific read function.
+  // Device-specific read functions.
   int _read(VPIN vpin) override;
+  int _readAnalogue(VPIN vpin) override;
   void _display() override;
 
 
   uint8_t *_pinPullups;
   uint8_t *_pinModes; // each bit is 1 for output, 0 for input
+  uint8_t *_pinInUse; 
 };
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////

IO_AnalogueInputs.h

@@ -0,0 +1,165 @@
+/*
+ *  © 2021, Neil McKechnie. All rights reserved.
+ *  
+ *  This file is part of DCC++EX API
+ *
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef io_analogueinputs_h
+#define io_analogueinputs_h
+
+// Uncomment following line to slow the scan cycle down to 1second ADC samples, with
+// diagnostic output of scanned values.
+//#define IO_ANALOGUE_SLOW
+
+#include "IODevice.h"
+#include "I2CManager.h"
+#include "DIAG.h"
+#include "FSH.h"
+
+/**********************************************************************************************
+ * ADS111x class for I2C-connected analogue input modules ADS1113, ADS1114 and ADS1115.
+ * 
+ * ADS1113 and ADS1114 are restricted to 1 input.  ADS1115 has a multiplexer which allows 
+ * any of four input pins to be read by its ADC.
+ * 
+ * The driver polls the device in accordance with the constant 'scanInterval' below.  On first loop
+ * entry, the multiplexer is set to pin A0 and the ADC is triggered.  On second and subsequent
+ * entries, the analogue value is read from the conversion register and then the multiplexer and
+ * ADC are set up to read the next pin.
+ * 
+ * The ADS111x is set up as follows:
+ *    Single-shot scan
+ *    Data rate 128 samples/sec (7.8ms/sample, but scanned every 10ms)
+ *    Comparator off
+ *    Gain FSR=6.144V
+ * The gain means that the maximum input voltage of 5V (when Vss=5V) gives a reading 
+ * of 32767*(5.0/6.144) = 26666.
+ * 
+ * A device is configured by the following:
+ *   ADS111x::create(firstVpin, nPins, i2cAddress);
+ * for example
+ *   ADS111x::create(300, 1, 0x48);  // single-input ADS1113
+ *   ADS111x::create(300, 4, 0x48);  // four-input ADS1115
+ * 
+ * Note: The device is simple and does not need initial configuration, so it should recover from
+ * temporary loss of communications or power.
+ **********************************************************************************************/
+class ADS111x: public IODevice { 
+public:
+  ADS111x(VPIN firstVpin, int nPins, uint8_t i2cAddress) {
+    _firstVpin = firstVpin;
+    _nPins = min(nPins,4);
+    _i2cAddress = i2cAddress;
+    _currentPin = 0;
+    for (int8_t i=0; i<_nPins; i++)
+      _value[i] = -1;
+    addDevice(this);
+  }
+  static void create(VPIN firstVpin, int nPins, uint8_t i2cAddress) {
+    new ADS111x(firstVpin, nPins, i2cAddress);
+  }
+private:
+  void _begin() {
+    // Initialise ADS device
+    if (I2CManager.exists(_i2cAddress)) {
+      _nextState = STATE_STARTSCAN;
+#ifdef DIAG_IO
+      _display();
+#endif
+    } else {
+      DIAG(F("ADS111x device not found, I2C:%x"), _i2cAddress);
+      _deviceState = DEVSTATE_FAILED;
+    }
+  }
+  void _loop(unsigned long currentMicros) override {
+
+    // Check that previous non-blocking write has completed, if not then wait
+    uint8_t status = _i2crb.status;
+    if (status == I2C_STATUS_PENDING) return;  // Busy, so don't do anything.
+    if (status == I2C_STATUS_OK) {
+      switch (_nextState) {
+        case STATE_STARTSCAN:
+          // Configure ADC and multiplexer for next scan.  See ADS111x datasheet for details
+          // of configuration register settings.
+          _outBuffer[0] = 0x01; // Config register address
+          _outBuffer[1] = 0xC0 + (_currentPin << 4); // Trigger single-shot, channel n
+          _outBuffer[2] = 0xA3;           // 250 samples/sec, comparator off
+          // Write command, without waiting for completion.
+          I2CManager.write(_i2cAddress, _outBuffer, 3, &_i2crb);
+
+          delayUntil(currentMicros + scanInterval);
+          _nextState = STATE_STARTREAD;
+          break;
+
+        case STATE_STARTREAD:
+          // Reading the pin value
+          _outBuffer[0] = 0x00;  // Conversion register address
+          I2CManager.read(_i2cAddress, _inBuffer, 2, _outBuffer, 1, &_i2crb); // Read register
+          _nextState = STATE_GETVALUE;
+          break;
+
+        case STATE_GETVALUE:
+          _value[_currentPin] = ((uint16_t)_inBuffer[0] << 8) + (uint16_t)_inBuffer[1];
+          #ifdef IO_ANALOGUE_SLOW
+          DIAG(F("ADS111x pin:%d value:%d"), _currentPin, _value[_currentPin]);
+          #endif
+
+          // Move to next pin
+          if (++_currentPin >= _nPins) _currentPin = 0;
+          _nextState = STATE_STARTSCAN;
+          break;
+        
+        default:
+          break;
+      }
+    } else { // error status
+      DIAG(F("ADS111x I2C:x%d Error:%d %S"), _i2cAddress, status, I2CManager.getErrorMessage(status));
+      _deviceState = DEVSTATE_FAILED;
+    }
+  }
+
+  int _readAnalogue(VPIN vpin) override {
+    int pin = vpin - _firstVpin;
+    return _value[pin];
+  }
+  
+  void _display() override {
+    DIAG(F("ADS111x I2C:x%x Configured on Vpins:%d-%d %S"), _i2cAddress, _firstVpin, _firstVpin+_nPins-1,
+      _deviceState == DEVSTATE_FAILED ? F("OFFLINE") : F(""));
+  }
+
+  // ADC conversion rate is 250SPS, or 4ms per conversion.  Set the period between updates to 10ms. 
+  // This is enough to allow the conversion to reliably complete in time.
+  #ifndef IO_ANALOGUE_SLOW
+  const unsigned long scanInterval = 10000UL;  // Period between successive ADC scans in microseconds.
+  #else
+  const unsigned long scanInterval = 1000000UL;  // Period between successive ADC scans in microseconds.
+  #endif
+  enum : uint8_t {
+    STATE_STARTSCAN,
+    STATE_STARTREAD, 
+    STATE_GETVALUE,
+  };
+  uint16_t _value[4];
+  uint8_t _i2cAddress;
+  uint8_t _outBuffer[3];
+  uint8_t _inBuffer[2];
+  uint8_t _currentPin;  // ADC pin currently being scanned
+  I2CRB _i2crb;
+  uint8_t _nextState;
+};
+
+#endif // io_analogueinputs_h

IO_DCCAccessory.cpp

@@ -20,49 +20,45 @@
 #include "DCC.h"
 #include "IODevice.h"
 #include "DIAG.h"
+#include "defines.h"
 
-// Note: For DCC Accessory Decoders, a particular output can be specified by 
-// a linear address, or by an address/subaddress pair, where the subaddress is
-// in the range 0 to 3 and specifies an output within a group of 4.
-// NMRA and DCC++EX accepts addresses in the range 0-511.  Linear addresses
-// are not specified by the NMRA and so different manufacturers may calculate them
-// in different ways.  DCC+EX uses a range of 1-2044 which excludes decoder address 0.
-// Therefore, I've avoided using linear addresses here because of the ambiguities 
-// involved.  Instead I've used the term 'packedAddress'.
+#define PACKEDADDRESS(addr, subaddr) (((addr) << 2)  + (subaddr))
+#define ADDRESS(packedaddr) ((packedaddr) >> 2)
+#define SUBADDRESS(packedaddr) ((packedaddr) % 4)
 
 void DCCAccessoryDecoder::create(VPIN vpin, int nPins, int DCCAddress, int DCCSubaddress) {
   new DCCAccessoryDecoder(vpin, nPins, DCCAddress, DCCSubaddress);
 }
 
-// Constructor
+// Constructors
 DCCAccessoryDecoder::DCCAccessoryDecoder(VPIN vpin, int nPins, int DCCAddress, int DCCSubaddress) {
    _firstVpin = vpin;
   _nPins = nPins;
-  _packedAddress = (DCCAddress << 2) + DCCSubaddress;
+  _packedAddress = PACKEDADDRESS(DCCAddress, DCCSubaddress);
+  addDevice(this);
 }
 
 void DCCAccessoryDecoder::_begin() {
-  int endAddress = _packedAddress + _nPins - 1;
-  int DCCAddress = _packedAddress >> 2;
-  int DCCSubaddress = _packedAddress & 3;
-  DIAG(F("DCC Accessory Decoder configured Vpins:%d-%d Linear Address:%d-%d (%d/%d-%d/%d)"), _firstVpin, _firstVpin+_nPins-1,
-      _packedAddress, _packedAddress+_nPins-1,
-     DCCAddress, DCCSubaddress, endAddress >> 2, endAddress % 4);
+#if defined(DIAG_IO)
+  _display();
+#endif
 }
 
-// Device-specific write function.
+// Device-specific write function.  State 1=closed, 0=thrown.  Adjust for RCN-213 compliance
 void DCCAccessoryDecoder::_write(VPIN id, int state) {
   int packedAddress = _packedAddress + id - _firstVpin;
   #ifdef DIAG_IO
   DIAG(F("DCC Write Linear Address:%d State:%d"), packedAddress, state);
   #endif
-  DCC::setAccessory(packedAddress >> 2, packedAddress % 4, state);
+#if !defined(DCC_ACCESSORY_RCN_213)
+  state = !state;
+#endif
+  DCC::setAccessory(ADDRESS(packedAddress), SUBADDRESS(packedAddress), state);
 }
 
 void DCCAccessoryDecoder::_display() {
   int endAddress = _packedAddress + _nPins - 1;
-  DIAG(F("DCC Accessory Vpins:%d-%d Linear Address:%d-%d (%d/%d-%d/%d)"), _firstVpin, _firstVpin+_nPins-1,
-      _packedAddress, _packedAddress+_nPins-1,
-      _packedAddress >> 2, _packedAddress % 4, endAddress >> 2, endAddress % 4);
+  DIAG(F("DCCAccessoryDecoder Configured on Vpins:%d-%d Addresses %d/%d-%d/%d)"), _firstVpin, _firstVpin+_nPins-1,
+      ADDRESS(_packedAddress), SUBADDRESS(_packedAddress), ADDRESS(endAddress), SUBADDRESS(endAddress));
 }
 

IO_DFPlayer.h

@@ -0,0 +1,255 @@
+/*
+ *  © 2021, Neil McKechnie. All rights reserved.
+ *  
+ *  This file is part of DCC++EX API
+ *
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+/*
+ * DFPlayer is an MP3 player module with an SD card holder.  It also has an integrated
+ * amplifier, so it only needs a power supply and a speaker.
+ * 
+ * This driver allows the device to be controlled through IODevice::write() and 
+ * IODevice::writeAnalogue() calls.
+ * 
+ * The driver is configured as follows:
+ * 
+ *       DFPlayer::create(firstVpin, nPins, Serialn);
+ * 
+ * Where firstVpin is the first vpin reserved for reading the device,
+ *       nPins is the number of pins to be allocated (max 5)
+ *   and Serialn is the name of the Serial port connected to the DFPlayer (e.g. Serial1).
+ * 
+ * Example:
+ *   In mySetup function within mySetup.cpp:
+ *       DFPlayer::create(3500, 5, Serial1);
+ * 
+ * Writing an analogue value 0-2999 to the first pin will select a numbered file from the SD card;
+ * Writing an analogue value 0-30 to the second pin will set the volume of the output;
+ * Writing a digital value to the first pin will play or stop the file;
+ * Reading a digital value from any pin will return true(1) if the player is playing, false(0) otherwise.
+ * 
+ * From EX-RAIL, the following commands may be used:
+ *   SET(3500)      -- starts playing the first file on the SD card
+ *   SET(3501)      -- starts playing the second file on the SD card
+ *   etc.
+ *   RESET(3500)    -- stops all playing on the player
+ *   WAITFOR(3500)  -- wait for the file currently being played by the player to complete
+ *   SERVO(3500,23,0)  -- plays file 23 at current volume
+ *   SERVO(3500,23,30)  -- plays file 23 at volume 30 (maximum)
+ *   SERVO(3501,20,0)   -- Sets the volume to 20
+ * 
+ * NB The DFPlayer's serial lines are not 5V safe, so connecting the Arduino TX directly 
+ * to the DFPlayer's RX terminal will cause lots of noise over the speaker, or worse.
+ * A 1k resistor in series with the module's RX terminal will alleviate this.
+ */
+
+#ifndef IO_DFPlayer_h
+#define IO_DFPlayer_h
+
+#include "IODevice.h"
+
+class DFPlayer : public IODevice {
+private: 
+  HardwareSerial *_serial;
+  bool _playing = false;
+  uint8_t _inputIndex = 0;
+  unsigned long _commandSendTime; // Allows timeout processing
+
+public:
+  // Constructor
+  DFPlayer(VPIN firstVpin, int nPins, HardwareSerial &serial) :
+    IODevice(firstVpin, nPins),
+    _serial(&serial) 
+  {
+    addDevice(this);
+  }
+
+  static void create(VPIN firstVpin, int nPins, HardwareSerial &serial) {
+    new DFPlayer(firstVpin, nPins, serial);
+  }
+
+protected:
+  void _begin() override {
+    _serial->begin(9600);
+    _deviceState = DEVSTATE_INITIALISING;
+
+    // Send a query to the device to see if it responds
+    sendPacket(0x42); 
+    _commandSendTime = micros();
+  }
+
+  void _loop(unsigned long currentMicros) override {
+    // Check for incoming data on _serial, and update busy flag accordingly.
+    // Expected message is in the form "7F FF 06 3D xx xx xx xx xx EF"
+    while (_serial->available()) {
+      int c = _serial->read();
+      if (c == 0x7E) 
+        _inputIndex = 1;
+      else if ((c==0xFF && _inputIndex==1)
+            || (c==0x3D && _inputIndex==3) 
+            || (_inputIndex >=4 && _inputIndex <= 8))
+        _inputIndex++;
+      else if (c==0x06 && _inputIndex==2) { 
+        // Valid message prefix, so consider the device online
+        if (_deviceState==DEVSTATE_INITIALISING) {
+          _deviceState = DEVSTATE_NORMAL;
+          #ifdef DIAG_IO
+          _display();
+          #endif
+        }
+        _inputIndex++;
+      } else if (c==0xEF && _inputIndex==9) {
+        // End of play
+        if (_playing) {
+          #ifdef DIAG_IO
+          DIAG(F("DFPlayer: Finished"));
+          #endif
+          _playing = false;
+        }
+        _inputIndex = 0;
+      } else 
+        _inputIndex = 0;  // Unrecognised character sequence, start again!
+    }
+    // Check if the initial prompt to device has timed out.  Allow 1 second
+    if (_deviceState == DEVSTATE_INITIALISING && currentMicros - _commandSendTime > 1000000UL) {
+      DIAG(F("DFPlayer device not responding on serial port"));
+      _deviceState = DEVSTATE_FAILED;
+    }
+    delayUntil(currentMicros + 10000); // Only enter every 10ms
+  }
+
+  // Write with value 1 starts playing a song.  The relative pin number is the file number.
+  // Write with value 0 stops playing.
+  void _write(VPIN vpin, int value) override {
+    int pin = vpin - _firstVpin;
+    if (value) {
+      // Value 1, start playing
+      #ifdef DIAG_IO
+      DIAG(F("DFPlayer: Play %d"), pin+1);
+      #endif
+      sendPacket(0x03, pin+1);
+      _playing = true;
+    } else {
+      // Value 0, stop playing
+      #ifdef DIAG_IO
+      DIAG(F("DFPlayer: Stop"));
+      #endif
+      sendPacket(0x16);
+      _playing = false;
+    }
+  }
+
+  // WriteAnalogue on first pin uses the nominated value as a file number to start playing, if file number > 0.
+  // Volume may be specified as second parameter to writeAnalogue.
+  // If value is zero, the player stops playing.  
+  // WriteAnalogue on second pin sets the output volume.
+  void _writeAnalogue(VPIN vpin, int value, uint8_t volume=0, uint16_t=0) override { 
+    uint8_t pin = vpin - _firstVpin;
+ 
+    // Validate parameter.
+    volume = min(30,volume);
+
+    if (pin == 0) {
+      // Play track 
+      if (value > 0) {
+        #ifdef DIAG_IO
+        DIAG(F("DFPlayer: Play %d"), value);
+        #endif
+        sendPacket(0x03, value); // Play track
+        _playing = true;
+        if (volume > 0) {
+          #ifdef DIAG_IO
+          DIAG(F("DFPlayer: Volume %d"), volume);
+          #endif
+          sendPacket(0x06, volume);  // Set volume
+        }
+      } else {
+        #ifdef DIAG_IO
+        DIAG(F("DFPlayer: Stop"));
+        #endif
+        sendPacket(0x16); // Stop play
+        _playing = false;
+      }
+    } else if (pin == 1) {
+      // Set volume (0-30)
+      if (value > 30) value = 30;
+      else if (value < 0) value = 0;
+      #ifdef DIAG_IO
+      DIAG(F("DFPlayer: Volume %d"), value);
+      #endif
+      sendPacket(0x06, value);      
+    }
+  }
+
+  // A read on any pin indicates whether the player is still playing.
+  int _read(VPIN) override {
+    return _playing;
+  }
+
+  void _display() override {
+    DIAG(F("DFPlayer Configured on Vpins:%d-%d %S"), _firstVpin, _firstVpin+_nPins-1,
+      (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
+  }
+  
+private:
+  // 7E FF 06 0F 00 01 01 xx xx EF
+  // 0	->	7E is start code
+  // 1	->	FF is version
+  // 2	->	06 is length
+  // 3	->	0F is command
+  // 4	->	00 is no receive
+  // 5~6	->	01 01 is argument
+  // 7~8	->	checksum = 0 - ( FF+06+0F+00+01+01 )
+  // 9	->	EF is end code
+
+  void sendPacket(uint8_t command, uint16_t arg = 0)
+  {
+    uint8_t out[] = { 0x7E,
+        0xFF,
+        06,
+        command,
+        00,
+        static_cast<uint8_t>(arg >> 8),
+        static_cast<uint8_t>(arg & 0x00ff),
+        00,
+        00,
+        0xEF };
+
+    setChecksum(out);
+
+    _serial->write(out, sizeof(out));
+  }
+
+  uint16_t calcChecksum(uint8_t* packet)
+  {
+    uint16_t sum = 0;
+    for (int i = 1; i < 7; i++)
+    {
+      sum += packet[i];
+    }
+    return -sum;
+  }
+
+  void setChecksum(uint8_t* out)
+  {
+    uint16_t sum = calcChecksum(out);
+
+    out[7] = (sum >> 8);
+    out[8] = (sum & 0xff);
+  }
+};
+
+#endif // IO_DFPlayer_h

IO_ExampleSerial.cpp

@@ -42,7 +42,9 @@ void IO_ExampleSerial::create(VPIN firstVpin, int nPins, HardwareSerial *serial,
 // Device-specific initialisation
 void IO_ExampleSerial::_begin() {
   _serial->begin(_baud);
-  DIAG(F("ExampleSerial configured Vpins:%d-%d"), _firstVpin, _firstVpin+_nPins-1);
+#if defined(DIAG_IO)
+  _display();
+#endif
 
   // Send a few # characters to the output
   for (uint8_t i=0; i<3; i++)
@@ -121,7 +123,7 @@ void IO_ExampleSerial::_loop(unsigned long currentMicros) {
 }
 
 void IO_ExampleSerial::_display() {
-  DIAG(F("IO_ExampleSerial VPins:%d-%d"), (int)_firstVpin, 
+  DIAG(F("IO_ExampleSerial Configured on VPins:%d-%d"), (int)_firstVpin, 
     (int)_firstVpin+_nPins-1);
 }
 

IO_GPIOBase.h

@@ -45,10 +45,6 @@ protected:
   int _read(VPIN vpin) override;
   void _display() override;
   void _loop(unsigned long currentMicros) override;
-  bool _hasCallback(VPIN vpin) {
-    (void)vpin;   // suppress compiler warning
-    return true;  // Enable callback if caller wants to use it.
-  }
 
   // Data fields
   uint8_t _I2CAddress; 
@@ -57,9 +53,9 @@ protected:
   T _portOutputState;
   T _portMode;
   T _portPullup;
+  T _portInUse;
   // Interval between refreshes of each input port
   static const int _portTickTime = 4000;
-  unsigned long _lastLoopEntry = 0;
 
   // Virtual functions for interfacing with I2C GPIO Device
   virtual void _writeGpioPort() = 0;
@@ -80,12 +76,13 @@ protected:
 
 // Constructor
 template <class T>
-GPIOBase<T>::GPIOBase(FSH *deviceName, VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin) {
+GPIOBase<T>::GPIOBase(FSH *deviceName, VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin) :
+  IODevice(firstVpin, nPins)
+{
   _deviceName = deviceName;
-  _firstVpin = firstVpin;
-  _nPins = nPins;
   _I2CAddress = I2CAddress;
   _gpioInterruptPin = interruptPin;
+  _hasCallback = true;
   // Add device to list of devices.
   addDevice(this);
 }
@@ -99,14 +96,19 @@ void GPIOBase<T>::_begin() {
   I2CManager.begin();
   I2CManager.setClock(400000);
   if (I2CManager.exists(_I2CAddress)) {
+#if defined(DIAG_IO)
     _display();
+#endif
     _portMode = 0;  // default to input mode
     _portPullup = -1; // default to pullup enabled
-    _portInputState = -1; 
+    _portInputState = -1;
+    _portInUse = 0;
+    _setupDevice();
+    _deviceState = DEVSTATE_NORMAL;
+  } else {
+    DIAG(F("%S I2C:x%x Device not detected"), _deviceName, _I2CAddress);
+    _deviceState = DEVSTATE_FAILED;
   }
-  _setupDevice();
-  _deviceState = DEVSTATE_NORMAL;
-  _lastLoopEntry = micros();
 }
 
 // Configuration parameters for inputs: 
@@ -126,11 +128,15 @@ bool GPIOBase<T>::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCoun
     _portPullup |= mask;
   else
     _portPullup &= ~mask;
+  // Mark that port has been accessed
+  _portInUse |= mask;
+  // Set input mode
+  _portMode &= ~mask;
 
   // Call subclass's virtual function to write to device
+  _writePortModes();
   _writePullups();
-  // Re-read port following change
-  _readGpioPort();
+  // Port change will be notified on next loop entry.
 
   return true;
 }
@@ -145,9 +151,12 @@ void GPIOBase<T>::_loop(unsigned long currentMicros) {
       _deviceState = DEVSTATE_NORMAL;
     } else {
       _deviceState = DEVSTATE_FAILED;
-      DIAG(F("%S I2C:x%x Error:%d"), _deviceName, _I2CAddress, status);
+      DIAG(F("%S I2C:x%x Error:%d %S"), _deviceName, _I2CAddress, status, 
+        I2CManager.getErrorMessage(status));
     }
     _processCompletion(status);
+  // Set unused pin and write mode pin value to 1
+    _portInputState |= ~_portInUse | _portMode;
 
     // Scan for changes in input states and invoke callback (if present)
     T differences = lastPortStates ^ _portInputState;
@@ -169,27 +178,25 @@ void GPIOBase<T>::_loop(unsigned long currentMicros) {
     #endif
   }
 
-  // Check if interrupt configured.  If so, and pin is not pulled down, finish.
-  if (_gpioInterruptPin >= 0) {
-    if (digitalRead(_gpioInterruptPin)) return;
-  } else
-  // No interrupt pin.  Check if tick has elapsed.  If not, finish.
-  if (currentMicros - _lastLoopEntry < _portTickTime) return;
+  // Check if interrupt configured.  If not, or if it is active (pulled down), then
+  //  initiate a scan.
+  if (_gpioInterruptPin < 0 || !digitalRead(_gpioInterruptPin)) {
+    // TODO: Could suppress reads if there are no pins configured as inputs!
 
-  // TODO: Could suppress reads if there are no pins configured as inputs!
-
-  // Read input
-  _lastLoopEntry = currentMicros;
-  if (_deviceState == DEVSTATE_NORMAL) {
-    _readGpioPort(false);  // Initiate non-blocking read
-    _deviceState= DEVSTATE_SCANNING;
+    // Read input
+    if (_deviceState == DEVSTATE_NORMAL) {
+      _readGpioPort(false);  // Initiate non-blocking read
+      _deviceState= DEVSTATE_SCANNING;
+    }
   }
+  // Delay next entry until tick elapsed.
+  delayUntil(currentMicros + _portTickTime);
 }
 
 template <class T>
 void GPIOBase<T>::_display() {
-  DIAG(F("%S I2C:x%x Configured on Vpins:%d-%d"), _deviceName, _I2CAddress, 
-    _firstVpin, _firstVpin+_nPins-1);
+  DIAG(F("%S I2C:x%x Configured on Vpins:%d-%d %S"), _deviceName, _I2CAddress, 
+    _firstVpin, _firstVpin+_nPins-1, (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
 }
 
 template <class T>
@@ -200,8 +207,9 @@ void GPIOBase<T>::_write(VPIN vpin, int value) {
   DIAG(F("%S I2C:x%x Write Pin:%d Val:%d"), _deviceName, _I2CAddress, pin, value);
   #endif
 
-  // Set port mode output
+  // Set port mode output if currently not output mode
   if (!(_portMode & mask)) {
+    _portInUse |= mask;
     _portMode |= mask;
     _writePortModes();
   }
@@ -221,12 +229,16 @@ int GPIOBase<T>::_read(VPIN vpin) {
   int pin = vpin - _firstVpin;
   T mask = 1 << pin;
 
-  // Set port mode to input
-  if (_portMode & mask) {
+  // Set port mode to input if currently output or first use
+  if ((_portMode | ~_portInUse) & mask) {
     _portMode &= ~mask;
+    _portInUse |= mask;
+    _writePullups();
     _writePortModes();
     // Port won't have been read yet, so read it now.
     _readGpioPort();
+  // Set unused pin and write mode pin value to 1
+    _portInputState |= ~_portInUse | _portMode;
     #ifdef DIAG_IO
     DIAG(F("%S I2C:x%x PortStates:%x"), _deviceName, _I2CAddress, _portInputState);
     #endif

IO_HCSR04.h

@@ -17,31 +17,37 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-/* 
+/*
  * The HC-SR04 module has an ultrasonic transmitter (40kHz) and a receiver.
- * It is operated through two signal pins.  When the transmit pin is set to 1 for 
- * 10us, on the falling edge the transmitter sends a short transmission of 
+ * It is operated through two signal pins.  When the transmit pin is set to 1
+ * for 10us, on the falling edge the transmitter sends a short transmission of
  * 8 pulses (like a sonar 'ping').  This is reflected off objects and received
  * by the receiver.  A pulse is sent on the receive pin whose length is equal
  * to the delay between the transmission of the pulse and the detection of
  * its echo.  The distance of the reflecting object is calculated by halving
  * the time (to allow for the out and back distance), then multiplying by the
  * speed of sound (assumed to be constant).
- * 
+ *
  * This driver polls the HC-SR04 by sending the trigger pulse and then measuring
- * the length of the received pulse.  If the calculated distance is less than the
- * threshold, the output changes to 1.  If it is greater than the threshold plus
- * a hysteresis margin, the output changes to 0.
- * 
- * The measurement would be more reliable if interrupts were disabled while the
- * pulse is being timed.  However, this would affect other functions in the CS
- * so the measurement is being performed with interrupts enabled.  Also, we could
- * use an interrupt pin in the Arduino for the timing, but the same consideration 
- * applies.
- * 
- * Note: The timing accuracy required by this means that the pins have to be 
- * direct Arduino pins; GPIO pins on an IO Extender cannot provide the required
- * accuracy.
+ * the length of the received pulse.  If the calculated distance is less than
+ * the threshold, the output state returned by a read() call changes to 1.  If
+ * the distance is greater than the threshold plus a hysteresis margin, the
+ * output changes to 0. The device also supports readAnalogue(), which returns
+ * the measured distance in cm, or 32767 if the distance exceeds the
+ * offThreshold.
+ *
+ * It might be thought that the measurement would be more reliable if interrupts
+ * were disabled while the pulse is being timed.  However, this would affect
+ * other functions in the CS so the measurement is being performed with
+ * interrupts enabled.  Also, we could use an interrupt pin in the Arduino for
+ * the timing, but the same consideration applies.  In any case, the DCC
+ * interrupt occurs once every 58us, so any IRC code is much faster than that.
+ * And 58us corresponds to 1cm in the calculation, so the effect of
+ * interrupts is negligible.
+ *
+ * Note: The timing accuracy required for measuring the pulse length means that
+ * the pins have to be direct Arduino pins; GPIO pins on an IO Extender cannot
+ * provide the required accuracy.
  */
 
 #ifndef IO_HCSR04_H
@@ -53,45 +59,45 @@ class HCSR04 : public IODevice {
 
 private:
   // pins must be arduino GPIO pins, not extender pins or HAL pins.
-  int _transmitPin = -1;
-  int _receivePin = -1;
+  int _trigPin = -1;
+  int _echoPin = -1;
   // Thresholds for setting active state in cm.
   uint8_t _onThreshold;  // cm
   uint8_t _offThreshold; // cm
+  // Last measured distance in cm.
+  uint16_t _distance;
   // Active=1/inactive=0 state 
   uint8_t _value = 0;
-  // Time of last loop execution
-  unsigned long _lastExecutionTime;
   // Factor for calculating the distance (cm) from echo time (ms).
   //  Based on a speed of sound of 345 metres/second.
   const uint16_t factor = 58; // ms/cm
 
 public:
   // Constructor perfroms static initialisation of the device object
-  HCSR04 (VPIN vpin, int transmitPin, int receivePin, uint16_t onThreshold, uint16_t offThreshold) {
+  HCSR04 (VPIN vpin, int trigPin, int echoPin, uint16_t onThreshold, uint16_t offThreshold) {
     _firstVpin = vpin;
     _nPins = 1;
-    _transmitPin = transmitPin;
-    _receivePin = receivePin;
+    _trigPin = trigPin;
+    _echoPin = echoPin;
     _onThreshold = onThreshold;
     _offThreshold = offThreshold;
     addDevice(this);
   }
 
   // Static create function provides alternative way to create object
-  static void create(VPIN vpin, int transmitPin, int receivePin, uint16_t onThreshold, uint16_t offThreshold) {
-    new HCSR04(vpin, transmitPin, receivePin, onThreshold, offThreshold);
+  static void create(VPIN vpin, int trigPin, int echoPin, uint16_t onThreshold, uint16_t offThreshold) {
+    new HCSR04(vpin, trigPin, echoPin, onThreshold, offThreshold);
   }
 
 protected:
   // _begin function called to perform dynamic initialisation of the device
   void _begin() override {
-    pinMode(_transmitPin, OUTPUT);
-    pinMode(_receivePin, INPUT);
-    ArduinoPins::fastWriteDigital(_transmitPin, 0);
-    _lastExecutionTime = micros();
-    DIAG(F("HCSR04 configured on VPIN:%d TXpin:%d RXpin:%d On:%dcm Off:%dcm"),
-      _firstVpin, _transmitPin, _receivePin, _onThreshold, _offThreshold);
+    pinMode(_trigPin, OUTPUT);
+    pinMode(_echoPin, INPUT);
+    ArduinoPins::fastWriteDigital(_trigPin, 0);
+#if defined(DIAG_IO)
+    _display();
+#endif
   }
 
   // _read function - just return _value (calculated in _loop).
@@ -100,13 +106,21 @@ protected:
     return _value;
   }
 
+  int _readAnalogue(VPIN vpin) override {
+    (void)vpin; // avoid compiler warning
+    return _distance;
+  }
+
   // _loop function - read HC-SR04 once every 50 milliseconds.
   void _loop(unsigned long currentMicros) override {
-    if (currentMicros - _lastExecutionTime > 50000) {
-      _lastExecutionTime = currentMicros;
+    read_HCSR04device();
+    // Delay next loop entry until 50ms have elapsed.
+    delayUntil(currentMicros + 50000UL);
+  }
 
-      _value = read_HCSR04device();
-    }
+  void _display() override {
+    DIAG(F("HCSR04 Configured on Vpin:%d TrigPin:%d EchoPin:%d On:%dcm Off:%dcm"),
+      _firstVpin, _trigPin, _echoPin, _onThreshold, _offThreshold);
   }
 
 private:
@@ -121,51 +135,52 @@ private:
   //  measured distance is less than the onThreshold, and is set to 0 if the measured distance is
   //  greater than the offThreshold.
   //
-  uint8_t read_HCSR04device() {
+  void read_HCSR04device() {
     // uint16 enough to time up to 65ms
     uint16_t startTime, waitTime, currentTime, maxTime;  
 
     // If receive pin is still set on from previous call, abort the read.
-    if (ArduinoPins::fastReadDigital(_receivePin)) return _value;
+    if (ArduinoPins::fastReadDigital(_echoPin))
+      return;
 
     // Send 10us pulse to trigger transmitter
-    ArduinoPins::fastWriteDigital(_transmitPin, 1);
+    ArduinoPins::fastWriteDigital(_trigPin, 1);
     delayMicroseconds(10);
-    ArduinoPins::fastWriteDigital(_transmitPin, 0);
+    ArduinoPins::fastWriteDigital(_trigPin, 0);
 
     // Wait for receive pin to be set
     startTime = currentTime = micros();
     maxTime = factor * _offThreshold * 2;
-    while (!ArduinoPins::fastReadDigital(_receivePin)) {
+    while (!ArduinoPins::fastReadDigital(_echoPin)) {
       // lastTime = currentTime;
       currentTime = micros();
       waitTime = currentTime - startTime;
       if (waitTime > maxTime) {
         // Timeout waiting for pulse start, abort the read
-        return _value;
+        return;
       }
     }
 
     // Wait for receive pin to reset, and measure length of pulse
     startTime = currentTime = micros();
     maxTime = factor * _offThreshold;
-    while (ArduinoPins::fastReadDigital(_receivePin)) {
+    while (ArduinoPins::fastReadDigital(_echoPin)) {
       currentTime = micros();
       waitTime = currentTime - startTime;
       // If pulse is too long then set return value to zero,
       //  and finish without waiting for end of pulse.
       if (waitTime > maxTime) {
         // Pulse length longer than maxTime, reset value.
-        return 0;
+        _value = 0;
+        _distance = 32767;
+        return;
       }
     } 
     // Check if pulse length is below threshold, if so set value.
     //DIAG(F("HCSR04: Pulse Len=%l Distance=%d"), waitTime, distance);
-    uint16_t distance = waitTime / factor; // in centimetres
-    if (distance < _onThreshold) 
-      return 1;
-
-    return _value;
+    _distance = waitTime / factor; // in centimetres
+    if (_distance < _onThreshold) 
+      _value = 1;
   }
 
 };

IO_MCP23008.h

@@ -42,14 +42,18 @@ private:
     I2CManager.write(_I2CAddress, 2, REG_GPIO, _portOutputState);
   }
   void _writePullups() override {
-    I2CManager.write(_I2CAddress, 2, REG_GPPU, _portPullup);  
+    // Set pullups only for in-use pins.  This prevents pullup being set for a pin that
+    //  is intended for use as an output but hasn't been written to yet.
+    I2CManager.write(_I2CAddress, 2, REG_GPPU, _portPullup & _portInUse);  
   }
   void _writePortModes() override {
-    // Each bit is 1 for an input, 0 for an output, i.e. inverted.
-    I2CManager.write(_I2CAddress, 2, REG_IODIR, ~_portMode);
-    // Enable interrupt-on-change for pins that are inputs (_portMode=0)
+    // Write 0 to IODIR for in-use pins that are outputs, 1 for others.
+    uint8_t temp = ~(_portMode & _portInUse);
+    I2CManager.write(_I2CAddress, 2, REG_IODIR, temp);
+    // Enable interrupt-on-change for in-use pins that are inputs (_portMode=0)
+    temp = ~_portMode & _portInUse;
     I2CManager.write(_I2CAddress, 2, REG_INTCON, 0x00);
-    I2CManager.write(_I2CAddress, 2, REG_GPINTEN, ~_portMode);
+    I2CManager.write(_I2CAddress, 2, REG_GPINTEN, temp);
   }
   void _readGpioPort(bool immediate) override {
     if (immediate) {

IO_MCP23017.h

@@ -48,14 +48,19 @@ private:
     I2CManager.write(_I2CAddress, 3, REG_GPIOA, _portOutputState, _portOutputState>>8);
   }
   void _writePullups() override {
-    I2CManager.write(_I2CAddress, 3, REG_GPPUA, _portPullup, _portPullup>>8);  
+    // Set pullups only for in-use pins.  This prevents pullup being set for a pin that
+    //  is intended for use as an output but hasn't been written to yet.
+    uint16_t temp = _portPullup & _portInUse;
+    I2CManager.write(_I2CAddress, 3, REG_GPPUA, temp, temp>>8);  
   }
   void _writePortModes() override {
-    // Write 1 to IODIR for pins that are inputs, 0 for outputs (i.e. _portMode inverted)
-    I2CManager.write(_I2CAddress, 3, REG_IODIRA, ~_portMode, (~_portMode)>>8);
-    // Enable interrupt for those pins which are inputs (_portMode=0)
+    // Write 0 to IODIR for in-use pins that are outputs, 1 for others.
+    uint16_t temp = ~(_portMode & _portInUse);
+    I2CManager.write(_I2CAddress, 3, REG_IODIRA, temp, temp>>8);
+    // Enable interrupt for in-use pins which are inputs (_portMode=0)
+    temp = ~_portMode & _portInUse;
     I2CManager.write(_I2CAddress, 3, REG_INTCONA, 0x00, 0x00);
-    I2CManager.write(_I2CAddress, 3, REG_GPINTENA, ~_portMode, (~_portMode)>>8);
+    I2CManager.write(_I2CAddress, 3, REG_GPINTENA, temp, temp>>8);
   }
   void _readGpioPort(bool immediate) override {
     if (immediate) {

IO_PCA9685.cpp

@@ -45,28 +45,30 @@ void PCA9685::create(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
 // Configure a port on the PCA9685.
 bool PCA9685::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) {
   if (configType != CONFIGURE_SERVO) return false;
-  if (paramCount != 4) return false;
+  if (paramCount != 5) return false;
   #ifdef DIAG_IO
-  DIAG(F("PCA9685 Configure VPIN:%d Apos:%d Ipos:%d Profile:%d state:%d"), 
-    vpin, params[0], params[1], params[2], params[3]);
+  DIAG(F("PCA9685 Configure VPIN:%d Apos:%d Ipos:%d Profile:%d Duration:%d state:%d"), 
+    vpin, params[0], params[1], params[2], params[3], params[4]);
   #endif
 
   int8_t pin = vpin - _firstVpin;
   struct ServoData *s = _servoData[pin];
-  if (!s) { 
+  if (s == NULL) { 
     _servoData[pin] = (struct ServoData *)calloc(1, sizeof(struct ServoData));
     s = _servoData[pin];
     if (!s) return false; // Check for failed memory allocation
   }
 
   s->activePosition = params[0];
-  s->currentPosition = s->inactivePosition = params[1];
+  s->inactivePosition = params[1];
   s->profile = params[2];
+  s->duration = params[3];
+  int state = params[4];
 
-  // Position servo to initial state
-  s->state = -1; // Set unknown state, to force reposition
-  _write(vpin, params[3]);
-
+  if (state != -1) {
+    // Position servo to initial state
+    _writeAnalogue(vpin, state ? s->activePosition : s->inactivePosition, 0, 0);
+  } 
   return true;
 }
 
@@ -75,6 +77,8 @@ PCA9685::PCA9685(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
   _firstVpin = firstVpin;
   _nPins = min(nPins, 16);
   _I2CAddress = I2CAddress;
+  // To save RAM, space for servo configuration is not allocated unless a pin is used.
+  // Initialise the pointers to NULL.
   for (int i=0; i<_nPins; i++)
     _servoData[i] = NULL;
 
@@ -93,7 +97,6 @@ void PCA9685::_begin() {
 
   // Initialise I/O module here.
   if (I2CManager.exists(_I2CAddress)) {
-    DIAG(F("PCA9685 I2C:%x configured Vpins:%d-%d"), _I2CAddress, _firstVpin, _firstVpin+_nPins-1);
     writeRegister(_I2CAddress, PCA9685_MODE1, MODE1_SLEEP | MODE1_AI);    
     writeRegister(_I2CAddress, PCA9685_PRESCALE, PRESCALE_50HZ);   // 50Hz clock, 20ms pulse period.
     writeRegister(_I2CAddress, PCA9685_MODE1, MODE1_AI);
@@ -101,11 +104,17 @@ void PCA9685::_begin() {
     // In theory, we should wait 500us before sending any other commands to each device, to allow
     // the PWM oscillator to get running.  However, we don't do any specific wait, as there's 
     // plenty of other stuff to do before we will send a command.
-  }
+  #if defined(DIAG_IO)
+    _display();
+  #endif
+  } else
+    _deviceState = DEVSTATE_FAILED;
 }
 
-// Device-specific write function, invoked from IODevice::write().
+// Device-specific write function, invoked from IODevice::write().  
+// For this function, the configured profile is used.
 void PCA9685::_write(VPIN vpin, int value) {
+  if (_deviceState == DEVSTATE_FAILED) return;
   #ifdef DIAG_IO
   DIAG(F("PCA9685 Write Vpin:%d Value:%d"), vpin, value);
   #endif
@@ -113,97 +122,93 @@ void PCA9685::_write(VPIN vpin, int value) {
   if (value) value = 1;
 
   struct ServoData *s = _servoData[pin];
-  if (!s) {
-    // Pin not configured, just write default positions to servo controller
-    if (value) 
-      writeDevice(pin, _defaultActivePosition);
-    else 
-      writeDevice(pin, _defaultInactivePosition);
-  } else {
-    // Use configured parameters for advanced transitions
-    uint8_t profile = s->profile;
-    // If current position not known, go straight to selected position.
-    if (s->state == -1) profile = Instant;
-
-    // Animated profile.  Initiate the appropriate action.
-    s->numSteps = profile==Fast ? 10 : 
-                  profile==Medium ? 20 : 
-                  profile==Slow ? 40 : 
-                  profile==Bounce ? sizeof(_bounceProfile)-1 : 
-                  1;
-    s->state = value;
-    s->stepNumber = 0;
-
-    // Update new from/to positions to initiate or change animation.
-    s->fromPosition = s->currentPosition;
-    s->toPosition = s->state ? s->activePosition : s->inactivePosition;
-  }
+  if (s != NULL) {
+    // Use configured parameters
+    _writeAnalogue(vpin, value ? s->activePosition : s->inactivePosition, s->profile, s->duration);
+  } // else { /* ignorethe request */ }
 }
 
 // Device-specific writeAnalogue function, invoked from IODevice::writeAnalogue().
-void PCA9685::_writeAnalogue(VPIN vpin, int value, int profile) {
+// Profile is as follows:
+//  Bit 7:     0=Set PWM to 0% to power off servo motor when finished
+//             1=Keep PWM pulses on (better when using PWM to drive an LED)
+//  Bits 6-0:  0           Use specified duration (defaults to 0 deciseconds)
+//             1 (Fast)    Move servo in 0.5 seconds
+//             2 (Medium)  Move servo in 1.0 seconds
+//             3 (Slow)    Move servo in 2.0 seconds
+//             4 (Bounce)  Servo 'bounces' at extremes.
+//            
+void PCA9685::_writeAnalogue(VPIN vpin, int value, uint8_t profile, uint16_t duration) {
+  if (_deviceState == DEVSTATE_FAILED) return;
   #ifdef DIAG_IO
-  DIAG(F("PCA9685 WriteAnalogue Vpin:%d Value:%d Profile:%d"), vpin, value, profile);
+  DIAG(F("PCA9685 WriteAnalogue Vpin:%d Value:%d Profile:%d Duration:%d"), 
+    vpin, value, profile, duration);
   #endif
   int pin = vpin - _firstVpin;
   if (value > 4095) value = 4095;
   else if (value < 0) value = 0;
 
   struct ServoData *s = _servoData[pin];
-
-  if (!s) {
-    // Servo pin not configured, so configure now.
+  if (s == NULL) {
+    // Servo pin not configured, so configure now using defaults
     s = _servoData[pin] = (struct ServoData *) calloc(sizeof(struct ServoData), 1);
-    s->activePosition = _defaultActivePosition;
-    s->inactivePosition = _defaultInactivePosition;
-    s->currentPosition = value; // Don't know where we're moving from.
+    if (s == NULL) return;  // Check for memory allocation failure
+    s->activePosition = 0;
+    s->inactivePosition = 0;
+    s->currentPosition = value;
+    s->profile = Instant;  // Use instant profile (but not this time)
   }
-  s->profile = profile;
+
   // Animated profile.  Initiate the appropriate action.
-  s->numSteps = profile==Fast ? 10 : 
-                profile==Medium ? 20 : 
-                profile==Slow ? 40 : 
-                profile==Bounce ? sizeof(_bounceProfile)-1 : 
-                1;
+  s->currentProfile = profile;
+  uint8_t profileValue = profile & ~NoPowerOff;  // Mask off 'don't-power-off' bit.
+  s->numSteps = profileValue==Fast ? 10 :   // 0.5 seconds
+                profileValue==Medium ? 20 : // 1.0 seconds
+                profileValue==Slow ? 40 :   // 2.0 seconds
+                profileValue==Bounce ? sizeof(_bounceProfile)-1 : // ~ 1.5 seconds
+                duration * 2 + 1; // Convert from deciseconds (100ms) to refresh cycles (50ms)
   s->stepNumber = 0;
   s->toPosition = value;
   s->fromPosition = s->currentPosition;
 }
 
-// _isActive returns true if the device is currently in executing an animation, 
+// _read returns true if the device is currently in executing an animation, 
 //  changing the output over a period of time.
-bool PCA9685::_isActive(VPIN vpin) {
+int PCA9685::_read(VPIN vpin) {
+  if (_deviceState == DEVSTATE_FAILED) return 0;
   int pin = vpin - _firstVpin;
   struct ServoData *s = _servoData[pin];
-  if (!s) 
+  if (s == NULL) 
     return false; // No structure means no animation!
   else
-    return (s->numSteps != 0);
+    return (s->stepNumber < s->numSteps);
 }
 
 void PCA9685::_loop(unsigned long currentMicros) {
-  if (currentMicros - _lastRefreshTime >= refreshInterval * 1000) {
-    for (int pin=0; pin<_nPins; pin++) {
-      updatePosition(pin);
-    }
-    _lastRefreshTime = currentMicros;
+  for (int pin=0; pin<_nPins; pin++) {
+    updatePosition(pin);
   }
+  delayUntil(currentMicros + refreshInterval * 1000UL);
 }
 
 // Private function to reposition servo
 // TODO: Could calculate step number from elapsed time, to allow for erratic loop timing.
 void PCA9685::updatePosition(uint8_t pin) {
   struct ServoData *s = _servoData[pin];
-  if (!s) return;
+  
+  if (s == NULL) return; // No pin configuration/state data
+
   if (s->numSteps == 0) return; // No animation in progress
+
   if (s->stepNumber == 0 && s->fromPosition == s->toPosition) {
-    // No movement required, so go straight to final step
-    s->stepNumber = s->numSteps;
+    // Go straight to end of sequence, output final position.
+    s->stepNumber = s->numSteps-1;
   }
+
   if (s->stepNumber < s->numSteps) {
     // Animation in progress, reposition servo
     s->stepNumber++;
-    if (s->profile == Bounce) {
+    if ((s->currentProfile & ~NoPowerOff) == Bounce) {
       // Retrieve step positions from array in flash
       byte profileValue = GETFLASH(&_bounceProfile[s->stepNumber]);
       s->currentPosition = map(profileValue, 0, 100, s->fromPosition, s->toPosition);
@@ -217,10 +222,12 @@ void PCA9685::updatePosition(uint8_t pin) {
     // We've finished animation, wait a little to allow servo to catch up
     s->stepNumber++;
   } else if (s->stepNumber == s->numSteps + _catchupSteps 
-            && s->currentPosition != 4095 && s->currentPosition != 0) {
+            && s->currentPosition != 0) {
 #ifdef IO_SWITCH_OFF_SERVO
-    // Wait has finished, so switch off PWM to prevent annoying servo buzz
-    writeDevice(pin, 0);
+    if ((s->currentProfile & NoPowerOff) == 0) {
+      // Wait has finished, so switch off PWM to prevent annoying servo buzz
+      writeDevice(pin, 0);
+    }
 #endif
     s->numSteps = 0;  // Done now.
   }
@@ -233,20 +240,25 @@ void PCA9685::writeDevice(uint8_t pin, int value) {
   DIAG(F("PCA9685 I2C:x%x WriteDevice Pin:%d Value:%d"), _I2CAddress, pin, value);
   #endif
   // Wait for previous request to complete
-  requestBlock.wait();
-  // Set up new request.
-  outputBuffer[0] = PCA9685_FIRST_SERVO + 4 * pin;
-  outputBuffer[1] = 0;
-  outputBuffer[2] = (value == 4095 ? 0x10 : 0);  // 4095=full on
-  outputBuffer[3] = value & 0xff;
-  outputBuffer[4] = value >> 8;
-  I2CManager.queueRequest(&requestBlock);
+  uint8_t status = requestBlock.wait();
+  if (status != I2C_STATUS_OK) {
+    _deviceState = DEVSTATE_FAILED;
+    DIAG(F("PCA9685 I2C:x%x failed %S"), _I2CAddress, I2CManager.getErrorMessage(status));
+  } else {
+    // Set up new request.
+    outputBuffer[0] = PCA9685_FIRST_SERVO + 4 * pin;
+    outputBuffer[1] = 0;
+    outputBuffer[2] = (value == 4095 ? 0x10 : 0);  // 4095=full on
+    outputBuffer[3] = value & 0xff;
+    outputBuffer[4] = value >> 8;
+    I2CManager.queueRequest(&requestBlock);
+  }
 }
 
 // Display details of this device.
 void PCA9685::_display() {
-  DIAG(F("PCA9685 I2C:x%x Vpins:%d-%d"), _I2CAddress, (int)_firstVpin, 
-    (int)_firstVpin+_nPins-1);
+  DIAG(F("PCA9685 I2C:x%x Configured on Vpins:%d-%d %S"), _I2CAddress, (int)_firstVpin, 
+    (int)_firstVpin+_nPins-1, (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
 }
 
 // Internal helper function for this device

IO_PCF8574.h

@@ -17,6 +17,24 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
 
+/* 
+ * The PCF8574 is a simple device; it only has one register.  The device 
+ * input/output mode and pullup are configured through this, and the 
+ * output state is written and the input state read through it too.
+ * 
+ * This is accomplished by having a weak resistor in series with the output,
+ * and a read-back of the other end of the resistor.  As an output, the 
+ * pin state is set to 1 or 0, and the output voltage goes to +5V or 0V
+ * (through the weak resistor).
+ * 
+ * In order to use the pin as an input, the output is written as
+ * a '1' in order to pull up the resistor.  Therefore the input will be
+ * 1 unless the pin is pulled down externally, in which case it will be 0.
+ * 
+ * As a consequence of this approach, it is not possible to use the device for
+ * inputs without pullups.
+ */
+
 #ifndef IO_PCF8574_H
 #define IO_PCF8574_H
 
@@ -70,7 +88,7 @@ private:
     if (status == I2C_STATUS_OK) 
       _portInputState = ((uint16_t)inputBuffer[0]) & 0xff;
     else  
-      _portInputState = 0xff;
+      _portInputState = 0xff; 
   }
 
   // Set up device ports

IO_VL53L0X.h

@@ -0,0 +1,299 @@
+/*
+ *  © 2021, Neil McKechnie. All rights reserved.
+ *  
+ *  This file is part of DCC++EX API
+ *
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+/*
+ * The VL53L0X Time-Of-Flight sensor operates by sending a short laser pulse and detecting
+ * the reflection of the pulse.  The time between the pulse and the receipt of reflections
+ * is measured and used to determine the distance to the reflecting object.
+ * 
+ * For economy of memory and processing time, this driver includes only part of the code 
+ * that ST provide in their API.  Also, the API code isn't very clear and it is not easy
+ * to identify what operations are useful and what are not. 
+ * The operation shown here doesn't include any calibration, so is probably not as accurate
+ * as using the full driver, but it's probably accurate enough for the purpose.
+ * 
+ * The device driver allocates up to 3 vpins to the device.  A digital read on the first pin
+ * will return a value that indicates whether the object is within the threshold range (1)
+ * or not (0).  An analogue read on the first pin returns the last measured distance (in mm), 
+ * the second pin returns the signal strength, and the third pin returns detected 
+ * ambient light level.  By default the device takes around 60ms to complete a ranging 
+ * operation, so we do a 100ms cycle (10 samples per second).
+ * 
+ * The VL53L0X is initially set to respond to I2C address 0x29.  If you only have one module,
+ * you can use this address.  However, the address can be modified by software.  If
+ * you select another address, that address will be written to the device and used until the device is reset.
+ * 
+ * If you have more than one module, then you will need to specify a digital VPIN (Arduino
+ * digital output or I/O extender pin) which you connect to the module's XSHUT pin.  Now,
+ * when the device driver starts, the XSHUT pin is set LOW to turn the module off.  Once 
+ * all VL53L0X modules are turned off, the driver works through each module in turn by
+ * setting XSHUT to HIGH to turn the module on,, then writing the module's desired I2C address.
+ * In this way, many VL53L0X modules can be connected to the one I2C bus, each one 
+ * using a distinct I2C address.
+ * 
+ * WARNING:  If the device's XSHUT pin is not connected, then it is very prone to noise, 
+ * and the device may even reset when handled.  If you're not using XSHUT, then it's 
+ * best to tie it to +5V.
+ * 
+ * The driver is configured as follows:
+ * 
+ * Single VL53L0X module:
+ *      VL53L0X::create(firstVpin, nPins, i2cAddress, lowThreshold, highThreshold);
+ * Where firstVpin is the first vpin reserved for reading the device,
+ *       nPins is 1, 2 or 3,
+ *       i2cAddress is the address of the device (normally 0x29),
+ *       lowThreshold is the distance at which the digital vpin state is set to 1 (in mm),
+ *   and highThreshold is the distance at which the digital vpin state is set to 0 (in mm).
+ * 
+ * Multiple VL53L0X modules:
+ *       VL53L0X::create(firstVpin, nPins, i2cAddress, lowThreshold, highThreshold, xshutPin);
+ *       ...
+ * Where firstVpin is the first vpin reserved for reading the device,
+ *       nPins is 1, 2 or 3,
+ *       i2cAddress is the address of the device (any valid address except 0x29),
+ *       lowThreshold is the distance at which the digital vpin state is set to 1 (in mm),
+ *       highThreshold is the distance at which the digital vpin state is set to 0 (in mm),
+ *   and xshutPin is the VPIN number corresponding to a digital output that is connected to the
+ *       XSHUT terminal on the module.
+ * 
+ * Example:
+ *   In mySetup function within mySetup.cpp:
+ *      VL53L0X::create(4000, 3, 0x29, 200, 250);
+ *      Sensor::create(4000, 4000, 0);  // Create a sensor
+ * 
+ *   When an object comes within 200mm of the sensor, a message 
+ *      <Q 4000>
+ *   will be sent over the serial USB, and when the object moves more than 250mm from the sensor, 
+ *   a message
+ *      <q 4000>
+ *   will be sent. 
+ * 
+ */
+
+#ifndef IO_VL53L0X_h
+#define IO_VL53L0X_h
+
+#include "IODevice.h"
+
+class VL53L0X : public IODevice {
+private: 
+  uint8_t _i2cAddress;
+  uint16_t _ambient;
+  uint16_t _distance;
+  uint16_t _signal;
+  uint16_t _onThreshold;
+  uint16_t _offThreshold;
+  VPIN _xshutPin;
+  bool _value;
+  uint8_t _nextState = 0;
+  I2CRB _rb;
+  uint8_t _inBuffer[12];
+  uint8_t _outBuffer[2];
+  // State machine states.
+  enum : uint8_t {
+    STATE_INIT = 0,
+    STATE_CONFIGUREADDRESS = 1,
+    STATE_SKIP = 2,
+    STATE_CONFIGUREDEVICE = 3,
+    STATE_INITIATESCAN = 4,
+    STATE_CHECKSTATUS = 5,
+    STATE_GETRESULTS = 6,
+    STATE_DECODERESULTS = 7,
+  };
+
+  // Register addresses
+  enum : uint8_t {
+    VL53L0X_REG_SYSRANGE_START=0x00,
+    VL53L0X_REG_RESULT_INTERRUPT_STATUS=0x13,
+    VL53L0X_REG_RESULT_RANGE_STATUS=0x14,
+    VL53L0X_CONFIG_PAD_SCL_SDA__EXTSUP_HV=0x89,
+    VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS=0x8A,
+  };
+  const uint8_t VL53L0X_I2C_DEFAULT_ADDRESS=0x29;
+
+public:
+  VL53L0X(VPIN firstVpin, int nPins, uint8_t i2cAddress, uint16_t onThreshold, uint16_t offThreshold, VPIN xshutPin = VPIN_NONE) {
+    _firstVpin = firstVpin;
+    _nPins = min(nPins, 3);
+    _i2cAddress = i2cAddress;
+    _onThreshold = onThreshold;
+    _offThreshold = offThreshold;
+    _xshutPin = xshutPin;
+    _value = 0;
+    addDevice(this);
+  }
+  static void create(VPIN firstVpin, int nPins, uint8_t i2cAddress, uint16_t onThreshold, uint16_t offThreshold, VPIN xshutPin = VPIN_NONE) {
+    new VL53L0X(firstVpin, nPins, i2cAddress, onThreshold, offThreshold, xshutPin);
+  }
+
+protected:
+  void _begin() override {
+    if (_xshutPin == VPIN_NONE) {
+      // Check if device is already responding on the nominated address.
+      if (I2CManager.exists(_i2cAddress)) {
+        // Yes, it's already on this address, so skip the address initialisation.
+        _nextState = STATE_CONFIGUREDEVICE;
+      } else {
+        _nextState = STATE_INIT;
+      }
+    }
+  }
+
+  void _loop(unsigned long currentMicros) override {
+    uint8_t status;
+    switch (_nextState) {
+      case STATE_INIT:
+        // On first entry to loop, reset this module by pulling XSHUT low.  All modules
+        // will be reset in turn.
+        if (_xshutPin != VPIN_NONE) IODevice::write(_xshutPin, 0);
+        _nextState = STATE_CONFIGUREADDRESS;
+        break;
+      case STATE_CONFIGUREADDRESS:
+        // On second entry, set XSHUT pin high to allow the module to restart.
+        // On the module, there is a diode in series with the XSHUT pin to 
+        // protect the low-voltage pin against +5V.
+        if (_xshutPin != VPIN_NONE) IODevice::write(_xshutPin, 1);
+        // Allow the module time to restart
+        delay(10);
+        // Then write the desired I2C address to the device, while this is the only
+        //  module responding to the default address.
+        I2CManager.write(VL53L0X_I2C_DEFAULT_ADDRESS, 2, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, _i2cAddress);
+        _nextState = STATE_SKIP;
+        break;
+      case STATE_SKIP:
+        // Do nothing on the third entry.
+        _nextState = STATE_CONFIGUREDEVICE;
+        break;
+      case STATE_CONFIGUREDEVICE:
+        // On next entry, check if device address has been set.
+        if (I2CManager.exists(_i2cAddress)) {
+          #ifdef DIAG_IO
+          _display();
+          #endif
+          // Set 2.8V mode
+          write_reg(VL53L0X_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 
+            read_reg(VL53L0X_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01);
+        } else {
+          DIAG(F("VL53L0X I2C:x%x device not responding"), _i2cAddress);
+          _deviceState = DEVSTATE_FAILED;
+        }
+        _nextState = STATE_INITIATESCAN;
+        break;
+      case STATE_INITIATESCAN:
+        // Not scanning, so initiate a scan
+        _outBuffer[0] = VL53L0X_REG_SYSRANGE_START;
+        _outBuffer[1] = 0x01;
+        I2CManager.write(_i2cAddress, _outBuffer, 2, &_rb);
+        _nextState = STATE_CHECKSTATUS;
+        break;
+      case STATE_CHECKSTATUS:
+        status = _rb.status;
+        if (status == I2C_STATUS_PENDING) return; // try next time
+        if (status != I2C_STATUS_OK) {
+          DIAG(F("VL53L0X I2C:x%x Error:%d %S"), _i2cAddress, status, I2CManager.getErrorMessage(status));
+          _deviceState = DEVSTATE_FAILED;
+          _value = false;
+        } else
+          _nextState = 2;
+        delayUntil(currentMicros + 95000); // wait for 95 ms before checking.
+        _nextState = STATE_GETRESULTS;
+        break;
+      case STATE_GETRESULTS:
+        // Ranging completed.  Request results
+        _outBuffer[0] = VL53L0X_REG_RESULT_RANGE_STATUS;
+        I2CManager.read(_i2cAddress, _inBuffer, 12, _outBuffer, 1, &_rb);
+        _nextState = 3;
+        delayUntil(currentMicros + 5000); // Allow 5ms to get data
+        _nextState = STATE_DECODERESULTS;
+        break;
+      case STATE_DECODERESULTS:
+        // If I2C write still busy, return.
+        status = _rb.status;
+        if (status == I2C_STATUS_PENDING) return; // try again next time
+        if (status == I2C_STATUS_OK) {
+          if (!(_inBuffer[0] & 1)) return; // device still busy
+          uint8_t deviceRangeStatus = ((_inBuffer[0] & 0x78) >> 3);
+          if (deviceRangeStatus == 0x0b) {
+            // Range status OK, so use data
+            _ambient = makeuint16(_inBuffer[7], _inBuffer[6]);
+            _signal = makeuint16(_inBuffer[9], _inBuffer[8]);
+            _distance = makeuint16(_inBuffer[11], _inBuffer[10]);
+            if (_distance <= _onThreshold) 
+              _value = true;
+            else if (_distance > _offThreshold) 
+              _value = false;
+          }
+        }
+        // Completed. Restart scan on next loop entry.
+        _nextState = STATE_INITIATESCAN;
+        break;
+      default:
+        break;
+    }
+  }
+
+  // For analogue read, first pin returns distance, second pin is signal strength, and third is ambient level.
+  int _readAnalogue(VPIN vpin) override {
+    int pin = vpin - _firstVpin;
+    switch (pin) {
+      case 0:
+        return _distance;
+      case 1:
+        return _signal;
+      case 2:
+        return _ambient;
+      default:
+        return -1;
+    }
+  }
+
+  // For digital read, return zero for all but first pin.
+  int _read(VPIN vpin) override {
+    if (vpin == _firstVpin)
+      return _value;
+    else
+      return 0;
+  }
+
+  void _display() override {
+    DIAG(F("VL53L0X I2C:x%x Configured on Vpins:%d-%d On:%dmm Off:%dmm %S"),
+      _i2cAddress, _firstVpin, _firstVpin+_nPins-1, _onThreshold, _offThreshold,
+      (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
+  }
+  
+
+private:
+  inline uint16_t makeuint16(byte lsb, byte msb) {
+    return (((uint16_t)msb) << 8) | lsb;
+  }
+  uint8_t write_reg(uint8_t reg, uint8_t data) {
+    // write byte to register
+    uint8_t outBuffer[2];
+    outBuffer[0] = reg;
+    outBuffer[1] = data;
+    return I2CManager.write(_i2cAddress, outBuffer, 2);
+  }
+  uint8_t read_reg(uint8_t reg) {
+    // read byte from register and return value
+    I2CManager.read(_i2cAddress, _inBuffer, 1, &reg, 1);
+    return _inBuffer[0];
+  }
+};
+
+#endif // IO_VL53L0X_h

LCDDisplay.h

@@ -19,15 +19,12 @@
 #ifndef LCDDisplay_h
 #define LCDDisplay_h
 #include <Arduino.h>
+#include "defines.h"
 #include "DisplayInterface.h"
 
-#if __has_include ( "config.h")
-  #include "config.h"
-#endif
-
 // Allow maximum message length to be overridden from config.h
 #if !defined(MAX_MSG_SIZE) 
-#define MAX_MSG_SIZE 16
+#define MAX_MSG_SIZE 20
 #endif
 
 // Set default scroll mode (overridable in config.h)

LCN.cpp

@@ -48,8 +48,7 @@ void LCN::loop() {
     }
     else if (ch == 't' || ch == 'T') { // Turnout opcodes
       if (Diag::LCN) DIAG(F("LCN IN %d%c"),id,(char)ch);
-      Turnout * tt = Turnout::get(id);
-      if (!tt) tt=Turnout::createLCN(id);
+      if (!Turnout::exists(id)) LCNTurnout::create(id);
       Turnout::setClosedStateOnly(id,ch=='t');
       Turnout::turnoutlistHash++; // signals ED update of turnout data
       id = 0;

LiquidCrystal_I2C.cpp

@@ -196,7 +196,7 @@ void LiquidCrystal_I2C::send(uint8_t value, uint8_t mode) {
   outputBuffer[len++] = highnib;
   outputBuffer[len++] = lownib|En;
   outputBuffer[len++] = lownib;
-  I2CManager.write(_Addr, outputBuffer, len, &requestBlock);
+  I2CManager.write(_Addr, outputBuffer, len);  // Write command synchronously
 }
 
 // write 4 data bits to the HD44780 LCD controller.
@@ -205,19 +205,15 @@ void LiquidCrystal_I2C::write4bits(uint8_t value) {
   // Enable must be set/reset for at least 450ns.  This is well within the
   // I2C clock cycle time of 2.5us at 400kHz. Data is clocked in to the
   // HD44780 on the trailing edge of the Enable pin.
-  // Wait for previous request to complete before writing to outputbuffer.
-  requestBlock.wait();
   uint8_t len = 0;
   outputBuffer[len++] = _data|En;
   outputBuffer[len++] = _data;
-  I2CManager.write(_Addr, outputBuffer, len, &requestBlock);
+  I2CManager.write(_Addr, outputBuffer, len);  // Write command synchronously
 }
 
 // write a byte to the PCF8574 I2C interface.  We don't need to set
 // the enable pin for this.
 void LiquidCrystal_I2C::expanderWrite(uint8_t value) {
-  // Wait for previous request to complete before writing to outputbuffer.
-  requestBlock.wait();
   outputBuffer[0] = value | _backlightval;
-  I2CManager.write(_Addr, outputBuffer, 1, &requestBlock);
+  I2CManager.write(_Addr, outputBuffer, 1);  // Write command synchronously
 }

LiquidCrystal_I2C.h

@@ -75,10 +75,8 @@ public:
   void backlight();
   
   void command(uint8_t);
-  void init();
 
 private:
-  void init_priv();
   void send(uint8_t, uint8_t);
   void write4bits(uint8_t);
   void expanderWrite(uint8_t);
@@ -88,9 +86,9 @@ private:
   uint8_t _displaymode;
   uint8_t _backlightval;
 
-  I2CRB requestBlock;
   uint8_t outputBuffer[4];
-  bool isBusy() { return requestBlock.isBusy(); }
+  // I/O is synchronous, so if this is called we're not busy!
+  bool isBusy() override { return false; } 
 };
 
 #endif

RMFT2.cpp

@@ -19,6 +19,7 @@
 #include <Arduino.h>
 #include "RMFT2.h"
 #include "DCC.h"
+#include "DCCWaveform.h"
 #include "DIAG.h"
 #include "WiThrottle.h"
 #include "DCCEXParser.h"
@@ -36,12 +37,14 @@ const int16_t HASH_KEYWORD_UNLATCH=1353;
 const int16_t HASH_KEYWORD_PAUSE=-4142;
 const int16_t HASH_KEYWORD_RESUME=27609;
 const int16_t HASH_KEYWORD_KILL=5218;
+const int16_t HASH_KEYWORD_ROUTES=-3702;
 
 // One instance of RMFT clas is used for each "thread" in the automation.
 // Each thread manages a loco on a journey through the layout, and/or may manage a scenery automation.
 // The thrrads exist in a ring, each time through loop() the next thread in the ring is serviced.
 
 // Statics 
+const int16_t LOCO_ID_WAITING=-99; // waiting for loco id from prog track
 int16_t RMFT2::progtrackLocoId;  // used for callback when detecting a loco on prograck
 bool RMFT2::diag=false;      // <D EXRAIL ON>  
 RMFT2 * RMFT2::loopTask=NULL; // loopTask contains the address of ONE of the tasks in a ring.
@@ -77,7 +80,7 @@ byte RMFT2::flags[MAX_FLAGS];
       VPIN id=GET_OPERAND(0);
       int addr=GET_OPERAND(1);
       byte subAddr=GET_OPERAND(2);
-      Turnout::createDCC(id,addr,subAddr);
+      DCCTurnout::create(id,addr,subAddr);
       continue;
      }
 
@@ -87,14 +90,14 @@ byte RMFT2::flags[MAX_FLAGS];
       int activeAngle=GET_OPERAND(2);
       int inactiveAngle=GET_OPERAND(3);
       int profile=GET_OPERAND(4);
-      Turnout::createServo(id,pin,activeAngle,inactiveAngle,profile);
+      ServoTurnout::create(id,pin,activeAngle,inactiveAngle,profile);
       continue;
      }
 
      if (opcode==OPCODE_PINTURNOUT) {
       int16_t id=GET_OPERAND(0);
       VPIN pin=GET_OPERAND(1);
-      Turnout::createVpin(id,pin);
+      VpinTurnout::create(id,pin);
       continue;
      }
      // other opcodes are not needed on this pass
@@ -192,7 +195,14 @@ bool RMFT2::parseSlash(Print * stream, byte & paramCount, int16_t p[]) {
                     task->loco=cab;                    
                  }
               return true;
-                 
+
+            case HASH_KEYWORD_ROUTES: // </ ROUTES > JMRI withrottle support 
+                 if (paramCount>1) return false;
+                 StringFormatter::send(stream,F("</ROUTES "));
+                 emitWithrottleRouteList(stream);                                  
+                 StringFormatter::send(stream,F(">"));
+              return true;
+              
             default:
               break;
           }
@@ -267,6 +277,7 @@ RMFT2::RMFT2(int progCtr) {
   forward=true;
   invert=false;
   stackDepth=0;
+  onTurnoutId=0; // Not handling an ONTHROW/ONCLOSE
    
   // chain into ring of RMFTs
   if (loopTask==NULL) {
@@ -317,15 +328,24 @@ int RMFT2::locateRouteStart(int16_t _route) {
 void RMFT2::driveLoco(byte speed) {
      if (loco<=0) return;  // Prevent broadcast! 
      if (diag) DIAG(F("EXRAIL drive %d %d %d"),loco,speed,forward^invert);
+     if (DCCWaveform::mainTrack.getPowerMode()==POWERMODE::OFF) {
+        DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON); 
+        Serial.println(F("<p1>")); // tell JMRI
+     }
      DCC::setThrottle(loco,speed, forward^invert);
      speedo=speed;
 }
 
-bool RMFT2::readSensor(int16_t sensorId) {
-  VPIN vpin=abs(sensorId);
+bool RMFT2::readSensor(uint16_t sensorId) {
+  // Exrail operands are unsigned but we need the signed version as inserted by the macros.  
+  int16_t sId=(int16_t) sensorId;
+
+  VPIN vpin=abs(sId);
   if (getFlag(vpin,LATCH_FLAG)) return true; // latched on
-  bool s= IODevice::read(vpin) ^ (sensorId<0);
-  if (s && diag) DIAG(F("EXRAIL Sensor %d hit"),sensorId);
+
+  // negative sensorIds invert the logic (e.g. for a break-beam sensor which goes OFF when detecting)
+  bool s= IODevice::read(vpin) ^ (sId<0);
+  if (s && diag) DIAG(F("EXRAIL Sensor %d hit"),sId);
   return s;
 }
 
@@ -357,7 +377,7 @@ bool RMFT2::skipIfBlock() {
 
 
 
-/* static */ void RMFT2::readLocoCallback(int cv) {
+/* static */ void RMFT2::readLocoCallback(int16_t cv) {
      progtrackLocoId=cv;
 }
 
@@ -432,6 +452,7 @@ void RMFT2::loop2() {
       if (readSensor(operand)) {
         // reset timer to half a second and keep waiting
         waitAfter=millis();
+        delayMe(50);
         return; 
       }
       if (millis()-waitAfter < 500 ) return;   
@@ -462,6 +483,13 @@ void RMFT2::loop2() {
         if (loco) DCC::writeCVByteMain(loco, operand, GET_OPERAND(1));
         break;
 
+    case OPCODE_POWEROFF:
+        DCCWaveform::mainTrack.setPowerMode(POWERMODE::OFF);
+        DCCWaveform::progTrack.setPowerMode(POWERMODE::OFF);
+        DCC::setProgTrackSyncMain(false);       
+        Serial.println(F("<p0>")); // Tell JMRI
+        break;
+
     case OPCODE_RESUME:
          pausingTask=NULL;
          driveLoco(speedo);
@@ -489,15 +517,15 @@ void RMFT2::loop2() {
       break;
     
     case OPCODE_DELAY:
-      delayMe(operand*100);
+      delayMe(operand*100L);
       break;
    
     case OPCODE_DELAYMINS:
-      delayMe(operand*60*1000);
+      delayMe(operand*60L*1000L);
       break;
     
     case OPCODE_RANDWAIT:
-      delayMe((long)random(operand*100));
+      delayMe(random(operand)*100L);
       break;
     
     case OPCODE_RED:
@@ -515,11 +543,19 @@ void RMFT2::loop2() {
     case OPCODE_FON:      
       if (loco) DCC::setFn(loco,operand,true);
       break;
-    
+     
     case OPCODE_FOFF:
       if (loco) DCC::setFn(loco,operand,false);
       break;
-
+    
+    case OPCODE_XFON:      
+      DCC::setFn(operand,GET_OPERAND(1),true);
+      break;
+   
+    case OPCODE_XFOFF:      
+      DCC::setFn(operand,GET_OPERAND(1),false);
+      break;
+   
     case OPCODE_FOLLOW:
       progCounter=locateRouteStart(operand);
       if (progCounter<0) kill(F("FOLLOW unknown"), operand); 
@@ -530,7 +566,7 @@ void RMFT2::loop2() {
         kill(F("CALL stack"), stackDepth);
         return;
       }
-      callStack[stackDepth++]=progCounter;
+      callStack[stackDepth++]=progCounter+3;
       progCounter=locateRouteStart(operand);
       if (progCounter<0) kill(F("CALL unknown"),operand); 
       return;
@@ -549,7 +585,10 @@ void RMFT2::loop2() {
       return;
       
     case OPCODE_JOIN:
+       DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON); 
+       DCCWaveform::progTrack.setPowerMode(POWERMODE::ON); 
        DCC::setProgTrackSyncMain(true);
+       Serial.println(F("<p1 JOIN>")); // Tell JMRI
        break;
 
     case OPCODE_UNJOIN:
@@ -557,14 +596,20 @@ void RMFT2::loop2() {
        break;
        
     case OPCODE_READ_LOCO1: // READ_LOCO is implemented as 2 separate opcodes
+       progtrackLocoId=LOCO_ID_WAITING;  // Nothing found yet
        DCC::getLocoId(readLocoCallback);
        break;
       
       case OPCODE_READ_LOCO2:
-       if (progtrackLocoId<0) {
+       if (progtrackLocoId==LOCO_ID_WAITING) {
         delayMe(100);
         return; // still waiting for callback
        }
+       if (progtrackLocoId<0) {
+        kill(F("No Loco Found"),progtrackLocoId);
+        return; // still waiting for callback
+       }
+       
        loco=progtrackLocoId;
        speedo=0;
        forward=true;
@@ -598,9 +643,16 @@ void RMFT2::loop2() {
        break;
 
           
-       case OPCODE_SERVO: // OPCODE_SERVO,V(id),OPCODE_PAD,V(position),OPCODE_PAD,V(profile),
-        IODevice::writeAnalogue(operand,GET_OPERAND(1),GET_OPERAND(2));          
+       case OPCODE_SERVO: // OPCODE_SERVO,V(vpin),OPCODE_PAD,V(position),OPCODE_PAD,V(profile),OPCODE_PAD,V(duration)
+        IODevice::writeAnalogue(operand,GET_OPERAND(1),GET_OPERAND(2),GET_OPERAND(3));          
         break;
+       
+       case OPCODE_WAITFOR: // OPCODE_SERVO,V(pin)
+            if (IODevice::isBusy(operand)) {
+              delayMe(100);
+              return;
+            }
+            break;
 
        case OPCODE_PRINT:
             printMessage(operand);
@@ -609,7 +661,7 @@ void RMFT2::loop2() {
        case OPCODE_ROUTE:
        case OPCODE_AUTOMATION:
        case OPCODE_SEQUENCE:
-          DIAG(F("EXRAIL begin(%d)"),operand);
+          if (diag) DIAG(F("EXRAIL begin(%d)"),operand);
           break;
 
        case OPCODE_PAD: // Just a padding for previous opcode needing >1 operad byte.
@@ -658,25 +710,39 @@ void RMFT2::kill(const FSH * reason, int operand) {
      byte opcode=GET_OPCODE;
      if (opcode==OPCODE_ENDEXRAIL) return;
      if (opcode!=OPCODE_SIGNAL) continue;
-     byte redpin=GET_OPERAND(1);
+     byte redpin=GET_OPERAND(0);
      if (redpin!=id)continue;
-     byte amberpin=GET_OPERAND(2);
-     byte greenpin=GET_OPERAND(3);
-     IODevice::write(redpin,red);
+     byte amberpin=GET_OPERAND(1);
+     byte greenpin=GET_OPERAND(2);
+     // If amberpin is zero, synthesise amber from red+green
+     IODevice::write(redpin,red || (amber && (amberpin==0)));
      if (amberpin) IODevice::write(amberpin,amber);
-     if (greenpin) IODevice::write(amberpin,green);
+     if (greenpin) IODevice::write(greenpin,green || (amber && (amberpin==0)));
      return;
    }
   } 
- void RMFT2::turnoutEvent(VPIN id, bool closed) {
+ void RMFT2::turnoutEvent(int16_t turnoutId, bool closed) {
+    
+    // Check we dont already have a task running this turnout
+    RMFT2 * task=loopTask;                 
+    while(task) {
+      if (task->onTurnoutId==turnoutId) {
+        DIAG(F("Recursive ONTHROW/ONCLOSE for Turnout %d"),turnoutId);
+        return;
+        }
+      task=task->next;      
+      if (task==loopTask) break;      
+      }
+    // Hunt for an ONTHROW/ONCLOSE for this turnout   
     byte huntFor=closed ?  OPCODE_ONCLOSE : OPCODE_ONTHROW ;
     // caution hides class progCounter;
     for (int progCounter=0;; SKIPOP){
      byte opcode=GET_OPCODE;
      if (opcode==OPCODE_ENDEXRAIL) return;
      if (opcode!=huntFor) continue;
-     if (id!=GET_OPERAND(0)) continue;
-     new RMFT2(progCounter);  // new task starts at this instruction
+     if (turnoutId!=(int16_t)GET_OPERAND(0)) continue;
+     task=new RMFT2(progCounter);  // new task starts at this instruction
+     task->onTurnoutId=turnoutId; // flag for recursion detector
      return;
    }
  }

RMFT2.h

@@ -34,15 +34,15 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
              OPCODE_LATCH,OPCODE_UNLATCH,OPCODE_SET,OPCODE_RESET,
              OPCODE_IF,OPCODE_IFNOT,OPCODE_ENDIF,OPCODE_IFRANDOM,OPCODE_IFRESERVE,
              OPCODE_DELAY,OPCODE_DELAYMINS,OPCODE_RANDWAIT,
-             OPCODE_FON,OPCODE_FOFF,
+             OPCODE_FON,OPCODE_FOFF,OPCODE_XFON,OPCODE_XFOFF,
              OPCODE_RED,OPCODE_GREEN,OPCODE_AMBER,
-             OPCODE_SERVO,OPCODE_SIGNAL,OPCODE_TURNOUT,
+             OPCODE_SERVO,OPCODE_SIGNAL,OPCODE_TURNOUT,OPCODE_WAITFOR,
              OPCODE_PAD,OPCODE_FOLLOW,OPCODE_CALL,OPCODE_RETURN,
              OPCODE_JOIN,OPCODE_UNJOIN,OPCODE_READ_LOCO1,OPCODE_READ_LOCO2,OPCODE_POM,
              OPCODE_START,OPCODE_SETLOCO,OPCODE_SENDLOCO,
-             OPCODE_PAUSE, OPCODE_RESUME,
+             OPCODE_PAUSE, OPCODE_RESUME,OPCODE_POWEROFF,
              OPCODE_ONCLOSE, OPCODE_ONTHROW, OPCODE_SERVOTURNOUT, OPCODE_PINTURNOUT,
-             OPCODE_PRINT,  
+             OPCODE_PRINT,
              OPCODE_ROUTE,OPCODE_AUTOMATION,OPCODE_SEQUENCE,OPCODE_ENDTASK,OPCODE_ENDEXRAIL
              };
 
@@ -65,18 +65,18 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
     RMFT2(int progCounter);
     RMFT2(int route, uint16_t cab);
     ~RMFT2();
-    static void readLocoCallback(int cv);
+    static void readLocoCallback(int16_t cv);
     static void emitWithrottleRouteList(Print* stream); 
     static void createNewTask(int route, uint16_t cab);
-    static void turnoutEvent(VPIN id, bool closed);  
+    static void turnoutEvent(int16_t id, bool closed);  
 private: 
-    static void ComandFilter(Print * stream, byte & opcode, byte & paramCount, int p[]);
-    static bool parseSlash(Print * stream, byte & paramCount, int p[]) ;
+    static void ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16_t p[]);
+    static bool parseSlash(Print * stream, byte & paramCount, int16_t p[]) ;
     static void streamFlags(Print* stream);
     static void setFlag(VPIN id,byte onMask, byte OffMask=0);
     static bool getFlag(VPIN id,byte mask);   
     static int locateRouteStart(int16_t _route);
-    static int progtrackLocoId;
+    static int16_t progtrackLocoId;
     static void doSignal(VPIN id,bool red, bool amber, bool green); 
     static void emitRouteDescription(Print * stream, char type, int id, const FSH * description);
     static void emitWithrottleDescriptions(Print * stream);
@@ -85,7 +85,7 @@ private:
     static RMFT2 * pausingTask;
     void delayMe(long millisecs);
     void driveLoco(byte speedo);
-    bool readSensor(int16_t sensorId);
+    bool readSensor(uint16_t sensorId);
     bool skipIfBlock();
     bool readLoco();
     void loop2();
@@ -106,10 +106,11 @@ private:
     unsigned long  delayTime;
     byte  taskId;
     
-    int loco;
+    uint16_t loco;
     bool forward;
     bool invert;
-    int speedo;
+    byte speedo;
+    int16_t onTurnoutId;
     byte stackDepth;
     int callStack[MAX_STACK_DEPTH];
 };

RMFTMacros.h

@@ -19,8 +19,9 @@
 #ifndef RMFTMacros_H
 #define RMFTMacros_H
 
-// remove normal code LCD macro (will be restored later)
+// remove normal code LCD & SERIAL macros (will be restored later)
 #undef LCD
+#undef SERIAL
 
 
 // This file will include and build the EXRAIL script and associated helper tricks.
@@ -51,7 +52,7 @@
 
 
 #define ALIAS(name,value) const int name=value; 
-#define EXRAIL  void  RMFT2::emitWithrottleDescriptions(Print * stream) {
+#define EXRAIL  void  RMFT2::emitWithrottleDescriptions(Print * stream) {(void)stream;
 #define ROUTE(id, description) emitRouteDescription(stream,'R',id,F(description));
 #define AUTOMATION(id, description) emitRouteDescription(stream,'A',id,F(description));
 #define ENDEXRAIL  }
@@ -68,6 +69,7 @@
 #define ENDIF  
 #define ENDTASK
 #define ESTOP 
+#define FADE(pin,value,ms)
 #define FOFF(func)
 #define FOLLOW(route) 
 #define FON(func) 
@@ -77,28 +79,35 @@
 #define IF(sensor_id) 
 #define IFNOT(sensor_id)
 #define IFRANDOM(percent) 
-#define IFRESERVE(block) 
+#define IFRESERVE(block)
 #define INVERT_DIRECTION 
 #define JOIN 
 #define LATCH(sensor_id) 
 #define LCD(row,msg) 
+#define LCN(msg) 
 #define ONCLOSE(turnout_id)
 #define ONTHROW(turnout_id) 
 #define PAUSE
 #define PRINT(msg) 
 #define POM(cv,value)
+#define POWEROFF
 #define READ_LOCO 
 #define RED(signal_id) 
 #define RESERVE(blockid) 
-#define RESET(sensor_id) 
+#define RESET(pin) 
 #define RESUME 
 #define RETURN 
 #define REV(speed) 
 #define START(route) 
 #define SENDLOCO(cab,route) 
+#define SERIAL(msg) 
+#define SERIAL1(msg) 
+#define SERIAL2(msg) 
+#define SERIAL3(msg) 
 #define SERVO(id,position,profile) 
+#define SERVO2(id,position,duration) 
 #define SETLOCO(loco) 
-#define SET(sensor_id) 
+#define SET(pin) 
 #define SEQUENCE(id) 
 #define SPEED(speed) 
 #define STOP 
@@ -110,6 +119,9 @@
 #define TURNOUT(id,addr,subaddr) 
 #define UNJOIN 
 #define UNLATCH(sensor_id) 
+#define WAITFOR(pin)
+#define XFOFF(cab,func)
+#define XFON(cab,func)
 
 #include "myAutomation.h"
 
@@ -122,14 +134,24 @@
 
 #undef EXRAIL 
 #undef PRINT
+#undef LCN
+#undef SERIAL
+#undef SERIAL1
+#undef SERIAL2
+#undef SERIAL3
 #undef ENDEXRAIL  
 #undef LCD
 const int StringMacroTracker1=__COUNTER__;
 #define ALIAS(name,value) 
 #define EXRAIL void  RMFT2::printMessage(uint16_t id) { switch(id) {
 #define ENDEXRAIL  default: DIAG(F("printMessage error %d %d"),id,StringMacroTracker1); return ; }}
-#define PRINT(msg)  case (__COUNTER__ - StringMacroTracker1) : printMessage2(F(msg));break;
-#define LCD(id,msg)  case (__COUNTER__ - StringMacroTracker1) : StringFormatter::lcd(id,F(msg));break;
+#define PRINT(msg)    case (__COUNTER__ - StringMacroTracker1) : printMessage2(F(msg));break;
+#define LCN(msg)      case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&LCN_SERIAL,F(msg));break;
+#define SERIAL(msg)   case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&Serial,F(msg));break;
+#define SERIAL1(msg)  case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&Serial1,F(msg));break;
+#define SERIAL2(msg)  case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&Serial2,F(msg));break;
+#define SERIAL3(msg)  case (__COUNTER__ - StringMacroTracker1) : StringFormatter::send(&Serial3,F(msg));break;
+#define LCD(id,msg)   case (__COUNTER__ - StringMacroTracker1) : StringFormatter::lcd(id,F(msg));break;
 #include "myAutomation.h"
 
 // Setup for Pass 3: create main routes table 
@@ -162,10 +184,12 @@ const int StringMacroTracker1=__COUNTER__;
 #undef JOIN
 #undef LATCH
 #undef LCD
+#undef LCN
 #undef ONCLOSE
 #undef ONTHROW
 #undef PAUSE
 #undef POM
+#undef POWEROFF
 #undef PRINT
 #undef READ_LOCO
 #undef RED
@@ -178,7 +202,13 @@ const int StringMacroTracker1=__COUNTER__;
 #undef START
 #undef SEQUENCE 
 #undef SERVO
+#undef SERVO2
+#undef FADE
 #undef SENDLOCO
+#undef SERIAL
+#undef SERIAL1
+#undef SERIAL2
+#undef SERIAL3
 #undef SETLOCO
 #undef SET
 #undef SPEED
@@ -190,6 +220,9 @@ const int StringMacroTracker1=__COUNTER__;
 #undef TURNOUT
 #undef UNJOIN
 #undef UNLATCH
+#undef WAITFOR
+#undef XFOFF
+#undef XFON
 
 // Define macros for route code creation 
 #define V(val) ((int16_t)(val))&0x00FF,((int16_t)(val)>>8)&0x00FF
@@ -209,11 +242,12 @@ const int StringMacroTracker1=__COUNTER__;
 #define AT(sensor_id) OPCODE_AT,V(sensor_id),
 #define CALL(route) OPCODE_CALL,V(route),
 #define CLOSE(id)  OPCODE_CLOSE,V(id),
-#define DELAY(ms) OPCODE_DELAY,V(ms/100),
+#define DELAY(ms) OPCODE_DELAY,V(ms/100L),
 #define DELAYMINS(mindelay) OPCODE_DELAYMINS,V(mindelay),
-#define DELAYRANDOM(mindelay,maxdelay) OPCODE_DELAY,V(mindelay/100),OPCODE_RANDWAIT,V((maxdelay-mindelay)/100),
+#define DELAYRANDOM(mindelay,maxdelay) OPCODE_DELAY,V(mindelay/100L),OPCODE_RANDWAIT,V((maxdelay-mindelay)/100L),
 #define ENDIF  OPCODE_ENDIF,NOP,
 #define ESTOP OPCODE_SPEED,V(1), 
+#define FADE(pin,value,ms) OPCODE_SERVO,V(pin),OPCODE_PAD,V(value),OPCODE_PAD,V(PCA9685::ProfileType::UseDuration|PCA9685::NoPowerOff),OPCODE_PAD,V(ms/100L),
 #define FOFF(func) OPCODE_FOFF,V(func),
 #define FOLLOW(route) OPCODE_FOLLOW,V(route),
 #define FON(func) OPCODE_FON,V(func),
@@ -227,24 +261,31 @@ const int StringMacroTracker1=__COUNTER__;
 #define INVERT_DIRECTION OPCODE_INVERT_DIRECTION,NOP,
 #define JOIN OPCODE_JOIN,NOP,
 #define LATCH(sensor_id) OPCODE_LATCH,V(sensor_id),
-#define LCD(id,msg) OPCODE_PRINT,V(__COUNTER__ - StringMacroTracker2),
+#define LCD(id,msg) PRINT(msg)
+#define LCN(msg) PRINT(msg)
 #define ONCLOSE(turnout_id) OPCODE_ONCLOSE,V(turnout_id),
 #define ONTHROW(turnout_id) OPCODE_ONTHROW,V(turnout_id),
 #define PAUSE OPCODE_PAUSE,NOP,
 #define POM(cv,value) OPCODE_POM,V(cv),OPCODE_PAD,V(value),
+#define POWEROFF OPCODE_POWEROFF,NOP,
 #define PRINT(msg) OPCODE_PRINT,V(__COUNTER__ - StringMacroTracker2),
 #define READ_LOCO OPCODE_READ_LOCO1,NOP,OPCODE_READ_LOCO2,NOP,
 #define RED(signal_id) OPCODE_RED,V(signal_id),
 #define RESERVE(blockid) OPCODE_RESERVE,V(blockid),
-#define RESET(sensor_id) OPCODE_RESET,V(sensor_id),
+#define RESET(pin) OPCODE_RESET,V(pin),
 #define RESUME OPCODE_RESUME,NOP,
 #define RETURN OPCODE_RETURN,NOP,
 #define REV(speed) OPCODE_REV,V(speed),
 #define SENDLOCO(cab,route) OPCODE_SENDLOCO,V(cab),OPCODE_PAD,V(route),
+#define SERIAL(msg) PRINT(msg)
+#define SERIAL1(msg) PRINT(msg)
+#define SERIAL2(msg) PRINT(msg)
+#define SERIAL3(msg) PRINT(msg)
 #define START(route) OPCODE_START,V(route),
-#define SERVO(id,position,profile) OPCODE_SERVO,V(id),OPCODE_PAD,V(position),OPCODE_PAD,V(PCA9685::ProfileType::profile),
+#define SERVO(id,position,profile) OPCODE_SERVO,V(id),OPCODE_PAD,V(position),OPCODE_PAD,V(PCA9685::profile),OPCODE_PAD,V(0),
+#define SERVO2(id,position,ms) OPCODE_SERVO,V(id),OPCODE_PAD,V(position),OPCODE_PAD,V(PCA9685::Instant),OPCODE_PAD,V(ms/100L),
 #define SETLOCO(loco) OPCODE_SETLOCO,V(loco),
-#define SET(sensor_id) OPCODE_SET,V(sensor_id),
+#define SET(pin) OPCODE_SET,V(pin),
 #define SPEED(speed) OPCODE_SPEED,V(speed),
 #define STOP OPCODE_SPEED,V(0), 
 #define SIGNAL(redpin,amberpin,greenpin) OPCODE_SIGNAL,V(redpin),OPCODE_PAD,V(amberpin),OPCODE_PAD,V(greenpin), 
@@ -254,13 +295,17 @@ const int StringMacroTracker1=__COUNTER__;
 #define TURNOUT(id,addr,subaddr) OPCODE_TURNOUT,V(id),OPCODE_PAD,V(addr),OPCODE_PAD,V(subaddr),
 #define UNJOIN OPCODE_UNJOIN,NOP,
 #define UNLATCH(sensor_id) OPCODE_UNLATCH,V(sensor_id),
+#define WAITFOR(pin) OPCODE_WAITFOR,V(pin),
+#define XFOFF(cab,func) OPCODE_XFOFF,V(cab),OPCODE_PAD,V(func),
+#define XFON(cab,func) OPCODE_XFON,V(cab),OPCODE_PAD,V(func),
 
 // PASS2 Build RouteCode
 const int StringMacroTracker2=__COUNTER__;
 #include "myAutomation.h"
 
-// Restore normal code LCD macro
+// Restore normal code LCD & SERIAL  macro
 #undef LCD
 #define LCD   StringFormatter::lcd
-
+#undef SERIAL
+#define SERIAL  0x0
 #endif

SSD1306Ascii.cpp

@@ -99,9 +99,6 @@ SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
   lcdRows = height / 8;
   lcdCols = width / 6;
 
-  // Initialise request block for I2C
-  requestBlock.init();
-
   I2CManager.begin();
   I2CManager.setClock(400000L);  // Set max supported I2C speed
   for (byte address = 0x3c; address <= 0x3d; address++) {

Sensors.cpp

@@ -103,12 +103,6 @@ void Sensor::checkAll(Print *stream){
       // Required time elapsed since last read cycle started,
       // so initiate new scan through the sensor list
       readingSensor = firstSensor;
-#ifdef USE_NOTIFY
-      if (firstSensor == firstPollSensor) 
-        pollSignalPhase = true;
-      else
-        pollSignalPhase = false;
-#endif
       lastReadCycle = thisTime;
     }
   }
@@ -117,12 +111,6 @@ void Sensor::checkAll(Print *stream){
   bool pause = false;
   while (readingSensor != NULL && !pause) {
 
-#ifdef USE_NOTIFY
-    // Check if we have reached the start of the polled portion of the sensor list.
-    if (readingSensor == firstPollSensor)
-      pollSignalPhase = true;
-#endif
-
     // Where the sensor is attached to a pin, read pin status.  For sources such as LCN,
     // which don't have an input pin to read, the LCN class calls setState() to update inputState when
     // a message is received.  The IODevice::read() call returns 1 for active pins (0v) and 0 for inactive (5v).
@@ -130,10 +118,8 @@ void Sensor::checkAll(Print *stream){
     // routine when an input signal change is detected, and this updates the inputState directly,
     // so these inputs don't need to be polled here.
     VPIN pin = readingSensor->data.pin;
-#ifdef USE_NOTIFY
-    if (pollSignalPhase)
-#endif
-      if (pin!=VPIN_NONE) readingSensor->inputState = IODevice::read(pin);
+    if (readingSensor->pollingRequired && pin != VPIN_NONE)
+      readingSensor->inputState = IODevice::read(pin);
 
     // Check if changed since last time, and process changes.
     if (readingSensor->inputState == readingSensor->active) {
@@ -156,22 +142,12 @@ void Sensor::checkAll(Print *stream){
     // Move to next sensor in list.
     readingSensor = readingSensor->nextSensor;
 
-    // Currently process max of 16 sensors per entry for polled sensors, and
-    //  16 per entry for sensors notified by callback.
-    // Performance measurements taken during development indicate that, with 64 sensors configured
-    // on 8x 8-pin PCF8574 GPIO expanders, all inputs can be read within 1.4ms (400Mhz I2C bus speed), and a
-    // full cycle of scanning 64 sensors for changes takes between 1.9 and 3.2 milliseconds.
+    // Currently process max of 16 sensors per entry.
+    // Performance measurements taken during development indicate that, with 128 sensors configured
+    // on 8x 16-pin MCP23017 GPIO expanders with polling (no change notification), all inputs can be read from the devices
+    // within 1.4ms (400Mhz I2C bus speed), and a full cycle of checking 128 sensors for changes takes under a millisecond.
     sensorCount++;
-#ifdef USE_NOTIFY
-    if (pollSignalPhase) {
-#endif
-      if (sensorCount >= 16) pause = true;
-#ifdef USE_NOTIFY
-    } else 
-    {
-      if (sensorCount >= 16) pause = true;
-    }
-#endif
+    if (sensorCount >= 16) pause = true;
   }
 
 } // Sensor::checkAll
@@ -223,23 +199,18 @@ Sensor *Sensor::create(int snum, VPIN pin, int pullUp){
   tt = (Sensor *)calloc(1,sizeof(Sensor));
   if (!tt) return tt;     // memory allocation failure
 
-#ifdef USE_NOTIFY
-  if (pin == VPIN_NONE || IODevice::hasCallback(pin)) {
-    // Callback available, or no pin to read, so link sensor on to the start of the list
-    tt->nextSensor = firstSensor;
-    firstSensor = tt;
-    if (lastSensor == NULL) lastSensor = tt;  // This is only item in list.
-  } else {
-    // No callback, so add to end of list so it's polled.
-    if (lastSensor != NULL) lastSensor->nextSensor = tt;
-    lastSensor = tt;
-    if (!firstSensor) firstSensor = tt;
-    if (!firstPollSensor) firstPollSensor = tt;
-  }
-#else
+  if (pin == VPIN_NONE) 
+    tt->pollingRequired = false;
+  #ifdef USE_NOTIFY
+  else if (IODevice::hasCallback(pin)) 
+    tt->pollingRequired = false;
+  #endif
+  else 
+    tt->pollingRequired = true;
+
+  // Add to the start of the list
   tt->nextSensor = firstSensor;
   firstSensor = tt;
-#endif
 
   tt->data.snum = snum;
   tt->data.pin = pin;
@@ -248,9 +219,8 @@ Sensor *Sensor::create(int snum, VPIN pin, int pullUp){
   tt->inputState = 0;
   tt->latchDelay = minReadCount;
 
-  int params[] = {pullUp};
   if (pin != VPIN_NONE) 
-    IODevice::configure(pin, IODevice::CONFIGURE_INPUT, 1, params);   
+    IODevice::configureInput(pin, pullUp);   
     // Generally, internal pull-up resistors are not, on their own, sufficient 
     // for external infrared sensors --- each sensor must have its own 1K external pull-up resistor
 
@@ -310,7 +280,8 @@ void Sensor::load(){
   struct SensorData data;
   Sensor *tt;
 
-  for(uint16_t i=0;i<EEStore::eeStore->data.nSensors;i++){
+  uint16_t i=EEStore::eeStore->data.nSensors;
+  while(i--){
     EEPROM.get(EEStore::pointer(),data);
     tt=create(data.snum, data.pin, data.pullUp);
     EEStore::advance(sizeof(tt->data));
@@ -342,6 +313,5 @@ unsigned long Sensor::lastReadCycle=0;
 #ifdef USE_NOTIFY
 Sensor *Sensor::firstPollSensor = NULL;
 Sensor *Sensor::lastSensor = NULL;
-bool Sensor::pollSignalPhase = false;
 bool Sensor::inputChangeCallbackRegistered = false;
 #endif

Sensors.h

@@ -45,14 +45,6 @@ struct SensorData {
 class Sensor{
   // The sensor list is a linked list where each sensor's 'nextSensor' field points to the next.
   //   The pointer is null in the last on the list.
-  //   To partition the sensor into those sensors which require polling through cyclic calls
-  //   to 'IODevice::read(vpin)', and those which support callback on change, 'firstSensor' 
-  //   points to the start of the overall list, and 'lastSensor' points to the end of the list
-  //   (the last sensor object). This structure allows sensors to be added to the start or the
-  //   end of the list easily.  So if an input pin supports change notification, it is placed at the 
-  //   end of the list.  If not, it is placed at the beginning.  And the pointer 'firstPollSensor' 
-  //   is set to the first of the sensor objects that requires scanning.  Thus, we can iterate
-  //   through the whole list, or just through the part that requires scanning.
 
 public:
   SensorData data;
@@ -74,6 +66,7 @@ public:
   // Constructor
   Sensor(); 
   Sensor *nextSensor;
+
   void setState(int state);
   static void load();
   static void store();
@@ -88,9 +81,9 @@ public:
   static const unsigned int minReadCount = 1; // number of additional scans before acting on change
                                         // E.g. 1 means that a change is ignored for one scan and actioned on the next.
                                         // Max value is 63
+  bool pollingRequired = true;
 
 #ifdef USE_NOTIFY
-  static bool pollSignalPhase;
   static void inputChangeCallback(VPIN vpin, int state);
   static bool inputChangeCallbackRegistered;
 #endif

Turnouts.cpp

@@ -1,4 +1,5 @@
 /*
+ *  © 2021 Restructured Neil McKechnie
  *  © 2013-2016 Gregg E. Berman
  *  © 2020, Chris Harlow. All rights reserved.
  *  © 2020, Harald Barth.
@@ -19,390 +20,506 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-// >>>>>> ATTENTION: This class requires major cleaning.
-// The public interface has been narrowed to avoid the ambuguity of "activated".
 
-
- 
-//#define EESTOREDEBUG 
-#include "defines.h"
-#include "Turnouts.h"
+#include "defines.h"  // includes config.h
 #include "EEStore.h"
 #include "StringFormatter.h"
 #include "RMFT2.h"
+#include "Turnouts.h"
+#include "DCC.h"
+#include "LCN.h"
 #ifdef EESTOREDEBUG
 #include "DIAG.h"
 #endif
 
-// Keywords used for turnout configuration.
-const int16_t HASH_KEYWORD_SERVO=27709;
-const int16_t HASH_KEYWORD_DCC=6436;
-const int16_t HASH_KEYWORD_VPIN=-415;
+  /* 
+   * Protected static data
+   */ 
 
-enum unit8_t {
-  TURNOUT_DCC = 1,
-  TURNOUT_SERVO = 2,
-  TURNOUT_VPIN = 3,
-  TURNOUT_LCN = 4,
-};
-
-
-
-
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function to print all Turnout states to stream in form "<H id state>"
-
-void Turnout::printAll(Print *stream){
-  for (Turnout *tt = Turnout::firstTurnout; tt != NULL; tt = tt->nextTurnout)
-    StringFormatter::send(stream, F("<H %d %d>\n"), tt->data.id, tt->data.active);
-} // Turnout::printAll
-
-///////////////////////////////////////////////////////////////////////////////
-// Object method to print configuration of one Turnout to stream, in one of the following forms:
-//  <H id SERVO vpin activePos inactivePos profile state>
-//  <H id LCN state>
-//  <H id VPIN vpin state>
-//  <H id DCC address subAddress state>
-
-void Turnout::print(Print *stream){
-  uint8_t state = ((data.active) != 0);
-  uint8_t type = data.type;
-  switch (type) {
-    case TURNOUT_LCN:
-      // LCN Turnout
-      StringFormatter::send(stream, F("<H %d LCN %d>\n"), data.id, state);
-      break;
-    case TURNOUT_DCC:
-      // DCC Turnout
-      StringFormatter::send(stream, F("<H %d DCC %d %d %d>\n"), data.id, 
-        (((data.dccAccessoryData.address-1) >> 2)+1), ((data.dccAccessoryData.address-1) & 3), state);
-      break;
-    case TURNOUT_VPIN:
-      // VPIN Digital output
-      StringFormatter::send(stream, F("<H %d VPIN %d %d>\n"), data.id, data.vpinData.vpin, state);
-      break;
-    case TURNOUT_SERVO:
-      // Servo Turnout
-      StringFormatter::send(stream, F("<H %d SERVO %d %d %d %d %d>\n"), data.id, data.servoData.vpin, 
-        data.servoData.activePosition, data.servoData.inactivePosition, data.servoData.profile, state);
-      break;
-    default:
-      break;
-  }
-}
-
-
-// Public interface to turnout throw/close
-bool Turnout::setClosed(int id, bool closed) {
-   // hides the internal activate argument to a single place
-   return activate(id, closed? false: true );  /// Needs cleaning up 
-}
-bool Turnout::isClosed(int id) {
-   // hides the internal activate argument to a single place
-   return !isActive(id);  /// Needs cleaning up 
-}
-int Turnout::getId() {
-  return data.id;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function to activate/deactivate Turnout with ID 'n'.
-//   Returns false if turnout not found.
+  /* static */ Turnout *Turnout::_firstTurnout = 0;
 
+  /* 
+   * Public static data
+   */
+  /* static */ int Turnout::turnoutlistHash = 0;
  
-bool Turnout::activate(int n, bool state){
-  Turnout * tt=get(n);
-  if (!tt) return false;
-  tt->activate(state);
-  turnoutlistHash++;
-  return true;
-}
+  /*
+   * Protected static functions
+   */
 
-///////////////////////////////////////////////////////////////////////////////
-// Static function to check if the Turnout with ID 'n' is activated or not.
-// Returns false if turnout not found.
-
-bool Turnout::isActive(int n){
-  Turnout * tt=get(n);
-  if (!tt) return false;
-  return tt->isActive();
-}
-
-
-///////////////////////////////////////////////////////////////////////////////
-// Object function to check the status of Turnout is activated or not.
-
-bool Turnout::isActive() {
-  return data.active;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Object method to activate or deactivate the Turnout.  
-
-// activate is virtual here so that it can be overridden by a non-DCC turnout mechanism
-void Turnout::activate(bool state) {
-#ifdef EESTOREDEBUG
-  DIAG(F("Turnout::activate(%d)"),state);
-#endif
-  if (data.type == TURNOUT_LCN) {
-      // A LCN turnout is transmitted to the LCN master.
-      LCN::send('T', data.id, state);
-      return;   // The tStatus will be updated by a message from the LCN master, later.
-  }
-    data.active = state;
-    switch (data.type) {
-      case TURNOUT_DCC:
-        DCC::setAccessory((((data.dccAccessoryData.address-1) >> 2) + 1), 
-          ((data.dccAccessoryData.address-1) & 3), state);
-        break;
-      case TURNOUT_SERVO:
-#ifndef IO_NO_HAL
-        IODevice::write(data.servoData.vpin, state);
-#endif
-        break;
-      case TURNOUT_VPIN:
-        IODevice::write(data.vpinData.vpin, state);
-        break;
-    }
-    // Save state if stored in EEPROM
-    if (EEStore::eeStore->data.nTurnouts > 0 && num > 0) 
-      EEPROM.put(num, data.tStatus);
-
-#if defined(RMFT_ACTIVE)
-  RMFT2::turnoutEvent(data.id, !state);
-#endif  
-
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function to find Turnout object specified by ID 'n'.  Return NULL if not found.
-
-Turnout* Turnout::get(int n){
-  Turnout *tt;
-  for(tt=firstTurnout;tt!=NULL && tt->data.id!=n;tt=tt->nextTurnout);
-  return(tt);
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function to delete Turnout object specified by ID 'n'.  Return false if not found.
-
-bool Turnout::remove(int n){
-  Turnout *tt,*pp=NULL;
-
-  for(tt=firstTurnout;tt!=NULL && tt->data.id!=n;pp=tt,tt=tt->nextTurnout);
-
-  if(tt==NULL) return false;
-  
-  if(tt==firstTurnout)
-    firstTurnout=tt->nextTurnout;
-  else
-    pp->nextTurnout=tt->nextTurnout;
-
-  free(tt);
-  turnoutlistHash++;
-  return true; 
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function to load all Turnout definitions from EEPROM
-// TODO: Consider transmitting the initial state of the DCC/LCN turnout here.
-//  (already done for servo turnouts and VPIN turnouts).
-
-void Turnout::load(){
-  struct TurnoutData data;
-  Turnout *tt=NULL;
-
-  for(uint16_t i=0;i<EEStore::eeStore->data.nTurnouts;i++){
-    // Retrieve data
-    EEPROM.get(EEStore::pointer(), data);
-    
-    int lastKnownState = data.active;
-    switch (data.type) {
-      case TURNOUT_DCC:
-        tt=createDCC(data.id, ((data.dccAccessoryData.address-1)>>2)+1, (data.dccAccessoryData.address-1)&3); // DCC-based turnout
-        break;
-      case TURNOUT_LCN:
-        // LCN turnouts are created when the remote device sends a message.
-        break;
-      case TURNOUT_SERVO:
-        tt=createServo(data.id, data.servoData.vpin, 
-          data.servoData.activePosition, data.servoData.inactivePosition, data.servoData.profile, lastKnownState);
-        break;
-      case TURNOUT_VPIN:
-        tt=createVpin(data.id, data.vpinData.vpin, lastKnownState);  // VPIN-based turnout
-        break;
-       
-      default:
-       tt=NULL;
-    }
-    if (tt) tt->num = EEStore::pointer() + offsetof(TurnoutData, tStatus);  // Save pointer to tStatus byte within EEPROM
-    // Advance by the actual size of the individual turnout struct.
-    EEStore::advance(data.size);
-#ifdef EESTOREDEBUG
-    if (tt) print(tt);
-#endif
-  }
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function to store all Turnout definitions to EEPROM
-
-void Turnout::store(){
-  Turnout *tt;
-
-  tt=firstTurnout;
-  EEStore::eeStore->data.nTurnouts=0;
-
-  while(tt!=NULL){
-    // LCN turnouts aren't saved to EEPROM
-    if (tt->data.type != TURNOUT_LCN) {  
-#ifdef EESTOREDEBUG
-      print(tt);
-#endif
-      tt->num = EEStore::pointer() + offsetof(TurnoutData, tStatus); // Save pointer to tstatus byte within EEPROM
-      EEPROM.put(EEStore::pointer(),tt->data);
-      EEStore::advance(tt->data.size);
-      EEStore::eeStore->data.nTurnouts++;
-    }
-    tt=tt->nextTurnout;
-  }
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function for creating a DCC-controlled Turnout.
-
-Turnout *Turnout::createDCC(int id, uint16_t add, uint8_t subAdd){
-  if (add > 511 || subAdd > 3) return NULL;
-  Turnout *tt=create(id);
-  if (!tt) return(tt);
-  tt->data.type = TURNOUT_DCC;
-  tt->data.size = sizeof(tt->data.header) + sizeof(tt->data.dccAccessoryData);
-  tt->data.active = 0;
-  tt->data.dccAccessoryData.address = ((add-1) << 2) + subAdd + 1;
-  return(tt);
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function for creating a LCN-controlled Turnout.
-
-Turnout *Turnout::createLCN(int id, uint8_t state) {
-  Turnout *tt=create(id);
-  if (!tt) return(tt);
-  tt->data.type = TURNOUT_LCN;
-  tt->data.size = sizeof(tt->data.header) + sizeof(tt->data.lcnData);
-  tt->data.active = (state != 0);
-  return(tt);
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Static function for associating a Turnout id with a virtual pin in IODevice space.
-// The actual creation and configuration of the pin must be done elsewhere,
-// e.g. in mySetup.cpp during startup of the CS.
-
-Turnout *Turnout::createVpin(int id, VPIN vpin, uint8_t state){
-  if (vpin > VPIN_MAX) return NULL;
-  Turnout *tt=create(id);
-  if(!tt) return(tt);
-  tt->data.type = TURNOUT_VPIN;;
-  tt->data.size = sizeof(tt->data.header) + sizeof(tt->data.vpinData);
-  tt->data.active = (state != 0);
-  tt->data.vpinData.vpin = vpin;
-  IODevice::write(vpin, state);   // Set initial state of output.
-  return(tt);
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Method for creating a Servo Turnout, e.g. connected to PCA9685 PWM device.
-
-Turnout *Turnout::createServo(int id, VPIN vpin, uint16_t activePosition, uint16_t inactivePosition, uint8_t profile, uint8_t state){
-#ifndef IO_NO_HAL
-  if (activePosition > 511 || inactivePosition > 511 || profile > 4) return NULL;
-
-  Turnout *tt=create(id);
-  if (!tt) return(tt);
-  if (tt->data.type != TURNOUT_SERVO) tt->data.active = (state != 0);  // Retain current state if it's an existing servo turnout.
-  tt->data.type = TURNOUT_SERVO;
-  tt->data.size = sizeof(tt->data.header) + sizeof(tt->data.servoData);
-  tt->data.servoData.vpin = vpin;
-  tt->data.servoData.activePosition = activePosition;
-  tt->data.servoData.inactivePosition = inactivePosition;
-  tt->data.servoData.profile = profile;
-  // Configure PWM interface device
-  int deviceParams[] = {(int)activePosition, (int)inactivePosition, profile, tt->data.active};
-  if (!IODevice::configure(vpin, IODevice::CONFIGURE_SERVO, 4, deviceParams)) {
-    remove(id);
+  /* static */ Turnout *Turnout::get(uint16_t id) {
+    // Find turnout object from list.
+    for (Turnout *tt = _firstTurnout; tt != NULL; tt = tt->_nextTurnout)
+      if (tt->_turnoutData.id == id) return tt;
     return NULL;
   }
-  return(tt);
-#else
-  (void)id; (void)vpin; (void)activePosition; (void)inactivePosition; (void)profile; (void)state;  // avoid compiler warnings
-  return NULL;
-#endif
-}
 
-///////////////////////////////////////////////////////////////////////////////
-// Support for <T id SERVO pin activepos inactive pos profile>
-// and <T id DCC address subaddress>
-// and <T id VPIN pin>
+  // Add new turnout to end of chain
+  /* static */ void Turnout::add(Turnout *tt) {
+    if (!_firstTurnout) 
+      _firstTurnout = tt;
+    else {
+      // Find last object on chain
+      Turnout *ptr = _firstTurnout;
+      for ( ; ptr->_nextTurnout!=0; ptr=ptr->_nextTurnout) {}
+      // Line new object to last object.
+      ptr->_nextTurnout = tt;
+    }
+    turnoutlistHash++;
+  }
+  
+  // For DCC++ classic compatibility, state reported to JMRI is 1 for thrown and 0 for closed; 
+  void Turnout::printState(Print *stream) { 
+    StringFormatter::send(stream, F("<H %d %d>\n"), 
+      _turnoutData.id, !_turnoutData.closed);
+  }
 
-Turnout *Turnout::create(int id, int params, int16_t p[]) {
-  if (p[0] == HASH_KEYWORD_SERVO) { // <T id SERVO n n n n>
-    if (params == 5)
-      return createServo(id, (VPIN)p[1], (uint16_t)p[2], (uint16_t)p[3], (uint8_t)p[4]);
-    else  
-      return NULL;
-  } else 
-  if (p[0] == HASH_KEYWORD_VPIN) { // <T id VPIN n>
-    if (params==2)
-      return createVpin(id, p[1]);
+  // Remove nominated turnout from turnout linked list and delete the object.
+  /* static */ bool Turnout::remove(uint16_t id) {
+    Turnout *tt,*pp=NULL;
+
+    for(tt=_firstTurnout; tt!=NULL && tt->_turnoutData.id!=id; pp=tt, tt=tt->_nextTurnout) {}
+    if (tt == NULL) return false;
+    
+    if (tt == _firstTurnout)
+      _firstTurnout = tt->_nextTurnout;
     else
-      return NULL;
-  } else
-  if (p[0]==HASH_KEYWORD_DCC) {
-    if (params==3 && p[1]>0 && p[1]<=512 && p[2]>=0 && p[2]<4)  // <T id DCC n n>
-      return createDCC(id, p[1], p[2]);
-    else if (params==2 && p[1]>0 && p[1]<=512*4)  // <T id DCC nn>
-      return createDCC(id, (p[1]-1)/4+1, (p[1]-1)%4);
-    else
-      return NULL;
-  } else if (params==2) { // <T id n n> for DCC or LCN
-    return createDCC(id, p[0], p[1]);
+      pp->_nextTurnout = tt->_nextTurnout;
+
+    delete (ServoTurnout *)tt;
+
+    turnoutlistHash++;
+    return true; 
   } 
-  else if (params==3) { // legacy <T id n n n> for Servo
-    return createServo(id, (VPIN)p[0], (uint16_t)p[1], (uint16_t)p[2]);
+
+
+  /*
+   * Public static functions
+   */
+
+  /* static */ bool Turnout::isClosed(uint16_t id) {
+    Turnout *tt = get(id);
+    if (tt) 
+      return tt->isClosed();
+    else
+      return false;
   }
 
-  return NULL;
-}
+  /* static */ bool Turnout::setClosedStateOnly(uint16_t id, bool closeFlag) {
+    Turnout *tt = get(id);
+    if (!tt) return false;
+    tt->_turnoutData.closed = closeFlag;
 
-///////////////////////////////////////////////////////////////////////////////
-// Create basic Turnout object.  The details of what sort of object it is 
-//  controlling are not set here.
+    // I know it says setClosedStateOnly, but we need to tell others
+    //  that the state has changed too.
+    #if defined(RMFT_ACTIVE)
+      RMFT2::turnoutEvent(id, closeFlag);
+    #endif
 
-Turnout *Turnout::create(int id){
-  Turnout *tt=get(id);
-  if (tt==NULL) { 
-     tt=(Turnout *)calloc(1,sizeof(Turnout));
-     if (!tt) return (tt);
-     tt->nextTurnout=firstTurnout;
-     firstTurnout=tt;
-     tt->data.id=id;
+      // Send message to JMRI etc. over Serial USB.  This is done here
+      // to ensure that the message is sent when the turnout operation
+      // is not initiated by a Serial command.
+      tt->printState(&Serial);
+    return true;
   }
-  turnoutlistHash++;
-  return tt;
-}
 
-///////////////////////////////////////////////////////////////////////////////
-//
-// Object method to print debug info about the state of a Turnout object
-//
+
+  // Static setClosed function is invoked from close(), throw() etc. to perform the 
+  //  common parts of the turnout operation.  Code which is specific to a turnout
+  //  type should be placed in the virtual function setClosedInternal(bool) which is
+  //  called from here.
+  /* static */ bool Turnout::setClosed(uint16_t id, bool closeFlag) { 
+  #if defined(DIAG_IO)
+    if (closeFlag) 
+      DIAG(F("Turnout::close(%d)"), id);
+    else
+      DIAG(F("Turnout::throw(%d)"), id);
+  #endif
+    Turnout *tt = Turnout::get(id);
+    if (!tt) return false;
+    bool ok = tt->setClosedInternal(closeFlag);
+
+    if (ok) {
+      turnoutlistHash++;  // let withrottle know something changed
+    
+      // Write byte containing new closed/thrown state to EEPROM if required.  Note that eepromAddress
+      // is always zero for LCN turnouts.
+      if (EEStore::eeStore->data.nTurnouts > 0 && tt->_eepromAddress > 0) 
+        EEPROM.put(tt->_eepromAddress, tt->_turnoutData.flags);  
+
+    #if defined(RMFT_ACTIVE)
+      RMFT2::turnoutEvent(id, closeFlag);
+    #endif
+
+      // Send message to JMRI etc. over Serial USB.  This is done here
+      // to ensure that the message is sent when the turnout operation
+      // is not initiated by a Serial command.
+      tt->printState(&Serial);
+    }
+    return ok;
+  }
+
+  // Load all turnout objects
+  /* static */ void Turnout::load() {
+    for (uint16_t i=0; i<EEStore::eeStore->data.nTurnouts; i++) {
+      Turnout::loadTurnout();
+    }
+  }
+
+  // Save all turnout objects
+  /* static */ void Turnout::store() {
+    EEStore::eeStore->data.nTurnouts=0;
+    for (Turnout *tt = _firstTurnout; tt != 0; tt = tt->_nextTurnout) {
+      tt->save();
+      EEStore::eeStore->data.nTurnouts++;
+    }
+  }
+
+  // Load one turnout from EEPROM
+  /* static */ Turnout *Turnout::loadTurnout () {
+    Turnout *tt = 0;
+    // Read turnout type from EEPROM
+    struct TurnoutData turnoutData;
+    int eepromAddress = EEStore::pointer() + offsetof(struct TurnoutData, flags); // Address of byte containing the closed flag.
+    EEPROM.get(EEStore::pointer(), turnoutData);
+    EEStore::advance(sizeof(turnoutData));
+
+    switch (turnoutData.turnoutType) {
+      case TURNOUT_SERVO:
+        // Servo turnout
+        tt = ServoTurnout::load(&turnoutData);
+        break;
+      case TURNOUT_DCC:
+        // DCC Accessory turnout
+        tt = DCCTurnout::load(&turnoutData);
+        break;
+      case TURNOUT_VPIN:
+        // VPIN turnout
+        tt = VpinTurnout::load(&turnoutData);
+        break;
+      default:
+        // If we find anything else, then we don't know what it is or how long it is, 
+        // so we can't go any further through the EEPROM!
+        return NULL;
+    }
+    if (tt) {
+      // Save EEPROM address in object.  Note that LCN turnouts always have eepromAddress of zero.
+      tt->_eepromAddress = eepromAddress + offsetof(struct TurnoutData, flags);
+    }
+
 #ifdef EESTOREDEBUG
-void Turnout::print(Turnout *tt) {
-  tt->print(StringFormatter::diagSerial);
-}
+    printAll(&Serial);
+#endif
+    return tt;
+  }
+
+  // Display, on the specified stream, the current state of the turnout (1=thrown or 0=closed).
+  /* static */ void Turnout::printState(uint16_t id, Print *stream) {
+    Turnout *tt = get(id);
+    if (tt) tt->printState(stream);
+  }
+
+
+/*************************************************************************************
+ * ServoTurnout - Turnout controlled by servo device.
+ * 
+ *************************************************************************************/
+
+  // Private Constructor
+  ServoTurnout::ServoTurnout(uint16_t id, VPIN vpin, uint16_t thrownPosition, uint16_t closedPosition, uint8_t profile, bool closed) :
+    Turnout(id, TURNOUT_SERVO, closed) 
+  {
+    _servoTurnoutData.vpin = vpin;
+    _servoTurnoutData.thrownPosition = thrownPosition; 
+    _servoTurnoutData.closedPosition = closedPosition;
+    _servoTurnoutData.profile = profile;
+  }
+
+  // Create function
+  /* static */ Turnout *ServoTurnout::create(uint16_t id, VPIN vpin, uint16_t thrownPosition, uint16_t closedPosition, uint8_t profile, bool closed) {
+#ifndef IO_NO_HAL
+    Turnout *tt = get(id);
+    if (tt) { 
+      // Object already exists, check if it is usable
+      if (tt->isType(TURNOUT_SERVO)) {
+        // Yes, so set parameters
+        ServoTurnout *st = (ServoTurnout *)tt;
+        st->_servoTurnoutData.vpin = vpin;
+        st->_servoTurnoutData.thrownPosition = thrownPosition;
+        st->_servoTurnoutData.closedPosition = closedPosition;
+        st->_servoTurnoutData.profile = profile;
+        // Don't touch the _closed parameter, retain the original value.
+
+        // We don't really need to do the following, since a call to IODevice::_writeAnalogue 
+        //  will provide all the data that is required!  However, if someone has configured 
+        //  a Turnout, we should ensure that the SET() RESET() and other commands that use write() 
+        //  behave consistently with the turnout commands.
+        IODevice::configureServo(vpin, thrownPosition, closedPosition, profile, 0, closed);
+
+        // Set position directly to specified position - we don't know where it is moving from.
+        IODevice::writeAnalogue(vpin, closed ? closedPosition : thrownPosition, PCA9685::Instant);
+
+        return tt;
+      } else {
+        // Incompatible object, delete and recreate
+        remove(id);
+      }
+    }
+    tt = (Turnout *)new ServoTurnout(id, vpin, thrownPosition, closedPosition, profile, closed);
+    IODevice::writeAnalogue(vpin, closed ? closedPosition : thrownPosition, PCA9685::Instant);
+    return tt;
+#else
+    (void)id; (void)vpin; (void)thrownPosition; (void)closedPosition;
+    (void)profile; (void)closed;          // avoid compiler warnings.
+    return NULL;
+#endif
+  }
+
+  // Load a Servo turnout definition from EEPROM.  The common Turnout data has already been read at this point.
+  Turnout *ServoTurnout::load(struct TurnoutData *turnoutData) {
+    ServoTurnoutData servoTurnoutData;
+    // Read class-specific data from EEPROM
+    EEPROM.get(EEStore::pointer(), servoTurnoutData);
+    EEStore::advance(sizeof(servoTurnoutData));
+    
+    // Create new object
+    Turnout *tt = ServoTurnout::create(turnoutData->id, servoTurnoutData.vpin, servoTurnoutData.thrownPosition,
+      servoTurnoutData.closedPosition, servoTurnoutData.profile, turnoutData->closed);
+    return tt;
+  }
+
+  // For DCC++ classic compatibility, state reported to JMRI is 1 for thrown and 0 for closed
+  void ServoTurnout::print(Print *stream) {
+    StringFormatter::send(stream, F("<H %d SERVO %d %d %d %d %d>\n"), _turnoutData.id, _servoTurnoutData.vpin, 
+      _servoTurnoutData.thrownPosition, _servoTurnoutData.closedPosition, _servoTurnoutData.profile, 
+      !_turnoutData.closed);
+  }
+
+  // ServoTurnout-specific code for throwing or closing a servo turnout.
+  bool ServoTurnout::setClosedInternal(bool close) {
+#ifndef IO_NO_HAL
+    IODevice::writeAnalogue(_servoTurnoutData.vpin, 
+      close ? _servoTurnoutData.closedPosition : _servoTurnoutData.thrownPosition, _servoTurnoutData.profile);
+    _turnoutData.closed = close;
+#else
+    (void)close;  // avoid compiler warnings
+#endif
+    return true;
+  }
+
+  void ServoTurnout::save() {
+    // Write turnout definition and current position to EEPROM
+    // First write common servo data, then
+    // write the servo-specific data
+    EEPROM.put(EEStore::pointer(), _turnoutData);
+    EEStore::advance(sizeof(_turnoutData));
+    EEPROM.put(EEStore::pointer(), _servoTurnoutData);
+    EEStore::advance(sizeof(_servoTurnoutData));
+  }
+
+/*************************************************************************************
+ * DCCTurnout - Turnout controlled by DCC Accessory Controller.
+ * 
+ *************************************************************************************/
+
+#if defined(DCC_TURNOUTS_RCN_213)
+  const bool DCCTurnout::rcn213Compliant = true;
+#else
+  const bool DCCTurnout::rcn213Compliant = false;
 #endif
 
-///////////////////////////////////////////////////////////////////////////////
-Turnout *Turnout::firstTurnout=NULL;
-int Turnout::turnoutlistHash=0; //bump on every change so clients know when to refresh their lists
+  // DCCTurnoutData contains data specific to this subclass that is 
+  // written to EEPROM when the turnout is saved.
+  struct DCCTurnoutData {
+    // DCC address (Address in bits 15-2, subaddress in bits 1-0
+    uint16_t address; // CS currently supports linear address 1-2048
+      // That's DCC accessory address 1-512 and subaddress 0-3.
+  } _dccTurnoutData; // 2 bytes
+
+  // Constructor
+  DCCTurnout::DCCTurnout(uint16_t id, uint16_t address, uint8_t subAdd) :
+    Turnout(id, TURNOUT_DCC, false)
+  {
+    _dccTurnoutData.address = address;
+    _dccTurnoutData.subAddress = subAdd;
+  }
+
+  // Create function
+  /* static */ Turnout *DCCTurnout::create(uint16_t id, uint16_t add, uint8_t subAdd) {
+    Turnout *tt = get(id);
+    if (tt) { 
+      // Object already exists, check if it is usable
+      if (tt->isType(TURNOUT_DCC)) {
+        // Yes, so set parameters<T>
+        DCCTurnout *dt = (DCCTurnout *)tt;
+        dt->_dccTurnoutData.address = add;
+        dt->_dccTurnoutData.subAddress = subAdd;
+        // Don't touch the _closed parameter, retain the original value.
+        return tt;
+      } else {
+        // Incompatible object, delete and recreate
+        remove(id);
+      }
+    }
+    tt = (Turnout *)new DCCTurnout(id, add, subAdd);
+    return tt;
+  }
+
+  // Load a DCC turnout definition from EEPROM.  The common Turnout data has already been read at this point.
+  /* static */ Turnout *DCCTurnout::load(struct TurnoutData *turnoutData) {
+    DCCTurnoutData dccTurnoutData;
+    // Read class-specific data from EEPROM
+    EEPROM.get(EEStore::pointer(), dccTurnoutData);
+    EEStore::advance(sizeof(dccTurnoutData));
+    
+    // Create new object
+    DCCTurnout *tt = new DCCTurnout(turnoutData->id, dccTurnoutData.address, dccTurnoutData.subAddress);
+
+    return tt;
+  }
+
+  void DCCTurnout::print(Print *stream) {
+    StringFormatter::send(stream, F("<H %d DCC %d %d %d>\n"), _turnoutData.id, 
+      _dccTurnoutData.address, _dccTurnoutData.subAddress, !_turnoutData.closed); 
+    // Also report using classic DCC++ syntax for DCC accessory turnouts, since JMRI expects this.
+    StringFormatter::send(stream, F("<H %d %d %d %d>\n"), _turnoutData.id, 
+      _dccTurnoutData.address, _dccTurnoutData.subAddress, !_turnoutData.closed); 
+  }
+
+  bool DCCTurnout::setClosedInternal(bool close) {
+    // DCC++ Classic behaviour is that Throw writes a 1 in the packet,
+    // and Close writes a 0.  
+    // RCN-213 specifies that Throw is 0 and Close is 1.
+    DCC::setAccessory(_dccTurnoutData.address, _dccTurnoutData.subAddress, close ^ !rcn213Compliant);
+    _turnoutData.closed = close;
+    return true;
+  }
+
+  void DCCTurnout::save() {
+    // Write turnout definition and current position to EEPROM
+    // First write common servo data, then
+    // write the servo-specific data
+    EEPROM.put(EEStore::pointer(), _turnoutData);
+    EEStore::advance(sizeof(_turnoutData));
+    EEPROM.put(EEStore::pointer(), _dccTurnoutData);
+    EEStore::advance(sizeof(_dccTurnoutData));
+  }
+
+
+
+/*************************************************************************************
+ * VpinTurnout - Turnout controlled through a HAL vpin.
+ * 
+ *************************************************************************************/
+
+  // Constructor
+  VpinTurnout::VpinTurnout(uint16_t id, VPIN vpin, bool closed) :
+    Turnout(id, TURNOUT_VPIN, closed)
+  {
+    _vpinTurnoutData.vpin = vpin;
+  }
+
+  // Create function
+  /* static */ Turnout *VpinTurnout::create(uint16_t id, VPIN vpin, bool closed) {
+    Turnout *tt = get(id);
+    if (tt) { 
+      // Object already exists, check if it is usable
+      if (tt->isType(TURNOUT_VPIN)) {
+        // Yes, so set parameters
+        VpinTurnout *vt = (VpinTurnout *)tt;
+        vt->_vpinTurnoutData.vpin = vpin;
+        // Don't touch the _closed parameter, retain the original value.
+        return tt;
+      } else {
+        // Incompatible object, delete and recreate
+        remove(id);
+      }
+    }
+    tt = (Turnout *)new VpinTurnout(id, vpin, closed);
+    return tt;
+  }
+
+  // Load a VPIN turnout definition from EEPROM.  The common Turnout data has already been read at this point.
+  /* static */ Turnout *VpinTurnout::load(struct TurnoutData *turnoutData) {
+    VpinTurnoutData vpinTurnoutData;
+    // Read class-specific data from EEPROM
+    EEPROM.get(EEStore::pointer(), vpinTurnoutData);
+    EEStore::advance(sizeof(vpinTurnoutData));
+    
+    // Create new object
+    VpinTurnout *tt = new VpinTurnout(turnoutData->id, vpinTurnoutData.vpin, turnoutData->closed);
+
+    return tt;
+  }
+
+  // Report 1 for thrown, 0 for closed.
+  void VpinTurnout::print(Print *stream) {
+    StringFormatter::send(stream, F("<H %d VPIN %d %d>\n"), _turnoutData.id, _vpinTurnoutData.vpin, 
+      !_turnoutData.closed); 
+  }
+
+  bool VpinTurnout::setClosedInternal(bool close) {
+    IODevice::write(_vpinTurnoutData.vpin, close);
+    _turnoutData.closed = close;
+    return true;
+  }
+
+  void VpinTurnout::save() {
+    // Write turnout definition and current position to EEPROM
+    // First write common servo data, then
+    // write the servo-specific data
+    EEPROM.put(EEStore::pointer(), _turnoutData);
+    EEStore::advance(sizeof(_turnoutData));
+    EEPROM.put(EEStore::pointer(), _vpinTurnoutData);
+    EEStore::advance(sizeof(_vpinTurnoutData));
+  }
+
+
+/*************************************************************************************
+ * LCNTurnout - Turnout controlled by Loconet
+ * 
+ *************************************************************************************/
+
+  // LCNTurnout has no specific data, and in any case is not written to EEPROM!
+  // struct LCNTurnoutData {
+  // } _lcnTurnoutData; // 0 bytes
+
+  // Constructor
+  LCNTurnout::LCNTurnout(uint16_t id, bool closed) :
+    Turnout(id, TURNOUT_LCN, closed)
+  { }
+
+  // Create function
+  /* static */ Turnout *LCNTurnout::create(uint16_t id, bool closed) {
+    Turnout *tt = get(id);
+    if (tt) { 
+      // Object already exists, check if it is usable
+      if (tt->isType(TURNOUT_LCN)) {
+        // Yes, so return this object
+        return tt;
+      } else {
+        // Incompatible object, delete and recreate
+        remove(id);
+      }
+    }
+    tt = (Turnout *)new LCNTurnout(id, closed);
+    return tt;
+  }
+
+  bool LCNTurnout::setClosedInternal(bool close) {
+    // Assume that the LCN command still uses 1 for throw and 0 for close...
+    LCN::send('T', _turnoutData.id, !close);
+    // The _turnoutData.closed flag should be updated by a message from the LCN master, later.
+    return true;
+  }
+
+  // LCN turnouts not saved to EEPROM.
+  //void save() override {  }
+  //static Turnout *load(struct TurnoutData *turnoutData) {
+
+  // Report 1 for thrown, 0 for closed.
+  void LCNTurnout::print(Print *stream) {
+    StringFormatter::send(stream, F("<H %d LCN %d>\n"), _turnoutData.id, 
+    !_turnoutData.closed); 
+  }
+

Turnouts.h

@@ -1,4 +1,6 @@
 /*
+ *  © 2021 Restructured Neil McKechnie
+ *  © 2013-2016 Gregg E. Berman
  *  © 2020, Chris Harlow. All rights reserved.
  *  
  *  This file is part of Asbelos DCC API
@@ -17,109 +19,282 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-/* 
- * Turnout data is stored in a structure whose length depends on the
- * type of turnout.  There is a common header of 3 bytes, followed by
- * 2 bytes for DCC turnout, 5 bytes for servo turnout, 2 bytes for a
- * VPIN turnout, or zero bytes for an LCN turnout.
- * The variable length allows the limited space in EEPROM to be used effectively.
- */
+#ifndef TURNOUTS_H
+#define TURNOUTS_H
 
-#ifndef Turnouts_h
-#define Turnouts_h
-
-#include <Arduino.h>
-#include "DCC.h"
-#include "LCN.h"
+//#define EESTOREDEBUG 
+#include "Arduino.h"
 #include "IODevice.h"
 
 
-const byte STATUS_ACTIVE=0x80; // Flag as activated in tStatus field
-const byte STATUS_TYPE = 0x7f;  // Mask for turnout type in tStatus field
-
-//  The struct 'header' is used to determine the length of the
-//  overlaid data so must be at least as long as the anonymous fields it
-//  is overlaid with.
-struct TurnoutData {
-  // Header common to all turnouts
-  union {
-    struct {
-      int id;
-      uint8_t tStatus;
-      uint8_t size;
-    } header;
-
-    struct {
-      int id;
-      union {
-        uint8_t tStatus;
-        struct {
-          uint8_t active: 1;
-          uint8_t type: 5;
-          uint8_t :2;
-        };
-      };
-      uint8_t size;  // set to actual total length of used structure
-    };
-  };
-  // Turnout-type-specific structure elements, different length depending
-  //  on turnout type.  This allows the data to be packed efficiently 
-  //  in the EEPROM.
-  union {
-    struct {
-      // DCC address (Address in bits 15-2, subaddress in bits 1-0
-      uint16_t address; // CS currently supports linear address 1-2048
-        // That's DCC accessory address 1-512 and subaddress 0-3.
-    } dccAccessoryData;
-
-    struct {
-      VPIN vpin;
-      uint16_t activePosition : 12;  // 0-4095
-      uint16_t inactivePosition : 12; // 0-4095
-      uint8_t profile;
-    } servoData;
-
-    struct {
-    } lcnData;
-
-    struct {
-      VPIN vpin;
-    } vpinData;
-  };
+// Turnout type definitions
+enum {
+  TURNOUT_DCC = 1,
+  TURNOUT_SERVO = 2,
+  TURNOUT_VPIN = 3,
+  TURNOUT_LCN = 4,
 };
 
+/*************************************************************************************
+ * Turnout - Base class for turnouts.
+ * 
+ *************************************************************************************/
+
 class Turnout {
-public:
-  static Turnout *firstTurnout;
-  static int turnoutlistHash;
-  Turnout *nextTurnout;
-  static Turnout* get(int);
-  static bool remove(int);
-  static bool isClosed(int);
-  static bool setClosed(int n, bool closed);  // return false if not found. 
-  static void setClosedStateOnly(int n, bool closed);
-  int getId();
-  static void load();
-  static void store();
-  static Turnout *createServo(int id , VPIN vpin , uint16_t activeAngle, uint16_t inactiveAngle, uint8_t profile=1, uint8_t initialState=0);
-  static Turnout *createVpin(int id, VPIN vpin, uint8_t initialState=0);
-  static Turnout *createDCC(int id, uint16_t address, uint8_t subAddress);
-  static Turnout *createLCN(int id, uint8_t initialState=0);
-  static Turnout *create(int id, int params, int16_t p[]);
-  static Turnout *create(int id);
-  static void printAll(Print *);
-  void print(Print *stream);
-#ifdef EESTOREDEBUG
-  static void print(Turnout *tt);
-#endif
-private:
-  int num;  // EEPROM address of tStatus in TurnoutData struct, or zero if not stored.
-  TurnoutData data;
-   static bool activate(int n, bool  thrown);
-   static bool isActive(int);
-   bool isActive();
-   void activate(bool state);
-   void setActive(bool state);
-  }; // Turnout
+protected:
+  /* 
+   * Object data
+   */
+
+  // The TurnoutData struct contains data common to all turnout types, that 
+  // is written to EEPROM when the turnout is saved.
+  // The first byte of this struct contains the 'closed' flag which is
+  // updated whenever the turnout changes from thrown to closed and
+  // vice versa.  If the turnout has been saved, then this byte is rewritten
+  // when changed in RAM.  The 'closed' flag must be located in the first byte.
+  struct TurnoutData {
+    union {
+      struct {
+        bool closed : 1;
+        bool _rfu: 2;
+        uint8_t turnoutType : 5;
+      };
+      uint8_t flags;
+    };
+    uint16_t id;
+  } _turnoutData;  // 3 bytes
+
+  // Address in eeprom of first byte of the _turnoutData struct (containing the closed flag).
+  // Set to zero if the object has not been saved in EEPROM, e.g. for newly created Turnouts, and 
+  // for all LCN turnouts.
+  uint16_t _eepromAddress = 0;
+
+  // Pointer to next turnout on linked list.
+  Turnout *_nextTurnout = 0;
+
+  /*
+   * Constructor
+   */
+  Turnout(uint16_t id, uint8_t turnoutType, bool closed) {
+    _turnoutData.id = id;
+    _turnoutData.turnoutType = turnoutType;
+    _turnoutData.closed = closed;
+    add(this);
+  }
+
+  /* 
+   * Static data
+   */ 
+
+  static Turnout *_firstTurnout;
+  static int _turnoutlistHash;
+
+  /* 
+   * Virtual functions
+   */
+
+  virtual bool setClosedInternal(bool close) = 0;  // Mandatory in subclass
+  virtual void save() {}
   
+  /*
+   * Static functions
+   */
+
+  static Turnout *get(uint16_t id);
+
+  static void add(Turnout *tt);
+  
+public:
+  /* 
+   * Static data
+   */
+  static int turnoutlistHash;
+  static const bool useClassicTurnoutCommands;
+  
+  /*
+   * Public base class functions
+   */
+  inline bool isClosed() { return _turnoutData.closed; };
+  inline bool isThrown() { return !_turnoutData.closed; }
+  inline bool isType(uint8_t type) { return _turnoutData.turnoutType == type; }
+  inline uint16_t getId() { return _turnoutData.id; }
+  inline Turnout *next() { return _nextTurnout; }
+  void printState(Print *stream);
+  /* 
+   * Virtual functions
+   */
+  virtual void print(Print *stream) {
+    (void)stream;  // avoid compiler warnings.
+  }
+  virtual ~Turnout() {}   // Destructor
+
+  /*
+   * Public static functions
+   */
+  inline static bool exists(uint16_t id) { return get(id) != 0; }
+
+  static bool remove(uint16_t id);
+
+  static bool isClosed(uint16_t id);
+
+  inline static bool isThrown(uint16_t id) {
+    return !isClosed(id);
+  }
+
+  static bool setClosed(uint16_t id, bool closeFlag);
+
+  inline static bool setClosed(uint16_t id) {
+    return setClosed(id, true);
+  }
+
+  inline static bool setThrown(uint16_t id) {
+    return setClosed(id, false);
+  }
+
+  static bool setClosedStateOnly(uint16_t id, bool close);
+
+  inline static Turnout *first() { return _firstTurnout; }
+
+  // Load all turnout definitions.
+  static void load();
+  // Load one turnout definition
+  static Turnout *loadTurnout();
+  // Save all turnout definitions
+  static void store();
+
+  static void printAll(Print *stream) {
+    for (Turnout *tt = _firstTurnout; tt != 0; tt = tt->_nextTurnout)
+      tt->printState(stream);
+  }
+
+  static void printState(uint16_t id, Print *stream);
+};
+
+
+/*************************************************************************************
+ * ServoTurnout - Turnout controlled by servo device.
+ * 
+ *************************************************************************************/
+class ServoTurnout : public Turnout {
+private:
+  // ServoTurnoutData contains data specific to this subclass that is 
+  // written to EEPROM when the turnout is saved.
+  struct ServoTurnoutData {
+    VPIN vpin;
+    uint16_t closedPosition : 12;
+    uint16_t thrownPosition : 12;
+    uint8_t profile;
+  } _servoTurnoutData; // 6 bytes
+
+  // Constructor
+  ServoTurnout(uint16_t id, VPIN vpin, uint16_t thrownPosition, uint16_t closedPosition, uint8_t profile, bool closed);
+
+public:
+  // Create function
+  static Turnout *create(uint16_t id, VPIN vpin, uint16_t thrownPosition, uint16_t closedPosition, uint8_t profile, bool closed=true);
+
+  // Load a Servo turnout definition from EEPROM.  The common Turnout data has already been read at this point.
+  static Turnout *load(struct TurnoutData *turnoutData);
+  void print(Print *stream) override;
+
+protected:
+  // ServoTurnout-specific code for throwing or closing a servo turnout.
+  bool setClosedInternal(bool close) override;
+  void save() override;
+
+};
+
+/*************************************************************************************
+ * DCCTurnout - Turnout controlled by DCC Accessory Controller.
+ * 
+ *************************************************************************************/
+class DCCTurnout : public Turnout {
+private:
+  // DCCTurnoutData contains data specific to this subclass that is 
+  // written to EEPROM when the turnout is saved.
+  struct DCCTurnoutData {
+    // DCC address (Address in bits 15-2, subaddress in bits 1-0)
+    struct {
+      uint16_t address : 14;
+      uint8_t subAddress : 2;
+    };
+  } _dccTurnoutData; // 2 bytes
+
+  // Constructor
+  DCCTurnout(uint16_t id, uint16_t address, uint8_t subAdd);
+
+public:
+  // Create function
+  static Turnout *create(uint16_t id, uint16_t add, uint8_t subAdd);
+  // Load a VPIN turnout definition from EEPROM.  The common Turnout data has already been read at this point.
+  static Turnout *load(struct TurnoutData *turnoutData);
+  void print(Print *stream) override;
+  // Flag whether DCC Accessory packets are to contain 1=close/0=throw(RCN-213) or 1=throw/0-close (DCC++ Classic)
+  static const bool rcn213Compliant;
+
+protected:
+  bool setClosedInternal(bool close) override;
+  void save() override;
+
+};
+
+
+/*************************************************************************************
+ * VpinTurnout - Turnout controlled through a HAL vpin.
+ * 
+ *************************************************************************************/
+class VpinTurnout : public Turnout {
+private:
+  // VpinTurnoutData contains data specific to this subclass that is 
+  // written to EEPROM when the turnout is saved.
+  struct VpinTurnoutData {
+    VPIN vpin;
+  } _vpinTurnoutData; // 2 bytes
+
+  // Constructor
+ VpinTurnout(uint16_t id, VPIN vpin, bool closed);
+
+public:
+  // Create function
+  static Turnout *create(uint16_t id, VPIN vpin, bool closed=true);
+
+  // Load a VPIN turnout definition from EEPROM.  The common Turnout data has already been read at this point.
+  static Turnout *load(struct TurnoutData *turnoutData);
+  void print(Print *stream) override;
+
+protected:
+  bool setClosedInternal(bool close) override;
+  void save() override;
+
+};
+
+
+/*************************************************************************************
+ * LCNTurnout - Turnout controlled by Loconet
+ * 
+ *************************************************************************************/
+class LCNTurnout : public Turnout {
+private:
+  // LCNTurnout has no specific data, and in any case is not written to EEPROM!
+  // struct LCNTurnoutData {
+  // } _lcnTurnoutData; // 0 bytes
+
+  // Constructor 
+  LCNTurnout(uint16_t id, bool closed);
+
+public:
+  // Create function
+  static Turnout *create(uint16_t id, bool closed=true);
+
+
+  bool setClosedInternal(bool close) override;
+
+  // LCN turnouts not saved to EEPROM.
+  //void save() override {  }
+  //static Turnout *load(struct TurnoutData *turnoutData) {
+
+  void print(Print *stream) override;
+
+};
+
 #endif

WiThrottle.cpp

@@ -120,7 +120,7 @@ void WiThrottle::parse(RingStream * stream, byte * cmdx) {
     // Send turnout list if changed since last sent (will replace list on client)
     if (turnoutListHash != Turnout::turnoutlistHash) {
       StringFormatter::send(stream,F("PTL"));
-      for(Turnout *tt=Turnout::firstTurnout;tt!=NULL;tt=tt->nextTurnout){
+      for(Turnout *tt=Turnout::first();tt!=NULL;tt=tt->next()){
           int id=tt->getId();
           StringFormatter::send(stream,F("]\\[%d}|{%d}|{%c"), id, id, Turnout::isClosed(id)?'2':'4');
       }
@@ -161,27 +161,27 @@ void WiThrottle::parse(RingStream * stream, byte * cmdx) {
 #endif    
             else if (cmd[1]=='T' && cmd[2]=='A') { // PTA accessory toggle 
                 int id=getInt(cmd+4); 
-                Turnout * tt=Turnout::get(id);
-                if (!tt) {
+                if (!Turnout::exists(id)) {
                   // If turnout does not exist, create it
                   int addr = ((id - 1) / 4) + 1;
                   int subaddr = (id - 1) % 4;
-                  Turnout::createDCC(id,addr,subaddr);
+                  DCCTurnout::create(id,addr,subaddr);
                   StringFormatter::send(stream, F("HmTurnout %d created\n"),id);
                 }
                 switch (cmd[3]) {
-		case 'T':
-		  Turnout::setClosed(id,false);
-		  break;
-		case 'C':
-		  Turnout::setClosed(id,true);
-		  break;
-		case '2':
-		  Turnout::setClosed(id,!Turnout::isClosed(id));
-		  break;
-		default :
-		  Turnout::setClosed(id,true);
-		  break;
+            		  // T and C according to RCN-213 where 0 is Stop, Red, Thrown, Diverging.
+            		  case 'T': 
+            		     Turnout::setClosed(id,false);
+                     break;
+            		  case 'C': 
+                     Turnout::setClosed(id,true);
+                     break;
+            		  case '2': 
+            		     Turnout::setClosed(id,!Turnout::isClosed(id));
+                     break;
+            		  default :
+            		     Turnout::setClosed(id,true);
+                     break;
                 }
 		            StringFormatter::send(stream, F("PTA%c%d\n"),Turnout::isClosed(id)?'2':'4',id );
 		        }

config.example.h

@@ -114,11 +114,11 @@ The configuration file for DCC-EX Command Station
 // DEFINE LCD SCREEN USAGE BY THE BASE STATION
 //
 // Note: This feature requires an I2C enabled LCD screen using a Hitachi HD44780
-//       controller and a PCF8574 based I2C 'backpack'.
+//       controller and a commonly available PCF8574 based I2C 'backpack'.
 // To enable, uncomment one of the #define lines below
 
-// define LCD_DRIVER for I2C LCD address 0x3f,16 cols, 2 rows
-// #define LCD_DRIVER  0x3F,16,2
+// define LCD_DRIVER for I2C address 0x27, 16 cols, 2 rows
+// #define LCD_DRIVER  0x27,16,2
 
 //OR define OLED_DRIVER width,height in pixels (address auto detected)
 // 128x32 or 128x64 I2C SSD1306-based devices are supported.
@@ -132,14 +132,19 @@ The configuration file for DCC-EX Command Station
 //
 // DEFINE TURNOUTS/ACCESSORIES FOLLOW NORM RCN-213
 //
-// According to norm RCN-213 a DCC packet with an 1 is closed/straight
-// and one with a 0 is thrown/diverging. This is reversed to the
-// definition from DCC++ in the DCC++ protocol. To make the states
-// match with the norm, we need to reverse the bit in the DCC packet
-// on the rails, but we don't want to cause havoc on existent layouts,
-// so we define this only for new installations. For any new install
-// there is no reason to not define this.
-#define TURNOUTS_RCN_213
+// According to norm RCN-213 a DCC packet with a 1 is closed/straight
+// and one with a 0 is thrown/diverging.  In DCC++ Classic, and in previous
+// versions of DCC++EX, a turnout throw command was implemented in the packet as 
+// '1' and a close command as '0'. The #define below makes the states
+// match with the norm.  But we don't want to cause havoc on existent layouts,
+// so we define this only for new installations. If you don't want this,
+// don't add it to your config.h.
+//#define DCC_TURNOUTS_RCN_213
+
+// The following #define likewise inverts the behaviour of the <a> command 
+// for triggering DCC Accessory Decoders, so that <a addr subaddr 0> generates a 
+// DCC packet with D=1 (close turnout) and <a addr subaddr 1> generates D=0 
+// (throw turnout).
+//#define DCC_ACCESSORY_RCN_213
 
 /////////////////////////////////////////////////////////////////////////////////////
-

defines.h

@@ -18,6 +18,19 @@
 
 */
 
+#ifndef DEFINES_H
+#define DEFINES_H
+
+// defines.h relies on macros defined in config.h
+// but it may have already been included (for cosmetic convenence) by the .ino
+#ifndef MOTOR_SHIELD_TYPE
+  #if __has_include ( "config.h")
+    #include "config.h"
+  #else
+    #include "config.example.h"
+  #endif
+#endif
+
 ////////////////////////////////////////////////////////////////////////////////
 //
 // WIFI_ON: All prereqs for running with WIFI are met
@@ -55,3 +68,5 @@
 #if __has_include ( "myAutomation.h") && defined(BIG_RAM)
   #define RMFT_ACTIVE
 #endif
+
+#endif

mySetup.cpp_example.txt

@@ -0,0 +1,214 @@
+//  Sample mySetup.cpp file.
+//
+// To use this file, copy it to mySetup.cpp and uncomment the directives and/or
+// edit them to satisfy your requirements.  
+
+// Note that if the file has a .cpp extension it WILL be compiled into the build
+// and the mySetup() function WILL be invoked.
+//
+// To prevent this, temporarily rename it to mySetup.txt or similar.
+//
+
+#include "IODevice.h"
+#include "Turnouts.h"
+#include "Sensors.h"
+#include "IO_HCSR04.h"
+#include "IO_VL53L0X.h"
+
+
+// The #if directive prevent compile errors for Uno and Nano by excluding the 
+//  HAL directives from the build.
+#if !defined(IO_NO_HAL)
+
+
+// Examples of statically defined HAL directives (alternative to the create() call).
+// These have to be outside of the mySetup() function.
+
+//=======================================================================
+// The following directive defines a PCA9685 PWM Servo driver module.
+//=======================================================================
+// The parameters are: 
+//   First Vpin=100
+//   Number of VPINs=16 (numbered 100-115)
+//   I2C address of module=0x40
+
+//PCA9685 pwmModule1(100, 16, 0x40);
+
+
+//=======================================================================
+// The following directive defines an MCP23017 16-port I2C GPIO Extender module.
+//=======================================================================
+// The parameters are: 
+//   First Vpin=196
+//   Number of VPINs=16 (numbered 196-211)
+//   I2C address of module=0x22
+
+//MCP23017 gpioModule2(196, 16, 0x22);
+
+
+// Alternative form, which allows the INT pin of the module to request a scan
+// by pulling Arduino pin 40 to ground.  Means that the I2C isn't being polled
+// all the time, only when a change takes place. Multiple modules' INT pins
+// may be connected to the same Arduino pin.
+
+//MCP23017 gpioModule2(196, 16, 0x22, 40);
+
+
+//=======================================================================
+// The following directive defines an MCP23008 8-port I2C GPIO Extender module.
+//=======================================================================
+// The parameters are: 
+//   First Vpin=300
+//   Number of VPINs=8 (numbered 300-307)
+//   I2C address of module=0x22
+
+//MCP23008 gpioModule3(300, 8, 0x22);
+
+
+//=======================================================================
+// The following directive defines a PCF8574 8-port I2C GPIO Extender module.
+//=======================================================================
+// The parameters are: 
+//   First Vpin=200
+//   Number of VPINs=8 (numbered 200-207)
+//   I2C address of module=0x23
+
+//PCF8574 gpioModule4(200, 8, 0x23);
+
+
+// Alternative form using INT pin (see above)
+
+//PCF8574 gpioModule4(200, 8, 0x23, 40);
+
+
+//=======================================================================
+// The following directive defines an HCSR04 ultrasonic ranging module.
+//=======================================================================
+// The parameters are: 
+//   Vpin=2000 (only one VPIN per directive)
+//   Number of VPINs=1
+//   Arduino pin connected to TRIG=30
+//   Arduino pin connected to ECHO=31
+//   Minimum trigger range=20cm (VPIN goes to 1 when <20cm)
+//   Maximum trigger range=25cm (VPIN goes to 0 when >25cm)
+// Note: Multiple devices can be configured by using a different ECHO pin
+// for each one.  The TRIG pin can be shared between multiple devices.
+// Be aware that the 'ping' of one device may be received by another
+// device and position them accordingly!
+
+//HCSR04 sonarModule1(2000, 30, 31, 20, 25);
+//HCSR04 sonarModule2(2001, 30, 32, 20, 25);
+
+
+//=======================================================================
+// The following directive defines a single VL53L0X Time-of-Flight range sensor.
+//=======================================================================
+// The parameters are:
+//   VPIN=5000
+//   Number of VPINs=1
+//   I2C address=0x29 (default for this chip)
+//   Minimum trigger range=200mm (VPIN goes to 1 when <20cm)
+//   Maximum trigger range=250mm (VPIN goes to 0 when >25cm)
+
+//VL53L0X tofModule1(5000, 1, 0x29, 200, 250); 
+
+// For multiple VL53L0X modules, add another parameter which is a VPIN connected to the
+// module's XSHUT pin.  This allows the modules to be configured, at start,
+// with distinct I2C addresses.  In this case, the address 0x29 is only used during
+// initialisation to configure each device in turn with the desired unique I2C address.
+// The examples below have the modules' XSHUT pins connected to the first two pins of 
+// the first MCP23017 module (164 and 165), but Arduino pins may be used instead.
+// The first module here is given I2C address 0x30 and the second is 0x31.
+
+//VL53L0X tofModule1(5000, 1, 0x30, 200, 250, 164); 
+//VL53L0X tofModule2(5001, 1, 0x31, 200, 250, 165); 
+
+
+//=======================================================================
+// The function mySetup() is invoked from CS if it exists within the build.
+// It is called just before mysetup.h is executed, so things set up within here can be
+// referenced by commands in mySetup.h.
+//=======================================================================
+
+void mySetup() {
+
+  // Alternative way of creating a module driver, which has to be within the mySetup() function
+  // The other devices can also be created in this way.  The parameter lists for the
+  // create() function are identical to the parameter lists for the declarations.
+
+  //MCP23017::create(196, 16, 0x22);
+
+
+  //=======================================================================
+  // Creating a Turnout
+  //=======================================================================
+  // Parameters: same as <T> command for Servo turnouts
+  // ID and VPIN are 100, sonar moves between positions 102 and 490 with slow profile.
+  // Profile may be Instant, Fast, Medium, Slow or Bounce.
+
+  //ServoTurnout::create(100, 100, 490, 102, PCA9685::Slow);
+
+
+  //=======================================================================
+  // DCC Accessory turnout
+  //=======================================================================
+  // Parameters: same as <T> command for DCC Accessory turnouts
+  //  ID=3000
+  //  Decoder address=23
+  //  Decoder subaddress = 1
+
+  //DCCTurnout::create(3000, 23, 1);
+
+
+  //=======================================================================
+  // Creating a Sensor
+  //=======================================================================
+  // Parameters: As for the <S> command,
+  //  id = 164,
+  //  Vpin = 164 (configured above as pin 0 of an MCP23017)
+  //  Pullup enable = 1 (enabled)
+
+  //Sensor::create(164, 164, 1);
+
+
+  //=======================================================================
+  // Way of creating lots of identical sensors in a range
+  //=======================================================================
+
+  //for (int i=165; i<180; i++) 
+  //  Sensor::create(i, i, 1);
+
+
+  //=======================================================================
+  // Play mp3 files from a Micro-SD card, using a DFPlayer MP3 Module.
+  //=======================================================================
+  // Parameters: 
+  //   10000 = first VPIN allocated.
+  //   10 = number of VPINs allocated.
+  //   Serial1 = name of serial port (usually Serial1 or Serial2).
+  // With these parameters, up to 10 files may be played on pins 10000-10009.
+  // Play is started from EX-RAIL with SET(10000) for first mp3 file, SET(10001)
+  // for second file, etc.  Play may also be initiated by writing an analogue
+  // value to the first pin, e.g. SERVO(10000,23,0) will play the 23rd mp3 file.
+  // SERVO(10000,23,30) will do the same thing, as well as setting the volume to 
+  // 30 (maximum value).
+  // Play is stopped by RESET(10000) (or any other allocated VPIN).
+  // Volume may also be set by writing an analogue value to the second pin for the player, 
+  // e.g. SERVO(10001,30,0) sets volume to maximum (30).
+  // The EX-RAIL script may check for completion of play by calling WAITFOR(pin), which will only proceed to the
+  // following line when the player is no longer busy.
+  // E.g.
+  //    SEQUENCE(1)
+  //      AT(164)           // Wait for sensor attached to pin 164 to activate
+  //      SET(10003)        // Play fourth MP3 file
+  //      LCD(4, "Playing") // Display message on LCD/OLED
+  //      WAITFOR(10003)    // Wait for playing to finish
+  //      LCD(4, " ")       // Clear LCD/OLED line 
+  //      FOLLOW(1)         // Go back to start
+
+  // DFPlayer::create(10000, 10, Serial1);
+
+
+}
+
+#endif

platformio.ini

@@ -16,6 +16,7 @@ default_envs =
 	unowifiR2
 	nano
 src_dir = .
+include_dir = .
 
 [env]
 build_flags = -Wall -Wextra
@@ -41,7 +42,7 @@ lib_deps =
 	SPI
 monitor_speed = 115200
 monitor_flags = --echo
-build_flags = -DDIAG_IO
+build_flags = -DDIAG_IO -DDIAG_LOOPTIMES
 
 [env:mega2560-no-HAL]
 platform = atmelavr

version.h

@@ -3,8 +3,26 @@
 
 #include "StringFormatter.h"
 
-
-#define VERSION "3.1.6"
+#define VERSION "3.2.0"
+// 3.2.0 Major functional and non-functional changes.
+//       New HAL added for I/O (digital and analogue inputs and outputs, servos etc).
+//         Support for MCP23008, MCP23017 and PCF9584 I2C GPIO Extender modules.
+//         Support for PCA9685 PWM (servo) control modules.
+//         Support for analogue inputs on Arduino pins and on ADS111x I2C modules.
+//         Support for MP3 sound playback via DFPlayer module.
+//         Support for HC-SR04 Ultrasonic range sensor module.
+//         Support for VL53L0X Laser range sensor module (Time-Of-Flight).
+//       Native non-blocking I2C drivers for AVR and Nano architectures (fallback
+//       to blocking Wire library for other platforms).
+//       EEPROM layout change - deletes EEPROM contents on first start following upgrade.
+//       New EX-RAIL automation capability.
+//       Turnout class revised to expand turnout capabilities, new commands added.
+//       Output class now allows ID > 255.
+//       Configuration options to globally flip polarity of DCC Accessory states when driven
+//       from <a> command and <T> command.
+//       Increased use of display for showing loco decoder programming information.
+//       ...
+// 3.1.7 Bugfix: Unknown locos should have speed forward 
 // 3.1.6 Make output ID two bytes and guess format/size of registered outputs found in EEPROM
 // 3.1.5 Fix LCD corruption on power-up
 // 3.1.4 Refactor OLED and LCD drivers and remove unused code