Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

New method to keep track of servo positions

Browse Source
This commit is contained in:
Pimpest
2026-04-16 23:58:32 +02:00
parent e3ea359508
commit 0a0c6da6f0
2 changed files with 43 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
toid_interaction/toid_interaction/interaction_node.py
View File

@@ -25,6 +25,11 @@ class InteracitionNode(Node):
output_pin_: OutputDevice output_pin_: OutputDevice
start_pin_action_: ActionServer 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): def __init__(self):
super().__init__("ToidInteractionNode") super().__init__("ToidInteractionNode")
@@ -71,13 +76,11 @@ class InteracitionNode(Node):
if self.step != 1: if self.step != 1:
return SendString.Response() return SendString.Response()
zidovi = Zidovi(self.st_motor_device_name) self.move_wall(600,210)
zidovi.zidovi(1, 1500, 600, 210, 120, 120)
okreni_niz(request.text) okreni_niz(request.text)
zidovi.zidovi(0, 1500, 420, 50, 120, 120) self.move_wall(180,160)
self.step = 2 self.step = 2
return response return response
@@ -86,14 +89,32 @@ class InteracitionNode(Node):
return Empty.Response() return Empty.Response()
zupcanik = ZupcanikAction(self.st_motor_device_name) zupcanik = ZupcanikAction(self.st_motor_device_name)
zidovi = Zidovi(self.st_motor_device_name)
zupcanik.zupcanik(1, 1015, 25) zupcanik.zupcanik(1, 1015, 25)
zidovi.zidovi(0, 1500, 180, 160, 120, 120)
self.move_wall(3,3)
okreni(5) okreni(5)
self.step = 0 self.step = 0
return response 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): async def start_plug_action_cb(self, goal_handle: ServerGoalHandle):
while not self.btn_.is_active: while not self.btn_.is_active:
pass pass

29
toid_interaction/toid_interaction/mechanism/zidovi.py
View File

@@ -54,10 +54,13 @@ class Zidovi:
time.sleep(0.01) time.sleep(0.01)
def zidovi(self, smer, brzina=1500, 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): 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: if smer == 1:
Speed_ID2 = brzina Speed_ID2 = brzina
@@ -87,19 +90,19 @@ class Zidovi:
curr_time = time.time() curr_time = time.time()
if smer == 1: if smer == 1:
motor3.doCycle(TargetPos_beli, Speed_ID3) motor3.doCycle(TargetPos_3, Speed_ID3)
motor5.doCycle(TargetPos_beli, Speed_ID5) motor5.doCycle(TargetPos_5, Speed_ID5)
if brojac >= 50: if brojac >= 50:
motor2.doCycle(TargetPos_plavi, Speed_ID2) motor2.doCycle(TargetPos_2, Speed_ID2)
motor4.doCycle(TargetPos_plavi, Speed_ID4) motor4.doCycle(TargetPos_4, Speed_ID4)
else: else:
brojac += 1 brojac += 1
else: else:
motor2.doCycle(TargetPos_plavi, Speed_ID2) motor2.doCycle(TargetPos_2, Speed_ID2)
motor4.doCycle(TargetPos_plavi, Speed_ID4) motor4.doCycle(TargetPos_4, Speed_ID4)
if brojac >= 50: if brojac >= 50:
motor3.doCycle(TargetPos_beli, Speed_ID3) motor3.doCycle(TargetPos_3, Speed_ID3)
motor5.doCycle(TargetPos_beli, Speed_ID5) motor5.doCycle(TargetPos_5, Speed_ID5)
else: else:
brojac += 1 brojac += 1
@@ -124,7 +127,7 @@ class Zidovi:
if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi: if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi:
load_blue_count+=1 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)) print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4))
stop_motor(self.packetHandler, 2) stop_motor(self.packetHandler, 2)
stop_motor(self.packetHandler, 4) stop_motor(self.packetHandler, 4)
@@ -135,7 +138,7 @@ class Zidovi:
if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli: if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli:
load_white_count+=1 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)) print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5))
stop_motor(self.packetHandler, 3) stop_motor(self.packetHandler, 3)
stop_motor(self.packetHandler, 5) stop_motor(self.packetHandler, 5)