diff --git a/src/main.cpp b/src/main.cpp
index d44cec0..44c841a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -39,15 +39,20 @@ bool sendSetPosition(uint16_t destination, float percentage);
 bool sendFetchPosition(uint16_t destination);
 bool sendPacket(Packet *packet);
 
-bool updatePosition(uint16_t shadeID);
+bool checkPosition(uint16_t shadeID);
+void startFetchingPosition(uint16_t shadeID, int8_t targetPosition);
+
+void publishPosition(const String& shadeName, const uint8_t position);
+void publishState(const String& shadeName, const String& state);
 
 struct Shade {
   uint16_t ID;
   String name;
-  uint8_t lastTargetPosition;
-  uint8_t lastPosition;
-  uint8_t lastPositionCount;
+  int8_t lastTargetPosition;
+  int8_t lastPosition;
+  uint8_t samePositionCount;
   uint8_t positionFetchCount;
+  void* timer;
 };
 
 std::vector<Shade> shades;
@@ -57,21 +62,21 @@ void setup() {
 
   Serial.println("Starting up");
 
-  shades.push_back(Shade{0x4EF1, "study_blind"});
-  shades.push_back(Shade{0xA51F, "studio_blockout_left_blind"});
-  shades.push_back(Shade{0xDAEF, "studio_blockout_right_blind"});
-  shades.push_back(Shade{0x7687, "studio_left_blind"});
-  shades.push_back(Shade{0xB0DE, "studio_right_blind"});
-  shades.push_back(Shade{0xB451, "bedroom_door_blockout_blind"});
-  shades.push_back(Shade{0x48A6, "bedroom_window_blockout_blind"});
-  shades.push_back(Shade{0x9E14, "bedroom_door_blind"});
-  shades.push_back(Shade{0x061C, "bedroom_window_blind"});
-  shades.push_back(Shade{0x2959, "bathroom_blind"});
-  shades.push_back(Shade{0x6FAD, "kitchen_door_blind"});
-  shades.push_back(Shade{0xFB21, "kitchen_window_blind"});
-  shades.push_back(Shade{0x8B10, "living_room_big_window_blind"});
-  shades.push_back(Shade{0x3EB8, "living_room_door_blind"});
-  shades.push_back(Shade{0x5463, "living_room_window_blind"});
+  shades.push_back(Shade{0x4EF1, "study_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0xA51F, "studio_blockout_left_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0xDAEF, "studio_blockout_right_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x7687, "studio_left_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0xB0DE, "studio_right_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0xB451, "bedroom_door_blockout_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x48A6, "bedroom_window_blockout_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x9E14, "bedroom_door_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x061C, "bedroom_window_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x2959, "bathroom_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x6FAD, "kitchen_door_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0xFB21, "kitchen_window_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x8B10, "living_room_big_window_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x3EB8, "living_room_door_blind", -1, -1, 0, 0, nullptr});
+  shades.push_back(Shade{0x5463, "living_room_window_blind", -1, -1, 0, 0, nullptr});
 
   powerView.setPacketCallback(processPacket);
   if (!powerView.begin()) {
@@ -109,14 +114,15 @@ 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].samePositionCount++;
             } else {
-              shades[i].lastPositionCount = 0;
+              shades[i].samePositionCount = 1;
             }
             shades[i].lastPosition = position;
-            String payload = String(position);
-            client.publish("hotdog/" + shades[i].name + "/position", payload, true);
+
+            publishPosition(shades[i].name, position);
           }
         }
       }
@@ -212,46 +218,6 @@ bool sendPacket(Packet *packet) {
   }
 }
 
-bool updatePosition(uint16_t shadeID) {
-  for (size_t i = 0; i < shades.size(); i++) {
-    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
-          if (shades[i].lastPosition > 0) {
-            client.publish("hotdog/" + shades[i].name + "/state", "open", true);
-          } else {
-            client.publish("hotdog/" + shades[i].name + "/state", "closed", true);
-          }
-          return false;
-        }
-
-        if (shades[i].lastPositionCount >= 2) {
-          // Blind hasn't moved after 3 fetches, so give up waiting
-          if (shades[i].lastPosition > 0) {
-            client.publish("hotdog/" + shades[i].name + "/state", "open", true);
-          } else {
-            client.publish("hotdog/" + shades[i].name + "/state", "closed", true);
-          }
-          return false;
-        }
-
-        shades[i].positionFetchCount++;
-        sendFetchPosition(shadeID);
-        return true;
-      } else {
-        if (shades[i].lastPosition > 0) {
-          client.publish("hotdog/" + shades[i].name + "/state", "open", true);
-        } else {
-          client.publish("hotdog/" + shades[i].name + "/state", "closed", true);
-        }
-        return false;
-      }
-    }
-  }
-  return false;
-}
-
 void processSetMessage(const String &topic, const String &payload) {
   int startIndex = topic.indexOf("/") + 1;
   int endIndex = topic.indexOf("/", startIndex);
@@ -262,31 +228,20 @@ void processSetMessage(const String &topic, const String &payload) {
       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);
+        startFetchingPosition(shades[i].ID, 100);
 
-        client.publish("hotdog/" + shades[i].name + "/state", "opening", true);
+        publishState(shades[i].name, "opening");
       } 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);
+        startFetchingPosition(shades[i].ID, 0);
 
-        client.publish("hotdog/" + shades[i].name + "/state", "closing", true);
+        publishState(shades[i].name, "closing");
       } else if (payload == "STOP") {
         sendStopPacket(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);
+        startFetchingPosition(shades[i].ID, -1);
+        timer.in(100, sendFetchPosition, shades[i].ID);
       }
     }
   }
@@ -303,16 +258,61 @@ void processSetPositionMessage(const String& topic, const String &payload)  {
       sendSetPosition(shades[i].ID, percentage);
 
       if (payload.toInt() > shades[i].lastPosition) {
-        client.publish("hotdog/" + shades[i].name + "/state", "opening", true);
+        publishState(shades[i].name, "opening");
       } else if (payload.toInt() < shades[i].lastPosition) {
-        client.publish("hotdog/" + shades[i].name + "/state", "closing", true);
+        publishState(shades[i].name, "closing");
       }
 
-      shades[i].lastPosition = -1;
-      shades[i].lastTargetPosition = payload.toInt();
-      shades[i].positionFetchCount = 0;
-      shades[i].lastPositionCount = 0;
-      timer.every(2000, updatePosition, shades[i].ID);
+      startFetchingPosition(shades[i].ID, payload.toInt());
     }
   }
+}
+
+bool checkPosition(uint16_t shadeID) {
+  for (size_t i = 0; i < shades.size(); i++) {
+    if (shades[i].ID == shadeID) {
+      // Keep fetching position if:
+      // - the last reported position doesn't match the target position
+      // - a position hasn't been reported yet
+      // - there is no target position
+      if (shades[i].lastTargetPosition != shades[i].lastPosition || shades[i].lastPosition == -1 || shades[i].lastTargetPosition == -1) {
+        // Keep fetching position if the count limits have not been reached
+        if (shades[i].positionFetchCount < MAX_FETCH_COUNT && shades[i].samePositionCount < 2) {
+          shades[i].positionFetchCount++;
+          sendFetchPosition(shadeID);
+          return true;
+        }
+      }
+
+      publishState(shades[i].name, shades[i].lastPosition > 0 ? "open" : "closed");
+      return false;
+    }
+  }
+  return false;
+}
+
+void startFetchingPosition(uint16_t shadeID, int8_t targetPosition) {
+  for (size_t i = 0; i < shades.size(); i++) {
+    if (shades[i].ID == shadeID) {
+      // Cancel any existing timer
+      if (shades[i].timer != nullptr) {
+        timer.cancel(shades[i].timer);
+        shades[i].timer = nullptr;
+      }
+
+      shades[i].lastTargetPosition = targetPosition;
+      shades[i].positionFetchCount = 0;
+      shades[i].samePositionCount = 1;
+
+      shades[i].timer = timer.every(2000, checkPosition, shades[i].ID);
+    }
+  }
+}
+
+void publishState(const String& shadeName, const String& state) {
+  client.publish("hotdog/" + shadeName + "/state", state, true);
+}
+
+void publishPosition(const String& shadeName, const uint8_t position) {
+  client.publish("hotdog/" + shadeName + "/position", String(position), true);
 }
\ No newline at end of file