Add RMFT WAITFOR() and SERVO2() commands.

WAITFOR(pin) waits until the corresponding pin is not busy (e.g. has finished moving the servo).  SERVO2(pin, value, ms) moves to the nominated position in a time given in milliseconds by ms.

IODevice.cpp

@@ -253,6 +253,7 @@ void IODevice::write(VPIN vpin, int value) {
   pinMode(vpin, OUTPUT);
 }
 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);

RMFT2.cpp

@@ -611,6 +611,13 @@ void RMFT2::loop2() {
         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);
             break;

RMFT2.h

@@ -36,7 +36,7 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
              OPCODE_DELAY,OPCODE_DELAYMINS,OPCODE_RANDWAIT,
              OPCODE_FON,OPCODE_FOFF,
              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,

RMFTMacros.h

@@ -91,15 +91,16 @@
 #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 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 
@@ -111,6 +112,7 @@
 #define TURNOUT(id,addr,subaddr) 
 #define UNJOIN 
 #define UNLATCH(sensor_id) 
+#define WAITFOR(pin)
 
 #include "myAutomation.h"
 
@@ -179,6 +181,7 @@ const int StringMacroTracker1=__COUNTER__;
 #undef START
 #undef SEQUENCE 
 #undef SERVO
+#undef SERVO2
 #undef FADE
 #undef SENDLOCO
 #undef SETLOCO
@@ -192,6 +195,7 @@ const int StringMacroTracker1=__COUNTER__;
 #undef TURNOUT
 #undef UNJOIN
 #undef UNLATCH
+#undef WAITFOR
 
 // Define macros for route code creation 
 #define V(val) ((int16_t)(val))&0x00FF,((int16_t)(val)>>8)&0x00FF
@@ -239,15 +243,16 @@ const int StringMacroTracker1=__COUNTER__;
 #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 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),OPCODE_PAD,V(0),
+#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), 
@@ -257,6 +262,7 @@ 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),
 
 // PASS2 Build RouteCode
 const int StringMacroTracker2=__COUNTER__;