168 lines
6.9 KiB
Python
168 lines
6.9 KiB
Python
from .STservo_sdk import PortHandler, sts
|
|
from .motori import get_load, stop_motor, get_current, CrniMotor
|
|
import time
|
|
|
|
|
|
BAUDRATE = 1000000
|
|
STS_MOVING_ACC = 0
|
|
|
|
|
|
class Zidovi:
|
|
def __init__(self, devicename: str = '/dev/ttyUSB0'):
|
|
self.devicename = devicename
|
|
self.portHandler = None
|
|
self.packetHandler = None
|
|
|
|
def _open_port(self):
|
|
self.portHandler = PortHandler(self.devicename)
|
|
self.packetHandler = sts(self.portHandler)
|
|
if self.portHandler.openPort():
|
|
print("Succeeded to open the port")
|
|
else:
|
|
print("Failed to open the port")
|
|
raise RuntimeError(f"Failed to open port: {self.devicename}")
|
|
|
|
def _close_port(self):
|
|
if self.portHandler is not None:
|
|
for motor_id in (3, 5, 2, 4):
|
|
stopped = False
|
|
while not stopped:
|
|
try:
|
|
stop_motor(self.packetHandler, motor_id)
|
|
stopped = True
|
|
except Exception:
|
|
self.portHandler.ser.reset_input_buffer()
|
|
self.portHandler.ser.reset_output_buffer()
|
|
time.sleep(0.01)
|
|
self.portHandler.closePort()
|
|
|
|
def _stop_all(self):
|
|
# Reset serial buffers — a KeyboardInterrupt can fire mid-packet,
|
|
# leaving garbage in the buffers that would corrupt subsequent writes.
|
|
if self.portHandler is not None and self.portHandler.ser is not None:
|
|
self.portHandler.ser.reset_input_buffer()
|
|
self.portHandler.ser.reset_output_buffer()
|
|
for motor_id in (2, 3, 4, 5):
|
|
stopped = False
|
|
while not stopped:
|
|
try:
|
|
stop_motor(self.packetHandler, motor_id)
|
|
stopped = True
|
|
except Exception:
|
|
self.portHandler.ser.reset_input_buffer()
|
|
self.portHandler.ser.reset_output_buffer()
|
|
time.sleep(0.01)
|
|
|
|
def zidovi(self, smer, brzina=1500,
|
|
TargetPos_beli=600, TargetPos_plavi=300,
|
|
opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150):
|
|
TargetPos_beli = TargetPos_beli * 4096 / 360
|
|
TargetPos_plavi = TargetPos_plavi * 4096 / 360
|
|
|
|
if smer == 1:
|
|
Speed_ID2 = brzina
|
|
Speed_ID4 = -brzina
|
|
Speed_ID3 = -brzina
|
|
Speed_ID5 = brzina
|
|
else:
|
|
Speed_ID2 = -brzina
|
|
Speed_ID4 = brzina
|
|
Speed_ID3 = brzina
|
|
Speed_ID5 = -brzina
|
|
|
|
self._open_port()
|
|
try:
|
|
motor2 = CrniMotor(self.packetHandler, 2)
|
|
motor3 = CrniMotor(self.packetHandler, 3)
|
|
motor4 = CrniMotor(self.packetHandler, 4)
|
|
motor5 = CrniMotor(self.packetHandler, 5)
|
|
|
|
brojac = 0
|
|
|
|
|
|
load_blue_count = 0
|
|
load_white_count = 0
|
|
|
|
while True:
|
|
curr_time = time.time()
|
|
|
|
if smer == 1:
|
|
motor3.doCycle(TargetPos_beli, Speed_ID3)
|
|
motor5.doCycle(TargetPos_beli, Speed_ID5)
|
|
if brojac >= 50:
|
|
motor2.doCycle(TargetPos_plavi, Speed_ID2)
|
|
motor4.doCycle(TargetPos_plavi, Speed_ID4)
|
|
else:
|
|
brojac += 1
|
|
else:
|
|
motor2.doCycle(TargetPos_plavi, Speed_ID2)
|
|
motor4.doCycle(TargetPos_plavi, Speed_ID4)
|
|
if brojac >= 50:
|
|
motor3.doCycle(TargetPos_beli, Speed_ID3)
|
|
motor5.doCycle(TargetPos_beli, Speed_ID5)
|
|
else:
|
|
brojac += 1
|
|
|
|
print("Time elapsed: %.4f seconds" % (time.time() - curr_time))
|
|
|
|
opterecenje2 = get_load(self.packetHandler, 2)
|
|
opterecenje4 = get_load(self.packetHandler, 4)
|
|
opterecenje3 = get_load(self.packetHandler, 3)
|
|
opterecenje5 = get_load(self.packetHandler, 5)
|
|
|
|
napon2 = get_current(self.packetHandler, 2)
|
|
napon4 = get_current(self.packetHandler, 4)
|
|
napon3 = get_current(self.packetHandler, 3)
|
|
napon5 = get_current(self.packetHandler, 5)
|
|
|
|
print("Current positions:")
|
|
print("[ID:002] PresPos:%d Load:%.1f Current:%.1f" % (motor2.prosla_pos, opterecenje2, napon2))
|
|
print("[ID:004] PresPos:%d Load:%.1f Current:%.1f" % (motor4.prosla_pos, opterecenje4, napon4))
|
|
print("[ID:003] PresPos:%d Load:%.1f Current:%.1f" % (motor3.prosla_pos, opterecenje3, napon3))
|
|
print("[ID:005] PresPos:%d Load:%.1f Current:%.1f" % (motor5.prosla_pos, opterecenje5, napon5))
|
|
|
|
if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi:
|
|
|
|
load_blue_count+=1
|
|
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)
|
|
motor2.targret_reached = True
|
|
motor4.targret_reached = True
|
|
else:
|
|
load_blue_count = max(0, load_white_count - 1)
|
|
|
|
if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli:
|
|
load_white_count+=1
|
|
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)
|
|
motor3.targret_reached = True
|
|
motor5.targret_reached = True
|
|
else:
|
|
load_white_count = max(0, load_white_count - 1)
|
|
|
|
print("PredjeniPut ID2: %d deg" % (motor2.predjeni_put * 360 / 4096))
|
|
print("PredjeniPut ID4: %d deg" % (motor4.predjeni_put * 360 / 4096))
|
|
print("PredjeniPut ID3: %d deg" % (motor3.predjeni_put * 360 / 4096))
|
|
print("PredjeniPut ID5: %d deg" % (motor5.predjeni_put * 360 / 4096))
|
|
|
|
targets_reached = (
|
|
motor2.targret_reached
|
|
and motor3.targret_reached
|
|
and motor4.targret_reached
|
|
and motor5.targret_reached
|
|
)
|
|
if targets_reached:
|
|
print("Svi motori su stigli na cilj!")
|
|
break
|
|
|
|
return smer, motor2.predjeni_put, motor3.predjeni_put, motor4.predjeni_put, motor5.predjeni_put
|
|
except KeyboardInterrupt:
|
|
print("\nCtrl+C detected, stopping all motors.")
|
|
self._stop_all()
|
|
finally:
|
|
self._close_port()
|