Merge branch 'ServoSignal' into TrackManager

EXRAIL2.cpp

@@ -64,6 +64,9 @@ const int16_t HASH_KEYWORD_RESUME=27609;
 const int16_t HASH_KEYWORD_KILL=5218;
 const int16_t HASH_KEYWORD_ALL=3457;
 const int16_t HASH_KEYWORD_ROUTES=-3702;
+const int16_t HASH_KEYWORD_RED=26099;
+const int16_t HASH_KEYWORD_AMBER=18713;
+const int16_t HASH_KEYWORD_GREEN=-31493;
 
 // 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.
@@ -407,6 +410,18 @@ bool RMFT2::parseSlash(Print * stream, byte & paramCount, int16_t p[]) {
     setFlag(p[1], 0, LATCH_FLAG);
     return true;
  
+  case HASH_KEYWORD_RED:
+    doSignal(p[1],SIGNAL_RED);
+    return true;
+ 
+  case HASH_KEYWORD_AMBER:
+    doSignal(p[1],SIGNAL_AMBER);
+    return true;
+ 
+  case HASH_KEYWORD_GREEN:
+    doSignal(p[1],SIGNAL_GREEN);
+    return true;
+    
   default:
     return false;
   }
@@ -508,28 +523,21 @@ bool RMFT2::skipIfBlock() {
   while (nest > 0) {
     SKIPOP;
     byte opcode =  GET_OPCODE;
-    switch(opcode) {
+    // all other IF type commands increase the nesting level
-    case OPCODE_ENDEXRAIL:
+    if (opcode>IF_TYPE_OPCODES) nest++;
-      kill(F("missing ENDIF"), nest);
+    else switch(opcode) {
-      return false;
+      case OPCODE_ENDEXRAIL:
-    case OPCODE_IF:
+        kill(F("missing ENDIF"), nest);
-    case OPCODE_IFCLOSED:
+        return false;
-    case OPCODE_IFGTE:
+    
-    case OPCODE_IFLT:
+      case OPCODE_ENDIF:
-    case OPCODE_IFNOT:
+        nest--;
-    case OPCODE_IFRANDOM:
+        break;
-    case OPCODE_IFRESERVE:
+    
-    case OPCODE_IFTHROWN:
+      case OPCODE_ELSE:
-    case OPCODE_IFTIMEOUT:
+        // if nest==1 then this is the ELSE for the IF we are skipping
-      nest++;
+        if (nest==1) nest=0; // cause loop exit and return after ELSE
-      break;
+        break;
-    case OPCODE_ENDIF:
-      nest--;
-      break;
-    case OPCODE_ELSE:
-      // if nest==1 then this is the ELSE for the IF we are skipping
-      if (nest==1) nest=0; // cause loop exit and return after ELSE
-      break;
     default:
       break;
     }
@@ -557,6 +565,10 @@ void RMFT2::loop2() {
 
   byte opcode = GET_OPCODE;
   int16_t operand =  GET_OPERAND(0);
+
+  // skipIf will get set to indicate a failing IF condition 
+  bool skipIf=false; 
+
   // if (diag) DIAG(F("RMFT2 %d %d"),opcode,operand);
   // Attention: Returning from this switch leaves the program counter unchanged.
   //            This is used for unfinished waits for timers or sensors.
@@ -643,7 +655,7 @@ void RMFT2::loop2() {
     return;
     
   case OPCODE_IFTIMEOUT: // do next operand if timeout flag set
-    if (!timeoutFlag) if (!skipIfBlock()) return;
+    skipIf=!timeoutFlag;
     break;
     
   case OPCODE_AFTER: // waits for sensor to hit and then remain off for 0.5 seconds. (must come after an AT operation)
@@ -704,40 +716,52 @@ void RMFT2::loop2() {
     break;
     
   case OPCODE_IF: // do next operand if sensor set
-    if (!readSensor(operand)) if (!skipIfBlock()) return;
+    skipIf=!readSensor(operand);
     break;
     
   case OPCODE_ELSE: // skip to matching ENDIF
-    if (!skipIfBlock()) return;
+    skipIf=true;
     break;
     
   case OPCODE_IFGTE: // do next operand if sensor>= value
-    if (IODevice::readAnalogue(operand)<(int)(GET_OPERAND(1))) if (!skipIfBlock()) return;
+    skipIf=IODevice::readAnalogue(operand)<(int)(GET_OPERAND(1));
     break;
     
   case OPCODE_IFLT: // do next operand if sensor< value
-    if (IODevice::readAnalogue(operand)>=(int)(GET_OPERAND(1))) if (!skipIfBlock()) return;
+    skipIf=IODevice::readAnalogue(operand)>=(int)(GET_OPERAND(1));
     break;
     
   case OPCODE_IFNOT: // do next operand if sensor not set
-    if (readSensor(operand)) if (!skipIfBlock()) return;
+    skipIf=readSensor(operand);
     break;
     
   case OPCODE_IFRANDOM: // do block on random percentage
-    if ((int16_t)random(100)>=operand) if (!skipIfBlock()) return;
+    skipIf=(int16_t)random(100)>=operand;
     break;
     
   case OPCODE_IFRESERVE: // do block if we successfully RERSERVE
     if (!getFlag(operand,SECTION_FLAG)) setFlag(operand,SECTION_FLAG);
-    else if (!skipIfBlock()) return;
+    else skipIf=true;
+    break;
+    
+  case OPCODE_IFRED: // do block if signal as expected
+    skipIf=!isSignal(operand,SIGNAL_RED);
+    break;
+    
+  case OPCODE_IFAMBER: // do block if signal as expected
+    skipIf=!isSignal(operand,SIGNAL_AMBER);
+    break;
+    
+  case OPCODE_IFGREEN: // do block if signal as expected
+    skipIf=!isSignal(operand,SIGNAL_GREEN);
     break;
     
   case OPCODE_IFTHROWN:
-    if (Turnout::isClosed(operand)) if (!skipIfBlock()) return;
+    skipIf=Turnout::isClosed(operand);
     break;
     
   case OPCODE_IFCLOSED:
-    if (!Turnout::isClosed(operand)) if (!skipIfBlock()) return;
+    skipIf=Turnout::isThrown(operand);
     break;
     
   case OPCODE_ENDIF:
@@ -760,15 +784,15 @@ void RMFT2::loop2() {
     break;
     
   case OPCODE_RED:
-    doSignal(operand,true,false,false);
+    doSignal(operand,SIGNAL_RED);
     break;
     
   case OPCODE_AMBER:
-    doSignal(operand,false,true,false);
+    doSignal(operand,SIGNAL_AMBER);
     break;
     
   case OPCODE_GREEN:
-    doSignal(operand,false,false,true);
+    doSignal(operand,SIGNAL_GREEN);
     break;
     
   case OPCODE_FON:
@@ -936,6 +960,8 @@ void RMFT2::loop2() {
     kill(F("INVOP"),operand);
   }
   // Falling out of the switch means move on to the next opcode
+  // but if we are skipping a false IF or else
+  if (skipIf)  if (!skipIfBlock()) return;
   SKIPOP;
 }
 
@@ -963,51 +989,64 @@ void RMFT2::kill(const FSH * reason, int operand) {
   delete this;
 }
 
-/* static */ void RMFT2::doSignal(VPIN id,bool red, bool amber, bool green) {
+int16_t RMFT2::getSignalSlot(VPIN id) {
-  //if (diag) DIAG(F(" dosignal %d"),id);
   for (int sigpos=0;;sigpos+=4) {
-    //if (diag) DIAG(F("red=%d"),redpin);
+      VPIN sigid=GETFLASHW(RMFT2::SignalDefinitions+sigpos);
-    VPIN sigid=GETFLASHW(RMFT2::SignalDefinitions+sigpos);
+      if (sigid==0) { // end of signal list 
-    if (sigid==0) { // end of signal list 
+        DIAG(F("EXRAIL Signal %d not defined"), id);
-      DIAG(F("EXRAIL Signal %d not defined"), id);
+        return -1;
-      return;  // signal not found
+      }
-    }
+      // sigid is the signal id used in RED/AMBER/GREEN macro
-    // sigid is the signal id used in RED/AMBER/GREEN macro
+      // for a LED signal it will be same as redpin
-    // for a LED signal it will be same as redpin
+      // but for a servo signal it will also have SERVO_SIGNAL_FLAG set. 
-    // but for a servo signal it will also have SERVO_SIGNAL_FLAG set. 
 
-    if ((sigid & ~SERVO_SIGNAL_FLAG & ~ACTIVE_HIGH_SIGNAL_FLAG)!= id) continue; // keep looking
+      if ((sigid & ~SERVO_SIGNAL_FLAG & ~ACTIVE_HIGH_SIGNAL_FLAG)!= id) continue; // keep looking
+      return sigpos/4; // relative slot in signals table
+  }  
+}
+/* static */ void RMFT2::doSignal(VPIN id,char rag) {
+  //if (diag) DIAG(F(" dosignal %d %x"),id,rag);
+  int16_t sigslot=getSignalSlot(id);
+  if (sigslot<0) return; 
   
-    // Correct signal definition found, get the rag values
+  // keep track of signal state 
-    VPIN redpin=GETFLASHW(RMFT2::SignalDefinitions+sigpos+1);
+  setFlag(sigslot,rag,SIGNAL_MASK);
-    VPIN amberpin=GETFLASHW(RMFT2::SignalDefinitions+sigpos+2);
-    VPIN greenpin=GETFLASHW(RMFT2::SignalDefinitions+sigpos+3);
-    //if (diag) DIAG(F("signal %d %d %d"),redpin,amberpin,greenpin);
  
-    if (sigid & SERVO_SIGNAL_FLAG) {
+  // Correct signal definition found, get the rag values
-      // A servo signal, the pin numbers are actually servo positions
+  int16_t sigpos=sigslot*4; 
-      // Note, setting a signal to a zero position has no effect.
+  VPIN sigid=GETFLASHW(RMFT2::SignalDefinitions+sigpos);
-      int16_t servopos= red? redpin: (green? greenpin : amberpin);
+  VPIN redpin=GETFLASHW(RMFT2::SignalDefinitions+sigpos+1);
-      if  (servopos!=0) IODevice::writeAnalogue(id,servopos,PCA9685::Bounce);
+  VPIN amberpin=GETFLASHW(RMFT2::SignalDefinitions+sigpos+2);
-      return;  
+  VPIN greenpin=GETFLASHW(RMFT2::SignalDefinitions+sigpos+3);
-    }
+  //if (diag) DIAG(F("signal %d %d %d"),redpin,amberpin,greenpin);
 
-    // LED or similar 3 pin signal
+  if (sigid & SERVO_SIGNAL_FLAG) {
-    // If amberpin is zero, synthesise amber from red+green
+    // A servo signal, the pin numbers are actually servo positions
-    if (amber && (amberpin==0)) {
+    // Note, setting a signal to a zero position has no effect.
-      red=true;
+    int16_t servopos= rag==SIGNAL_RED? redpin: (rag==SIGNAL_GREEN? greenpin : amberpin);
-      green=true;
+    if  (servopos!=0) IODevice::writeAnalogue(id,servopos,PCA9685::Bounce);
-    }
-
-    // Manage invert (HIGH on) pins
-    bool aHigh=sigid & ACTIVE_HIGH_SIGNAL_FLAG;
-    
-    // set the three pins 
-    if (redpin) IODevice::write(redpin,red^aHigh);
-    if (amberpin) IODevice::write(amberpin,amber^aHigh);
-    if (greenpin) IODevice::write(greenpin,green^aHigh);
     return;  
   }
+
+  // LED or similar 3 pin signal
+  // If amberpin is zero, synthesise amber from red+green
+  const byte SIMAMBER=0x00;
+  if (rag==SIGNAL_AMBER && (amberpin==0)) rag=SIMAMBER; // special case this func only
+   
+  // Manage invert (HIGH on) pins
+  bool aHigh=sigid & ACTIVE_HIGH_SIGNAL_FLAG;
+    
+  // set the three pins 
+  if (redpin) IODevice::write(redpin,(rag==SIGNAL_RED || rag==SIMAMBER)^aHigh);
+  if (amberpin) IODevice::write(amberpin,(rag==SIGNAL_AMBER)^aHigh);
+  if (greenpin) IODevice::write(greenpin,(rag==SIGNAL_GREEN || rag==SIMAMBER)^aHigh);
+  return;  
+}
+
+/* static */ bool RMFT2::isSignal(VPIN id,char rag) {
+  int16_t sigslot=getSignalSlot(id);
+  if (sigslot<0) return false; 
+  return (flags[sigslot] & SIGNAL_MASK) == rag;
 }
 
 void RMFT2::turnoutEvent(int16_t turnoutId, bool closed) {

EXRAIL2.h

@@ -35,10 +35,9 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
              OPCODE_RESERVE,OPCODE_FREE,
              OPCODE_AT,OPCODE_AFTER,OPCODE_AUTOSTART,
              OPCODE_ATGTE,OPCODE_ATLT,
-             OPCODE_ATTIMEOUT1,OPCODE_ATTIMEOUT2,OPCODE_IFTIMEOUT,
+             OPCODE_ATTIMEOUT1,OPCODE_ATTIMEOUT2,
              OPCODE_LATCH,OPCODE_UNLATCH,OPCODE_SET,OPCODE_RESET,
-             OPCODE_IF,OPCODE_IFNOT,OPCODE_ENDIF,OPCODE_IFRANDOM,OPCODE_IFRESERVE,
+             OPCODE_ENDIF,OPCODE_ELSE,
-             OPCODE_IFCLOSED, OPCODE_IFTHROWN,OPCODE_ELSE,
              OPCODE_DELAY,OPCODE_DELAYMINS,OPCODE_DELAYMS,OPCODE_RANDWAIT,
              OPCODE_FON,OPCODE_FOFF,OPCODE_XFON,OPCODE_XFOFF,
              OPCODE_RED,OPCODE_GREEN,OPCODE_AMBER,OPCODE_DRIVE,
@@ -49,19 +48,35 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
              OPCODE_PAUSE, OPCODE_RESUME,OPCODE_POWEROFF,OPCODE_POWERON,
              OPCODE_ONCLOSE, OPCODE_ONTHROW, OPCODE_SERVOTURNOUT, OPCODE_PINTURNOUT,
              OPCODE_PRINT,OPCODE_DCCACTIVATE,
-             OPCODE_ONACTIVATE,OPCODE_ONDEACTIVATE,OPCODE_IFGTE,OPCODE_IFLT,
+             OPCODE_ONACTIVATE,OPCODE_ONDEACTIVATE,
-             OPCODE_ROSTER,OPCODE_SET_TRACK,OPCODE_KILLALL,
+             OPCODE_ROSTER,OPCODE_KILLALL,
-             OPCODE_ROUTE,OPCODE_AUTOMATION,OPCODE_SEQUENCE,OPCODE_ENDTASK,OPCODE_ENDEXRAIL
+             OPCODE_ROUTE,OPCODE_AUTOMATION,OPCODE_SEQUENCE,
+             OPCODE_ENDTASK,OPCODE_ENDEXRAIL,
+             OPCODE_SET_TRACK,
+
+             // OPcodes below this point are skip-nesting IF operations
+             // placed here so that they may be skipped as a group
+             // see skipIfBlock()
+            IF_TYPE_OPCODES, // do not move this... 
+             OPCODE_IFRED,OPCODE_IFAMBER,OPCODE_IFGREEN,
+             OPCODE_IFGTE,OPCODE_IFLT,
+             OPCODE_IFTIMEOUT,
+             OPCODE_IF,OPCODE_IFNOT,
+             OPCODE_IFRANDOM,OPCODE_IFRESERVE,
+             OPCODE_IFCLOSED, OPCODE_IFTHROWN
              };
 
 
  
   // Flag bits for status of hardware and TPL
   static const byte SECTION_FLAG = 0x80;
-  static const byte LATCH_FLAG = 0x40;
+  static const byte LATCH_FLAG   = 0x40;
-  static const byte TASK_FLAG = 0x20;
+  static const byte TASK_FLAG    = 0x20;
-  static const byte SPARE_FLAG = 0x10;
+  static const byte SPARE_FLAG   = 0x10;
-  static const byte COUNTER_MASK= 0x0F;
+  static const byte SIGNAL_MASK  = 0x0C;
+  static const byte SIGNAL_RED   = 0x08;
+  static const byte SIGNAL_AMBER = 0x0C;
+  static const byte SIGNAL_GREEN = 0x04;
 
   static const byte  MAX_STACK_DEPTH=4;
  
@@ -112,7 +127,9 @@ private:
     static void setFlag(VPIN id,byte onMask, byte OffMask=0);
     static bool getFlag(VPIN id,byte mask); 
     static int16_t progtrackLocoId;
-    static void doSignal(VPIN id,bool red, bool amber, bool green); 
+    static void doSignal(VPIN id,char rag); 
+    static bool isSignal(VPIN id,char rag); 
+    static int16_t getSignalSlot(VPIN id);
     static void setTurnoutHiddenState(Turnout * t);
     static RMFT2 * loopTask;
     static RMFT2 * pausingTask;

EXRAIL2MacroReset.h

@@ -59,11 +59,14 @@
 #undef FWD 
 #undef GREEN
 #undef IF 
+#undef IFAMBER
 #undef IFCLOSED
+#undef IFGREEN
 #undef IFGTE
 #undef IFLT
 #undef IFNOT
 #undef IFRANDOM 
+#undef IFRED
 #undef IFRESERVE
 #undef IFTHROWN
 #undef IFTIMEOUT
@@ -159,11 +162,14 @@
 #define FWD(speed) 
 #define GREEN(signal_id)
 #define IF(sensor_id) 
+#define IFAMBER(signal_id)
 #define IFCLOSED(turnout_id) 
+#define IFGREEN(signal_id)
 #define IFGTE(sensor_id,value) 
 #define IFLT(sensor_id,value) 
 #define IFNOT(sensor_id)
 #define IFRANDOM(percent)
+#define IFRED(signal_id)
 #define IFTHROWN(turnout_id) 
 #define IFRESERVE(block)
 #define IFTIMEOUT

EXRAILMacros.h

@@ -235,11 +235,14 @@ const  FLASH  int16_t RMFT2::SignalDefinitions[] = {
 #define FWD(speed) OPCODE_FWD,V(speed),
 #define GREEN(signal_id) OPCODE_GREEN,V(signal_id),
 #define IF(sensor_id) OPCODE_IF,V(sensor_id),
+#define IFAMBER(signal_id) OPCODE_IFAMBER,V(signal_id),
 #define IFCLOSED(turnout_id) OPCODE_IFCLOSED,V(turnout_id),
+#define IFGREEN(signal_id) OPCODE_IFGREEN,V(signal_id),
 #define IFGTE(sensor_id,value) OPCODE_IFGTE,V(sensor_id),OPCODE_PAD,V(value),
 #define IFLT(sensor_id,value) OPCODE_IFLT,V(sensor_id),OPCODE_PAD,V(value),
 #define IFNOT(sensor_id) OPCODE_IFNOT,V(sensor_id),
 #define IFRANDOM(percent) OPCODE_IFRANDOM,V(percent),
+#define IFRED(signal_id) OPCODE_IFRED,V(signal_id),
 #define IFRESERVE(block) OPCODE_IFRESERVE,V(block),
 #define IFTHROWN(turnout_id) OPCODE_IFTHROWN,V(turnout_id),
 #define IFTIMEOUT OPCODE_IFTIMEOUT,0,0,

I2CManager_Mega4809.h

@@ -72,7 +72,7 @@ void I2CManagerClass::I2C_sendStart() {
   bytesToReceive = currentRequest->readLen;
 
   // If anything to send, initiate write.  Otherwise initiate read.
-  if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) & !bytesToSend))
+  if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) && !bytesToSend))
     TWI0.MADDR = (currentRequest->i2cAddress << 1) | 1;
   else
     TWI0.MADDR = (currentRequest->i2cAddress << 1) | 0;

I2CManager_Wire.h

@@ -94,22 +94,24 @@ uint8_t I2CManagerClass::read(uint8_t address, uint8_t readBuffer[], uint8_t rea
 /***************************************************************************
  *  Function to queue a request block and initiate operations.
  * 
- * For the Wire version, this executes synchronously, but the status is
+ * For the Wire version, this executes synchronously.
- * returned in the I2CRB as for the asynchronous version.
+ * The read/write/write_P functions return I2C_STATUS_OK always, and the 
+ * completion status of the operation is in the request block, as for
+ * the non-blocking version.
  ***************************************************************************/
 void I2CManagerClass::queueRequest(I2CRB *req) {
   switch (req->operation) {
     case OPERATION_READ:
-      req->status = read(req->i2cAddress, req->readBuffer, req->readLen, NULL, 0, req);
+      read(req->i2cAddress, req->readBuffer, req->readLen, NULL, 0, req);
       break;
     case OPERATION_SEND:
-      req->status = write(req->i2cAddress, req->writeBuffer, req->writeLen, req);
+      write(req->i2cAddress, req->writeBuffer, req->writeLen, req);
       break;
     case OPERATION_SEND_P:
-      req->status = write_P(req->i2cAddress, req->writeBuffer, req->writeLen, req);
+      write_P(req->i2cAddress, req->writeBuffer, req->writeLen, req);
       break;
     case OPERATION_REQUEST:
-      req->status = read(req->i2cAddress, req->readBuffer, req->readLen, req->writeBuffer, req->writeLen, req);
+      read(req->i2cAddress, req->readBuffer, req->readLen, req->writeBuffer, req->writeLen, req);
       break;
   }
 }

WiThrottle.cpp

@@ -258,11 +258,14 @@ void WiThrottle::parse(RingStream * stream, byte * cmdx) {
 
 int WiThrottle::getInt(byte * cmd) {
   int i=0;
+  bool negate=cmd[0]=='-';
+  if (negate) cmd++;
   while (cmd[0]>='0' && cmd[0]<='9') {
     i=i*10 + (cmd[0]-'0');
     cmd++;
   }
-  return i;    
+  if (negate) i=0-i;
+  return i ;    
 }
 
 int WiThrottle::getLocoId(byte * cmd) {

version.h

@@ -12,7 +12,13 @@
 //       Automatic ALIAS(name) 
 //       Command Parser now accepts Underscore in Alias Names
 // 4.0.2 EXRAIL additions:
-//       PARSE <> commands
+//       FIX negative route ids in WIthrottle problem. 
+//       IFRED(signal_id), IFAMBER(signal_id), IFGREEN(signal_id)
+//       </RED signal_id> </AMBER signal_id> </GREEN signal_id> commands
+//       <t cab> command to obtain current throttle settings 
+//       JA, JR, JT commands to obtain route, roster and turnout descriptions
+//       HIDDEN turnouts
+//       PARSE <> commands in EXRAIL
 //       VIRTUAL_TURNOUT
 //       </KILL ALL> and KILLALL command to stop all tasks. 
 //       FORGET forgets the current loco in DCC reminder tables.