From 250a34bc093de59da1f9ee9b01a59664c8b6949c Mon Sep 17 00:00:00 2001
From: travis-farmer <travis.farmer@outlook.com>
Date: Sun, 22 Dec 2024 16:02:07 -0500
Subject: [PATCH] sorta works

---
 IO_RSproto.cpp | 397 ++++++++++++++++++++++++++-----------------------
 IO_RSproto.h   | 110 ++++++++++----
 2 files changed, 295 insertions(+), 212 deletions(-)

diff --git a/IO_RSproto.cpp b/IO_RSproto.cpp
index f2123b7..f16beb9 100644
--- a/IO_RSproto.cpp
+++ b/IO_RSproto.cpp
@@ -37,7 +37,7 @@ RSproto::RSproto(uint8_t busNo, HardwareSerial &serial, unsigned long baud, int8
   
   _txPin = txPin;
   _busNo = busNo;
-  _cycleTime = cycleTime;
+  _cycleTime = cycleTime * 1000UL;
   bufferLength=0;
   inCommandPayload=PAYLOAD_FALSE;
   // Add device to HAL device chain
@@ -74,199 +74,233 @@ uint16_t RSproto::crc16(uint8_t *data, uint16_t length) {
 void RSproto::_loop(unsigned long currentMicros) {
   _currentMicros = currentMicros;
   //if (_busy == true) return;
-  if (_currentMicros - _cycleStartTime < _cycleTime) return;
-  _cycleStartTime = _currentMicros;
+ 
   
-  Task currentTask = getNextTask(); 
-  if (currentTask.completed != true) { // Check if a task was found
-    
-    // Calculate CRC for response data
-    if (!currentTask.rxMode) {
-      currentTask.crcPassFail = 0;
-      uint16_t response_crc = crc16((uint8_t*)currentTask.commandArray, currentTask.byteCount-1);
+  if ( hasTasks()){
+    Task* currentTask = getTaskById(getNextTaskId());
+  if (currentTask == nullptr) return;
+    if (!currentTask->rxMode) { // Check if a task was found
+      currentTask->crcPassFail = 0;
+      uint16_t response_crc = crc16((uint8_t*)currentTask->commandArray, currentTask->byteCount-1);
       if (_txPin != -1) digitalWrite(_txPin,HIGH);
       // Send response data with CRC
       _serial->write(0xFE);
       _serial->write(0xFE);
       _serial->write(response_crc >> 8);
       _serial->write(response_crc & 0xFF);
-      _serial->write(currentTask.byteCount);
-      for (int i = 0; i < currentTask.byteCount; i++) {
-        _serial->write(currentTask.commandArray[i]);
+      _serial->write(currentTask->byteCount);
+      for (int i = 0; i < currentTask->byteCount; i++) {
+        _serial->write(currentTask->commandArray[i]);
       }
       _serial->write(0xFD);
       _serial->write(0xFD);
       _serial->flush();
       if (_txPin != -1) digitalWrite(_txPin,LOW);
       // delete task command after sending, for now
-      markTaskCompleted(currentTask.taskID);
-    }
+      currentTask->rxMode = true;
+      
+    } else if (currentTask->rxMode) {
+      if (_serial->available() && currentTask->rxMode) {
+        
+        
+        uint16_t calculated_crc;
+        int byteCount = 100;
+        
+        uint8_t byte_array[byteCount];
+        int curByte = _serial->read();
+        
+        if (curByte == 0xFE && flagStart == false) flagStart = true;
+        else if ( curByte == 0xFE && flagStart == true) {
+          flagProc = false;
+          byteCounter = 0;
+          flagStarted = true;
+          flagStart = false;
+          flagEnded = false;
+          rxStart = true;
+          rxEnd = false;
+          crcPass = false;
+          memset(received_data, 0, ARRAY_SIZE);
+        }else if (flagStarted) {
+          crc[0] = curByte;
+          byteCounter++;
+          flagStarted = false;
+        } else if (byteCounter == 1) {
+          crc[1] = curByte;
+          received_crc  = (crc[0] << 8) | crc[1];
+          byteCounter++;
+        } else if (byteCounter == 2) {
+          byteCount = curByte;
+          byteCounter++;
+        } else if (flagEnded == false && byteCounter >= 3) {
+          received_data[byteCounter-3] = curByte;
+          byteCounter++;
+        }
+        if (curByte == 0xFD && flagEnd == false) flagEnd = true;
+        else if ( curByte == 0xFD && flagEnd == true) {
+          flagEnded = true;
+          flagEnd = false;
+          rxEnd = true;
+          byteCount = byteCounter;
+          byteCounter = 0;
+        }
+        if (flagEnded) {
+          calculated_crc = crc16((uint8_t*)received_data, byteCount-6);
+          if (received_crc == calculated_crc) {
+            //DIAG(F("Loop CRC PASS"));
+            crcPass = true;
+            currentTask->crcPassFail = 1;
+          }else {
+            //DIAG(F("Loop CRC Fail %x %x"),received_crc,calculated_crc);
+            currentTask->processed = true;
 
-    if (_serial->available() && currentTask.rxMode) {
-      
-      
-      uint16_t calculated_crc;
-      int byteCount = 100;
-      
-      uint8_t byte_array[byteCount];
-      int curByte = _serial->read();
-      
-      if (curByte == 0xFE && flagStart == false) flagStart = true;
-      else if ( curByte == 0xFE && flagStart == true) {
-        flagProc = false;
-        byteCounter = 0;
-        flagStarted = true;
-        flagStart = false;
-        flagEnded = false;
-        rxStart = true;
-        rxEnd = false;
-        crcPass = false;
-      }else if (flagStarted) {
-        crc[0] = curByte;
-        byteCounter++;
-        flagStarted = false;
-      } else if (byteCounter == 1) {
-        crc[1] = curByte;
-        received_crc  = (crc[0] << 8) | crc[1];
-        byteCounter++;
-      } else if (byteCounter == 2) {
-        byteCount = curByte;
-        byteCounter++;
-      } else if (flagEnded == false && byteCounter >= 3) {
-        received_data[byteCounter-3] = curByte;
-        byteCounter++;
-      }
-      if (curByte == 0xFD && flagEnd == false) flagEnd = true;
-      else if ( curByte == 0xFD && flagEnd == true) {
-        flagEnded = true;
-        flagEnd = false;
-        rxEnd = true;
-        byteCount = byteCounter;
-        byteCounter = 0;
-      }
-      if (flagEnded) {
-        calculated_crc = crc16((uint8_t*)received_data, byteCount-7);
-        if (received_crc == calculated_crc) {
-          DIAG(F("Loop CRC PASS"));
-          crcPass = true;
-          currentTask.crcPassFail = 1;
-        }else {
-          DIAG(F("Loop CRC Fail %x %x"),received_crc,calculated_crc);
-          currentTask.crcPassFail = -1;
-        } 
-        flagEnded = false;
+            currentTask->crcPassFail = -1;
+          } 
+          flagEnded = false;
         
 
+        }
+        // Check CRC validity
+        if (crcPass) {
+          // Data received successfully, process it (e.g., print)
+          int nodeTo = received_data[0];
+          if (nodeTo == 0) { // for master. master does not retransmit, or a loop will runaway.
+            flagProc = true;
+            currentTask->gotCallback = true;
+          
+          }
+          
+        } else {
+          //DIAG(F("IO_RSproto: CRC Error!"));
+        }
       }
-      // Check CRC validity
-      if (crcPass) {
-        // Data received successfully, process it (e.g., print)
+
+      // temp debug
+      
+
+      if (flagProc) {
         int nodeTo = received_data[0];
-        if (nodeTo == 0) { // for master. master does not retransmit, or a loop will runaway.
-          flagProc = true;
-          currentTask.gotCallback = true;
+        int nodeFr = received_data[1];
+        RSprotonode *node = findNode(nodeFr);
+        //DIAG(F("Node from %i %i"), nodeFr, node->getNodeID());
+        int AddrCode = received_data[2];
+        //DIAG(F("From: %i, To: %i | %i %i %i %i %i"), nodeFr,nodeTo, received_data[3], received_data[4], received_data[5], received_data[6],received_data[7]);
+        //return;
         
-        }
-        
-      } else {
-        //DIAG(F("IO_RSproto: CRC Error!"));
-      }
-    }
+        switch (AddrCode) {
+          case EXIOPINS:
+            {node->setnumDigitalPins(received_data[3]);
+            node->setnumAnalogPins(received_data[4]);
 
-    // temp debug
-    
-    //end debug
-
-    if (flagProc) {
-      int nodeTo = received_data[0];
-      int nodeFr = received_data[1];
-      int AddrCode = received_data[2];
-      //DIAG(F("From: %i, To: %i | %i %i %i %i %i"), nodeFr,nodeTo, received_data[3], received_data[4], received_data[5], received_data[6],received_data[7]);
-      //return;
-      RSprotonode *node = findNode(nodeFr);
-      switch (AddrCode) {
-        case EXIOPINS:
-          {node->setnumDigitalPins(received_data[3]);
-          node->setnumAnalogPins(received_data[4]);
-
-          // See if we already have suitable buffers assigned
-          if (node->getnumDigialPins()>0) {
-            size_t digitalBytesNeeded = (node->getnumDigialPins() + 7) / 8;
-            if (node->getdigitalPinBytes() < digitalBytesNeeded) {
-              // Not enough space, free any existing buffer and allocate a new one
-              if (node->cleandigitalPinStates(digitalBytesNeeded)) {
-                node->setdigitalPinBytes(digitalBytesNeeded);
-              } else {
-                DIAG(F("EX-IOExpander485 node:%d ERROR alloc %d bytes"), nodeFr, digitalBytesNeeded);
-                //_deviceState = DEVSTATE_FAILED;
-                node->setdigitalPinBytes(0);
-                return;
+            // See if we already have suitable buffers assigned
+            if (node->getnumDigialPins()>0) {
+              size_t digitalBytesNeeded = (node->getnumDigialPins() + 7) / 8;
+              if (node->getdigitalPinBytes() < digitalBytesNeeded) {
+                // Not enough space, free any existing buffer and allocate a new one
+                if (node->cleandigitalPinStates(digitalBytesNeeded)) {
+                  node->setdigitalPinBytes(digitalBytesNeeded);
+                } else {
+                  DIAG(F("EX-IOExpander485 node:%d ERROR alloc %d bytes"), nodeFr, digitalBytesNeeded);
+                  //_deviceState = DEVSTATE_FAILED;
+                  node->setdigitalPinBytes(0);
+                }
               }
             }
-          }
-          
-          if (node->getnumAnalogPins()>0) {
-            size_t analogueBytesNeeded = node->getnumAnalogPins() * 2;
-            if (node->getanalogPinBytes() < analogueBytesNeeded) {
-              // Free any existing buffers and allocate new ones.
-              
-              if (node->cleanAnalogStates(analogueBytesNeeded)) {
-                node->setanalogPinBytes(analogueBytesNeeded);
-              } else {
-                DIAG(F("EX-IOExpander485 node:%d ERROR alloc analog pin bytes"), nodeFr);
-                //_deviceState = DEVSTATE_FAILED;
-                node->setanalogPinBytes(0);
-                return;
+            
+            if (node->getnumAnalogPins()>0) {
+              size_t analogueBytesNeeded = node->getnumAnalogPins() * 2;
+              if (node->getanalogPinBytes() < analogueBytesNeeded) {
+                // Free any existing buffers and allocate new ones.
+                
+                if (node->cleanAnalogStates(analogueBytesNeeded)) {
+                  node->setanalogPinBytes(analogueBytesNeeded);
+                } else {
+                  DIAG(F("EX-IOExpander485 node:%d ERROR alloc analog pin bytes"), nodeFr);
+                  //_deviceState = DEVSTATE_FAILED;
+                  node->setanalogPinBytes(0);
+                }
               }
             }
+            currentTask->processed = true;
+            node->resFlag[currentTask->retFlag] = 1;
+            break;}
+          case EXIOINITA: {
+            for (int i = 0; i < node->getnumAnalogPins(); i++) {
+              node->setanalogPinMap(received_data[i+3], i);
+            }
+            currentTask->processed = true;
+            node->resFlag[currentTask->retFlag] = 1;
+            break;
           }
-          node->resFlag[currentTask.retFlag] = 1;
-          break;}
-        case EXIOINITA: {
-          for (int i = 0; i < node->getnumAnalogPins(); i++) {
-            node->setanalogPinMap(received_data[i+3], i);
+          case EXIOVER: {
+            node->setMajVer(received_data[3]);
+            node->setMinVer(received_data[4]);
+            node->setPatVer(received_data[5]);
+            DIAG(F("EX-IOExpander485: Found node %i v%i.%i.%i"),nodeFr, node->getMajVer(), node->getMinVer(), node->getPatVer());
+            //if (!_currentNode->isInitialised()) {
+              //_currentNode->setInitialised();
+              //DIAG(F("EX-IOExpander485: Initialized  Node:%d "),  nodeFr);         
+            //}
+            currentTask->processed = true;
+            node->resFlag[currentTask->retFlag] = 1;
+            break;
           }
-          node->resFlag[currentTask.retFlag] = 1;
-          break;
-        }
-        case EXIOVER: {
-          node->setMajVer(received_data[3]);
-          node->setMinVer(received_data[4]);
-          node->setPatVer(received_data[5]);
-          DIAG(F("EX-IOExpander485: Found node %i v%i.%i.%i"),node->getNodeID(), node->getMajVer(), node->getMinVer(), node->getPatVer());
-          node->resFlag[currentTask.retFlag] = 1;
-          break;
-        }
-        case EXIORDY: {
-          node->resFlag[currentTask.retFlag] = 1;
-          break;
-        }
-        case EXIOERR: {
-          node->resFlag[currentTask.retFlag] = -1;
-          break;
-        }
-        case EXIORDD: {
-          for (int i = 0; i < (node->_numDigitalPins+7)/8; i++) {
-            node->setanalogInputStates(received_data[i+3], i);
+          case EXIORDY: {
+            currentTask->processed = true;
+            node->resFlag[currentTask->retFlag] = 1;
+            break;
           }
-          node->resFlag[currentTask.retFlag] = 1;
-          break;
-        }
-        case EXIORDAN: {
-          for (int i = 0; i < node->_numAnaloguePins; i++) {
-            node->setanalogInputBuffer(received_data[i+3], i);
+          case EXIOERR: {
+            currentTask->processed = true;
+            node->resFlag[currentTask->retFlag] = -1;
+            DIAG(F("Some sort of error was received... WHAT DID YOU DO!"));
+            break;
+          }
+          case EXIORDD: {
+            for (int i = 0; i < (node->_numDigitalPins+7)/8; i++) {
+              node->setanalogInputStates(received_data[i+3], i);
+            }
+            currentTask->processed = true;
+            node->resFlag[currentTask->retFlag] = 1;
+            break;
+          }
+          case EXIORDAN: {
+            for (int i = 0; i < node->_numAnaloguePins; i++) {
+              node->setanalogInputBuffer(received_data[i+3], i);
+            }
+            currentTask->processed = true;
+            node->resFlag[currentTask->retFlag] = 1;
+            break;
           }
-          
-          node->resFlag[currentTask.retFlag] = 1;
-          break;
         }
+        
       }
-      
-    }
-    if (currentTask.gotCallback) {
-      
-   }
+      flagProc = false;
 
+    }
+    if (currentTask->processed) {
+      markTaskCompleted(currentTask->taskID);
+    }
+  }
+  
+
+  if (_currentMicros - _cycleStartTime >= _cycleTime/* && _currentNode->isInitialised()*/) {
+    _cycleStartTime = _currentMicros;
+    if (_currentNode == NULL) _currentNode = _nodeListStart;
+    RSproto *bus = RSproto::findBus(0);
+    uint8_t buffB[3];
+    buffB[0] = (_currentNode->getNodeID());
+    buffB[1] = (0);
+    buffB[2] = (EXIORDD);
+    bus->setBusy();
+    bus->addTask(buffB, 3, EXIORDD);
+
+    buffB[0] = (_currentNode->getNodeID());
+    buffB[1] = (0);
+    buffB[2] = (EXIORDD);
+    bus->setBusy();
+    bus->addTask(buffB, 3, EXIORDAN);
+    _currentNode = _currentNode->getNext();
+    //DIAG(F("Polling"));
   }
 }
 
@@ -287,6 +321,7 @@ RSprotonode::RSprotonode(VPIN firstVpin, int nPins, uint8_t nodeID) {
   _nPins = nPins;
   _busNo = 0;
   _nodeID = nodeID;
+  _initialised = false;
   memset(resFlag, 0, 255);
   //bus = bus->findBus(0);
   //_serial = bus->_serialD;
@@ -318,9 +353,8 @@ bool RSprotonode::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCoun
     buff[4] = (pullup);
     unsigned long startMillis = millis();
     RSproto *bus = RSproto::findBus(0);
-    bus->_busy = true;
-    bus->addTask(bus->taskIDCntr++, buff, 5, EXIODPUP);
-    bus->_busy = false;
+    bus->setBusy();
+    bus->addTask(buff, 5, EXIODPUP);
     
     return true;
   }
@@ -337,11 +371,10 @@ bool RSprotonode::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCoun
     buff[5] = lowByte(_firstVpin);
     unsigned long startMillis = millis();
     RSproto *bus = RSproto::findBus(0);
-    bus->_busy = true;
-    bus->addTask(bus->taskIDCntr++, buff, 6, EXIOENAN);
-    bus->_busy = false;
+    bus->setBusy();
+    bus->addTask(buff, 6, EXIOENAN);
     
-    return true;
+    return false;
   }
 
 void RSprotonode::_begin() {
@@ -354,28 +387,26 @@ void RSprotonode::_begin() {
   buff[5] = ((_firstVpin >> 8));
   unsigned long startMillis = millis();
   RSproto *bus = RSproto::findBus(0);
-  bus->_busy = true;
-  bus->addTask(bus->taskIDCntr++, buff, 6, EXIOINIT);
-  bus->_busy = false;
+  bus->initTask();
+  bus->setBusy();
+  bus->addTask(buff, 6, EXIOINIT);
   
   buff[0] = (_nodeID);
   buff[1] = (0);
   buff[2] = (EXIOINITA);
   startMillis = millis();
-  bus->_busy = true;
-  bus->addTask(bus->taskIDCntr++, buff, 3, EXIOINITA);
-  bus->_busy = false;
+  bus->setBusy();
+  bus->addTask(buff, 3, EXIOINITA);
   
   buff[0] = (_nodeID);
   buff[1] = (0);
   buff[2] = (EXIOVER);
   startMillis = millis();
-   bus->_busy = true;
-  bus->addTask(bus->taskIDCntr++, buff, 3, EXIOVER);
-  bus->_busy = false;
+   bus->setBusy();
+  bus->addTask(buff, 3, EXIOVER);
   
 
-
+  setInitialised();
 #ifdef DIAG_IO
   _display();
 #endif
@@ -399,7 +430,8 @@ void RSprotonode::_write(VPIN vpin, int value) {
     buff[4] = (value);
     unsigned long startMillis = millis();
     RSproto *bus = RSproto::findBus(0);
-    bus->addTask(bus->taskIDCntr++, buff, 5, EXIOWRD);
+    bus->setBusy();
+    bus->addTask(buff, 5, EXIOWRD);
     
   }
 
@@ -430,6 +462,7 @@ void RSprotonode::_write(VPIN vpin, int value) {
     buff[8] = lowByte(duration);
     unsigned long startMillis = millis();
     RSproto *bus = RSproto::findBus(0);
-    bus->addTask(bus->taskIDCntr++, buff, 9, EXIOWRAN);
+    bus->setBusy();
+    bus->addTask(buff, 9, EXIOWRAN);
     
   }
\ No newline at end of file
diff --git a/IO_RSproto.h b/IO_RSproto.h
index c1404f3..cd64746 100644
--- a/IO_RSproto.h
+++ b/IO_RSproto.h
@@ -93,7 +93,7 @@ private:
   uint8_t _nodeID;
   char _type;
   RSprotonode *_next = NULL;
-  bool _initialised = false;
+  bool _initialised;
   RSproto *bus;
   HardwareSerial* _serial;
   enum {
@@ -286,7 +286,7 @@ private:
   int _operationCount = 0;
   int _refreshOperation = 0;
   byte bufferLength;
-  static const int ARRAY_SIZE = 254;
+  static const int ARRAY_SIZE = 75;
   int buffer[ARRAY_SIZE]; 
     byte inCommandPayload;
   static RSproto *_busList; // linked list of defined bus instances
@@ -329,42 +329,82 @@ struct Task {
     bool gotCallback;
     bool rxMode;
     int crcPassFail;
-    bool completed = false;
+    bool completed;
+    bool processed;
   };
+static const int MAX_TASKS = 100;
   int taskIDCntr = 0;
-Task taskBuffer[100]; // Buffer to hold up to 100 tasks
-int taskCount = 0;
-void addTask(int id, const uint8_t* cmd, int byteCount, uint8_t retFlag, bool gotCallBack=false, bool rxMode=false, int crcPassFail=0) {
-  if (taskCount < 100) { // Check if buffer is not full
-    taskBuffer[taskCount].taskID = id;
-    memcpy(taskBuffer[taskCount].commandArray, cmd, ARRAY_SIZE); 
-    taskBuffer[taskCount].commandArray[ARRAY_SIZE] = 0; // Ensure null-termination
-    taskBuffer[taskCount].byteCount = byteCount;
-    taskBuffer[taskCount].retFlag = retFlag;
-    taskBuffer[taskCount].gotCallback = false;
-    taskBuffer[taskCount].rxMode = false;
-    taskBuffer[taskCount].crcPassFail = 0;
-    taskBuffer[taskCount].completed = false;
-    taskCount++;
-  } else {
-    Serial.println("Task buffer overflow!");
+Task taskBuffer[MAX_TASKS]; // Buffer to hold up to 100 tasks
+int currentTaskIndex = 0;
+void initTask() {
+  for (int i = 0; i <MAX_TASKS; i++) {
+    taskBuffer[i].completed = true;
+    taskBuffer[i].processed = false;
+    taskBuffer[i].byteCount = 0;
+    taskBuffer[i].crcPassFail = 0;
+    taskBuffer[i].gotCallback = false;
+    taskBuffer[i].retFlag = 0;
+    taskBuffer[i].rxMode = false;
+    taskBuffer[i].taskID = i;
+    memset(taskBuffer[i].commandArray,0,ARRAY_SIZE);
   }
 }
-Task getNextTask() {
-  for (int i = 0; i < taskCount; i++) {
-    if (!taskBuffer[i].completed) {
-      return taskBuffer[i]; 
+void addTask(const uint8_t* cmd, int byteCount, uint8_t retFlag) {
+  // Find an empty slot in the buffer
+  int emptySlot = -1;
+  for (int i = 0; i < MAX_TASKS; i++) {
+    if (taskBuffer[i].completed) {
+      emptySlot = i;
+      break;
     }
   }
-  // Return a default task if no uncompleted tasks found
-  Task emptyTask; 
-  return emptyTask;
+  // If no empty slot found, return (buffer full)
+  if (emptySlot == -1) {
+    DIAG(F("Task Buffer Full!"));
+    return;
+  }
+  for (int i = 0; i < ARRAY_SIZE; i++) taskBuffer[emptySlot].commandArray[i] = cmd[i];
+  taskBuffer[emptySlot].byteCount = byteCount;
+  taskBuffer[emptySlot].retFlag = retFlag;
+  taskBuffer[emptySlot].rxMode = false;
+  taskBuffer[emptySlot].crcPassFail = 0;
+  taskBuffer[emptySlot].gotCallback = false;
+  taskBuffer[emptySlot].completed = false;
+  taskBuffer[emptySlot].processed = false;
+  currentTaskIndex = emptySlot; 
 }
-void markTaskCompleted(int taskID) {
-  for (int i = 0; i < taskCount; i++) {
-    if (taskBuffer[i].taskID == taskID) {
+bool hasTasks() {
+  for (int i = 0; i < MAX_TASKS; i++) {
+    if (!taskBuffer[i].completed) {
+      return true; // At least one task is not completed
+    }
+  }
+  return false; // All tasks are completed
+}
+// Function to get a specific task by ID
+Task* getTaskById(int id) {
+  for (int i = 0; i < MAX_TASKS; i++) {
+    if (taskBuffer[i].taskID == id) {
+      return &taskBuffer[i]; // Return a pointer to the task
+    }
+  }
+  return nullptr; // Task not found
+}
+// Function to get the next task (optional)
+int getNextTaskId() {
+  for (int i = 0; i < MAX_TASKS; i++) {
+    if (!taskBuffer[i].completed) {
+      return taskBuffer[i].taskID; 
+    }
+  }
+  return -1; // No tasks available
+}
+// Function to mark a task as completed
+void markTaskCompleted(int id) {
+  for (int i = 0; i < MAX_TASKS; i++) {
+    if (taskBuffer[i].taskID == id) {
       taskBuffer[i].completed = true;
-      break; 
+      break;
     }
   }
 }
@@ -408,6 +448,15 @@ uint16_t crc16(uint8_t *data, uint16_t length);
   uint16_t _pin;
   int8_t _txPin;
   bool _busy = false;
+  void setBusy() {
+    _busy = true;
+  }
+  void clearBusy() {
+    _busy = false;
+  }
+  bool getBusy() {
+    return _busy;
+  }
   unsigned long _baud;
   int taskCnt = 0;
   uint8_t initBuffer[1] = {0xFE};
@@ -418,6 +467,7 @@ uint16_t crc16(uint8_t *data, uint16_t length);
     if (_txPin >0) {
       pinMode(_txPin, OUTPUT);
       digitalWrite(_txPin, LOW);
+      
     }
   #if defined(RSproto_STM_OK)
     pinMode(RSproto_STM_OK, OUTPUT);