diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py
index 036f28d..dc69da4 100644
--- a/toid_interaction/toid_interaction/interaction_node.py
+++ b/toid_interaction/toid_interaction/interaction_node.py
@@ -25,11 +25,6 @@ class InteracitionNode(Node):
     output_pin_: OutputDevice
     start_pin_action_: ActionServer
 
-    servo_2_pos: float = 0
-    servo_3_pos: float = 0
-    servo_4_pos: float = 0
-    servo_5_pos: float = 0
-
     def __init__(self):
         super().__init__("ToidInteractionNode")
 
@@ -76,11 +71,13 @@ class InteracitionNode(Node):
         if self.step != 1:
             return SendString.Response()
 
-        self.move_wall(600,210)
+        zidovi = Zidovi(self.st_motor_device_name)
+
+        zidovi.zidovi(1, 1500, 600, 210, 120, 120)
 
         okreni_niz(request.text)
 
-        self.move_wall(180,160)
+        zidovi.zidovi(0, 1500, 420, 50, 120, 120)
         self.step = 2
         return response
 
@@ -89,32 +86,14 @@ class InteracitionNode(Node):
             return Empty.Response()
 
         zupcanik = ZupcanikAction(self.st_motor_device_name)
+        zidovi = Zidovi(self.st_motor_device_name)
 
         zupcanik.zupcanik(1, 1015, 25)
-
-        self.move_wall(3,3)
-
+        zidovi.zidovi(0, 1500, 180, 160, 120, 120)
         okreni(5)
         self.step = 0
         return response
 
-    def move_wall(self, white : float, blue : float, max_current:float = 110):
-        zidovi = Zidovi(self.st_motor_device_name)
-        sign = -1 if self.servo_3_pos > white else 1
-        targets = (
-            abs(self.servo_2_pos - blue),
-            abs(self.servo_3_pos - white),
-            abs(self.servo_4_pos - blue),
-            abs(self.servo_5_pos - white),
-        )
-
-        _, dist1, dist2, dist3, dist4 = zidovi.zidovi(max(sign, 0), TargetPos=targets)
-
-        self.servo_2_pos += dist1 * sign
-        self.servo_3_pos += dist2 * sign
-        self.servo_4_pos += dist3 * sign
-        self.servo_5_pos += dist4 * sign        
-
     async def start_plug_action_cb(self, goal_handle: ServerGoalHandle):
         while not self.btn_.is_active:
             pass
diff --git a/toid_interaction/toid_interaction/mechanism/zidovi.py b/toid_interaction/toid_interaction/mechanism/zidovi.py
index cab7695..4ee2140 100644
--- a/toid_interaction/toid_interaction/mechanism/zidovi.py
+++ b/toid_interaction/toid_interaction/mechanism/zidovi.py
@@ -54,13 +54,10 @@ class Zidovi:
                     time.sleep(0.01)
 
     def zidovi(self, smer, brzina=1500,
-               TargetPos : tuple[float,float,float,float] = (0,0,0,0),
+               TargetPos_beli=600, TargetPos_plavi=300,
                opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150):
-
-        TargetPos_2 = TargetPos[0] * 4096 / 360
-        TargetPos_3 = TargetPos[1] * 4096 / 360
-        TargetPos_4 = TargetPos[2] * 4096 / 360
-        TargetPos_5 = TargetPos[3] * 4096 / 360
+        TargetPos_beli = TargetPos_beli * 4096 / 360
+        TargetPos_plavi = TargetPos_plavi * 4096 / 360
 
         if smer == 1:
             Speed_ID2 = brzina
@@ -90,19 +87,19 @@ class Zidovi:
                 curr_time = time.time()
 
                 if smer == 1:
-                    motor3.doCycle(TargetPos_3, Speed_ID3)
-                    motor5.doCycle(TargetPos_5, Speed_ID5)
+                    motor3.doCycle(TargetPos_beli, Speed_ID3)
+                    motor5.doCycle(TargetPos_beli, Speed_ID5)
                     if brojac >= 50:
-                        motor2.doCycle(TargetPos_2, Speed_ID2)
-                        motor4.doCycle(TargetPos_4, Speed_ID4)
+                        motor2.doCycle(TargetPos_plavi, Speed_ID2)
+                        motor4.doCycle(TargetPos_plavi, Speed_ID4)
                     else:
                         brojac += 1
                 else:
-                    motor2.doCycle(TargetPos_2, Speed_ID2)
-                    motor4.doCycle(TargetPos_4, Speed_ID4)
+                    motor2.doCycle(TargetPos_plavi, Speed_ID2)
+                    motor4.doCycle(TargetPos_plavi, Speed_ID4)
                     if brojac >= 50:
-                        motor3.doCycle(TargetPos_3, Speed_ID3)
-                        motor5.doCycle(TargetPos_5, Speed_ID5)
+                        motor3.doCycle(TargetPos_beli, Speed_ID3)
+                        motor5.doCycle(TargetPos_beli, Speed_ID5)
                     else:
                         brojac += 1
 
@@ -127,7 +124,7 @@ class Zidovi:
                 if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi:
 
                     load_blue_count+=1
-                    if load_blue_count > 10:
+                    if load_blue_count > 2:
                         print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4))
                         stop_motor(self.packetHandler, 2)
                         stop_motor(self.packetHandler, 4)
@@ -138,7 +135,7 @@ class Zidovi:
 
                 if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli:
                     load_white_count+=1
-                    if load_white_count > 10:
+                    if load_white_count > 2:
                         print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5))
                         stop_motor(self.packetHandler, 3)
                         stop_motor(self.packetHandler, 5)