From 4d63e621c9a577a121a1b4bc4a72b5e3d88ae1cf Mon Sep 17 00:00:00 2001
From: Rick Rongen <rickrongen@outlook.com>
Date: Fri, 28 Aug 2020 11:41:05 +0200
Subject: [PATCH] feat: handle remove of target properly

---
 control/walle.py | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/control/walle.py b/control/walle.py
index 641e9f0..6ae8236 100644
--- a/control/walle.py
+++ b/control/walle.py
@@ -51,10 +51,11 @@ class WallE:
             self.servo_positions[channel] = min_max.restval
 
     def tick(self):
+        remove = {}
         for channel, target in self.servo_targets.items():
             current_value = self.servo_positions[channel]
             if target == current_value:
-                del self.servo_targets[channel]
+                remove[channel] = target
                 continue
             try:
                 servo_min_max = SERVO_MIN_MAX[channel]
@@ -65,6 +66,9 @@ class WallE:
             new_val = current_value + step_size if target > current_value else current_value - step_size
             self.servo_controller.write(channel, new_val)
             self.servo_positions[channel] = new_val
+        for channel, target in remove.items():
+            if self.servo_targets[channel] == target:
+                del self.servo_targets[channel]
 
     def set_servo(self, channel, value):
         try: