Merge branch 'EX-RAIL' into EX-RAIL-Ash

IODevice.cpp

@@ -241,6 +241,18 @@ 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)
 
@@ -259,6 +271,13 @@ 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!"));
@@ -330,7 +349,7 @@ void ArduinoPins::_write(VPIN vpin, int value) {
   }
 }
 
-// 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);
@@ -352,6 +371,39 @@ int ArduinoPins::_read(VPIN vpin) {
   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;
+  if (_pinModes[index] & mask) {
+    // Currently in write mode, 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);
+  }
+  // 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);
+  #endif
+  return value;
+}
+
 void ArduinoPins::_display() {
   DIAG(F("Arduino Vpins:%d-%d"), (int)_firstVpin, (int)_firstVpin+_nPins-1);
 }

IODevice.h

@@ -141,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();
 
@@ -188,12 +191,18 @@ protected:
     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;
   };
 
+  // Method to read analogue pin state (optionally implemented within device class)
+  virtual int _readAnalogue(VPIN vpin) { 
+    (void)vpin; 
+    return 0;
+  };
+
   // _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.  Returns false unless overridden in sub class.
   virtual bool _isBusy(VPIN vpin) {
@@ -343,8 +352,9 @@ 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;
 
 

IO_AnalogueInputs.h

@@ -0,0 +1,131 @@
+/*
+ *  © 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)
+ *    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.
+ * 
+ * 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 = _nPins; // Suppress read on first loop entry.
+    addDevice(this);
+  }
+  static void create(VPIN firstVpin, int nPins, uint8_t i2cAddress) {
+    new ADS111x(firstVpin, nPins, i2cAddress);
+  }
+  void _begin() {
+    // Initialise ADS device
+    if (I2CManager.exists(_i2cAddress)) {
+#ifdef DIAG_IO
+      _display();
+#endif
+    } else {
+      DIAG(F("ADS111x device not found, I2C:%x"), _i2cAddress);
+    }
+  }
+  void _loop(unsigned long currentMicros) {
+
+    if (currentMicros - _lastMicros >= scanInterval) {
+      // Check that previous non-blocking write has completed, if not then wait
+      _i2crb.wait();
+
+      // If _currentPin is in the valid range, continue reading the pin values
+      if (_currentPin < _nPins) {
+        _outBuffer[0] = 0x00;  // Conversion register address
+        uint8_t status = I2CManager.read(_i2cAddress, _inBuffer, 2, 1, _outBuffer); // Read register
+        if (status == I2C_STATUS_OK) {
+          _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;
+      
+      // 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] = 0x83;           // 128 samples/sec, comparator off
+      // Write command, without waiting for completion.
+      I2CManager.write(_i2cAddress, _outBuffer, 3, &_i2crb);
+
+      _lastMicros = currentMicros;
+    }
+  }
+  int _readAnalogue(VPIN vpin) {
+    int pin = vpin - _firstVpin;
+    return _value[pin];
+  }
+  void _display() {
+    DIAG(F("ADS111x I2C:x%x Configured on Vpins:%d-%d"), _i2cAddress, _firstVpin, _firstVpin+_nPins-1);
+  }
+
+protected:
+  // With ADC set to 128 samples/sec, that's 7.8ms/sample.  So set the period between updates to 10ms
+  #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
+  uint16_t _value[4];
+  uint8_t _i2cAddress;
+  uint8_t _outBuffer[3];
+  uint8_t _inBuffer[2];
+  uint8_t _currentPin;  // ADC pin currently being scanned
+  unsigned long _lastMicros = 0;
+  I2CRB _i2crb;
+};
+
+#endif // io_analogueinputs_h

RMFT2.cpp

@@ -275,6 +275,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) {
@@ -329,11 +330,16 @@ void RMFT2::driveLoco(byte speed) {
      speedo=speed;
 }
 
-bool RMFT2::readSensor(int16_t sensorId) {
+bool RMFT2::readSensor(uint16_t sensorId) {
-  VPIN vpin=abs(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;
 }
 
@@ -529,6 +535,14 @@ void RMFT2::loop2() {
       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); 
@@ -625,7 +639,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.
@@ -674,26 +688,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 amberpin=GET_OPERAND(1);
-     byte greenpin=GET_OPERAND(3);
+     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 || (amber && (amberpin==0)));
+     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;
+     if (turnoutId!=(int16_t)GET_OPERAND(0)) continue;
-     new RMFT2(progCounter);  // new task starts at this instruction
+     task=new RMFT2(progCounter);  // new task starts at this instruction
+     task->onTurnoutId=turnoutId; // flag for recursion detector
      return;
    }
  }

RMFT2.h

@@ -34,7 +34,7 @@ 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_WAITFOR,
              OPCODE_PAD,OPCODE_FOLLOW,OPCODE_CALL,OPCODE_RETURN,
@@ -68,7 +68,7 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
     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, int16_t p[]);
     static bool parseSlash(Print * stream, byte & paramCount, int16_t p[]) ;
@@ -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.
@@ -83,6 +84,7 @@
 #define JOIN 
 #define LATCH(sensor_id) 
 #define LCD(row,msg) 
+#define LCN(msg) 
 #define ONCLOSE(turnout_id)
 #define ONTHROW(turnout_id) 
 #define PAUSE
@@ -97,6 +99,10 @@
 #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) 
@@ -113,6 +119,8 @@
 #define UNJOIN 
 #define UNLATCH(sensor_id) 
 #define WAITFOR(pin)
+#define XFOFF(cab,func)
+#define XFON(cab,func)
 
 #include "myAutomation.h"
 
@@ -125,6 +133,11 @@
 
 #undef EXRAIL 
 #undef PRINT
+#undef LCN
+#undef SERIAL
+#undef SERIAL1
+#undef SERIAL2
+#undef SERIAL3
 #undef ENDEXRAIL  
 #undef LCD
 const int StringMacroTracker1=__COUNTER__;
@@ -132,6 +145,11 @@ const int StringMacroTracker1=__COUNTER__;
 #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 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(L&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"
 
@@ -165,6 +183,7 @@ const int StringMacroTracker1=__COUNTER__;
 #undef JOIN
 #undef LATCH
 #undef LCD
+#undef LCN
 #undef ONCLOSE
 #undef ONTHROW
 #undef PAUSE
@@ -184,6 +203,10 @@ const int StringMacroTracker1=__COUNTER__;
 #undef SERVO2
 #undef FADE
 #undef SENDLOCO
+#undef SERIAL
+#undef SERIAL1
+#undef SERIAL2
+#undef SERIAL3
 #undef SETLOCO
 #undef SET
 #undef SPEED
@@ -196,6 +219,8 @@ const int StringMacroTracker1=__COUNTER__;
 #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
@@ -234,7 +259,8 @@ 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,
@@ -248,6 +274,10 @@ const int StringMacroTracker1=__COUNTER__;
 #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::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),
@@ -263,13 +293,16 @@ const int StringMacroTracker1=__COUNTER__;
 #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

Turnouts.cpp

@@ -119,7 +119,7 @@
       // 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.
-      printState(id, &Serial);
+      tt->printState(&Serial);
     return true;
   }
 
@@ -140,6 +140,8 @@
     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) 
@@ -152,7 +154,7 @@
       // 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.
-      printState(id, &Serial);
+      tt->printState(&Serial);
     }
     return ok;
   }
@@ -214,7 +216,7 @@
   // 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);
+    if (tt) tt->printState(stream);
   }
 
 

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