From 0a0c6da6f081384e1c0eb4beff282254e2aa9687 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Thu, 16 Apr 2026 23:58:32 +0200 Subject: [PATCH] New method to keep track of servo positions --- .../toid_interaction/interaction_node.py | 33 +++++++++++++++---- .../toid_interaction/mechanism/zidovi.py | 29 ++++++++-------- 2 files changed, 43 insertions(+), 19 deletions(-) diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index dc69da4..036f28d 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/toid_interaction/toid_interaction/interaction_node.py @@ -25,6 +25,11 @@ 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") @@ -71,13 +76,11 @@ class InteracitionNode(Node): if self.step != 1: return SendString.Response() - zidovi = Zidovi(self.st_motor_device_name) - - zidovi.zidovi(1, 1500, 600, 210, 120, 120) + self.move_wall(600,210) okreni_niz(request.text) - zidovi.zidovi(0, 1500, 420, 50, 120, 120) + self.move_wall(180,160) self.step = 2 return response @@ -86,14 +89,32 @@ 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) - zidovi.zidovi(0, 1500, 180, 160, 120, 120) + + self.move_wall(3,3) + 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 4ee2140..cab7695 100644 --- a/toid_interaction/toid_interaction/mechanism/zidovi.py +++ b/toid_interaction/toid_interaction/mechanism/zidovi.py @@ -54,10 +54,13 @@ class Zidovi: time.sleep(0.01) def zidovi(self, smer, brzina=1500, - TargetPos_beli=600, TargetPos_plavi=300, + TargetPos : tuple[float,float,float,float] = (0,0,0,0), opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150): - TargetPos_beli = TargetPos_beli * 4096 / 360 - TargetPos_plavi = TargetPos_plavi * 4096 / 360 + + TargetPos_2 = TargetPos[0] * 4096 / 360 + TargetPos_3 = TargetPos[1] * 4096 / 360 + TargetPos_4 = TargetPos[2] * 4096 / 360 + TargetPos_5 = TargetPos[3] * 4096 / 360 if smer == 1: Speed_ID2 = brzina @@ -87,19 +90,19 @@ class Zidovi: curr_time = time.time() if smer == 1: - motor3.doCycle(TargetPos_beli, Speed_ID3) - motor5.doCycle(TargetPos_beli, Speed_ID5) + motor3.doCycle(TargetPos_3, Speed_ID3) + motor5.doCycle(TargetPos_5, Speed_ID5) if brojac >= 50: - motor2.doCycle(TargetPos_plavi, Speed_ID2) - motor4.doCycle(TargetPos_plavi, Speed_ID4) + motor2.doCycle(TargetPos_2, Speed_ID2) + motor4.doCycle(TargetPos_4, Speed_ID4) else: brojac += 1 else: - motor2.doCycle(TargetPos_plavi, Speed_ID2) - motor4.doCycle(TargetPos_plavi, Speed_ID4) + motor2.doCycle(TargetPos_2, Speed_ID2) + motor4.doCycle(TargetPos_4, Speed_ID4) if brojac >= 50: - motor3.doCycle(TargetPos_beli, Speed_ID3) - motor5.doCycle(TargetPos_beli, Speed_ID5) + motor3.doCycle(TargetPos_3, Speed_ID3) + motor5.doCycle(TargetPos_5, Speed_ID5) else: brojac += 1 @@ -124,7 +127,7 @@ class Zidovi: if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi: load_blue_count+=1 - if load_blue_count > 2: + if load_blue_count > 10: print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4)) stop_motor(self.packetHandler, 2) stop_motor(self.packetHandler, 4) @@ -135,7 +138,7 @@ class Zidovi: if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli: load_white_count+=1 - if load_white_count > 2: + if load_white_count > 10: print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5)) stop_motor(self.packetHandler, 3) stop_motor(self.packetHandler, 5)