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

Reverted mechanism code to ol'reliable

Browse Source
This commit is contained in:
Pimpest
2026-04-17 11:39:29 +02:00
parent d26adb576a
commit 6ff7af65ea
2 changed files with 19 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
toid_interaction/toid_interaction/interaction_node.py
View File

@@ -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

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

@@ -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)