Fetch position every 2 seconds after sending a command

src/main.cpp

@@ -25,6 +25,7 @@ EspMQTTClient client(
   1883
 );
 
+#define MAX_FETCH_COUNT               (20)
 auto timer = Timer<10, millis, uint16_t>();
 
 void processPacket(const Packet*);
@@ -43,6 +44,10 @@ bool updatePosition(uint16_t shadeID);
 struct Shade {
   uint16_t ID;
   String name;
+  uint8_t lastTargetPosition;
+  uint8_t lastPosition;
+  uint8_t lastPositionCount;
+  uint8_t positionFetchCount;
 };
 
 std::vector<Shade> shades;
@@ -104,6 +109,10 @@ void processPacket(const Packet *packet) {
           if (packet->source == shades[i].ID) {
             uint16_t value = std::get<uint16_t>(field.value);
             uint8_t position = (uint8_t)std::round(((float)value / 0xFFFF) * 100);
+            if (shades[i].lastPosition == position) {
+              shades[i].lastPositionCount++;
+            }
+            shades[i].lastPosition = position;
             String payload = String(position);
             client.publish("hotdog/" + shades[i].name + "/position", payload, true);
           }
@@ -116,7 +125,7 @@ void processPacket(const Packet *packet) {
 void onConnectionEstablished() {
   Serial.println("Connection established");
 
-  for (int i = 0; i < shades.size(); i++) {
+  for (size_t i = 0; i < shades.size(); i++) {
     client.subscribe("hotdog/" + shades[i].name + "/set", processSetMessage);
     client.subscribe("hotdog/" + shades[i].name + "/set_position", processSetPositionMessage);
   }
@@ -202,8 +211,27 @@ bool sendPacket(Packet *packet) {
 }
 
 bool updatePosition(uint16_t shadeID) {
-  Serial.println("Triggering position update");
+  for (size_t i = 0; i < shades.size(); i++) {
-  sendFetchPosition(shadeID);
+    if (shades[i].ID == shadeID) {
+      if (shades[i].lastTargetPosition != shades[i].lastPosition || shades[i].lastPosition == -1) {
+        if (shades[i].positionFetchCount >= MAX_FETCH_COUNT) {
+          // Give up waiting for the blind to reach the target position
+          return false;
+        }
+
+        if (shades[i].lastPositionCount >= 3) {
+          // Blind hasn't moved after 3 fetches, so give up waiting
+          return false;
+        }
+
+        shades[i].positionFetchCount++;
+        sendFetchPosition(shadeID);
+        return true;
+      } else {
+        return false;
+      }
+    }
+  }
   return false;
 }
 
@@ -216,12 +244,28 @@ void processSetMessage(const String &topic, const String &payload) {
     if (shades[i].name == shadeName) {
       if (payload == "OPEN") {
         sendOpenPacket(shades[i].ID);
+
+        shades[i].lastPosition = -1;
+        shades[i].lastTargetPosition = 100;
+        shades[i].positionFetchCount = 0;
+        shades[i].lastPositionCount = 0;
+        timer.every(2000, updatePosition, shades[i].ID);
       } else if (payload == "CLOSE") {
         sendClosePacket(shades[i].ID);
+
+        shades[i].lastPosition = -1;
+        shades[i].lastTargetPosition = 0;
+        shades[i].positionFetchCount = 0;
+        shades[i].lastPositionCount = 0;
+        timer.every(2000, updatePosition, shades[i].ID);
       } else if (payload == "STOP") {
         sendStopPacket(shades[i].ID);
-        // TODO: Schedule fetching position of blind
+
-        sendFetchPosition(shades[i].ID);
+        shades[i].lastPosition = -1;
+        shades[i].lastTargetPosition = -1;
+        shades[i].positionFetchCount = 0;
+        shades[i].lastPositionCount = 0;
+        timer.every(2000, updatePosition, shades[i].ID);
       }
     }
   }
@@ -236,14 +280,12 @@ void processSetPositionMessage(const String& topic, const String &payload)  {
     if (shades[i].name == shadeName) {
       float percentage = payload.toInt() / 100.0f;
       sendSetPosition(shades[i].ID, percentage);
-      // TODO: Schedule fetching position of blind
+
-      timer.in(1000, updatePosition, shades[i].ID);
+      shades[i].lastPosition = -1;
-      timer.in(3000, updatePosition, shades[i].ID);
+      shades[i].lastTargetPosition = payload.toInt();
-      timer.in(5000, updatePosition, shades[i].ID);
+      shades[i].positionFetchCount = 0;
-      timer.in(7000, updatePosition, shades[i].ID);
+      shades[i].lastPositionCount = 0;
-      timer.in(9000, updatePosition, shades[i].ID);
+      timer.every(2000, updatePosition, shades[i].ID);
-      timer.in(15000, updatePosition, shades[i].ID);
-      timer.in(30000, updatePosition, shades[i].ID);
     }
   }
 }