''' ''' import serial import array import time import threading import sys import logging logger = logging.getLogger(__name__) class rob3IF: BAUDRATE = 2400 SLEEP_READ = .1 RL_INIT = [ 0x15, 0xf1, 0xf2, 0xf3 ] # Note: 0x15 bekapcs utan, 0xf1 kesobb B_TERM = 0x03 B_INIT = 0x20 B_MOTOR_REGULATION_ON = 0x61 B_MOTOR_REGULATION_OFF = 0x60 # ezt inkabb ne hasznald, mert van motor, ami nem all le a cel eleresekor, ha a tobbi meg a celja fele tart A_BODY = 0b000 A_SHOULDER = 0b001 A_ELBOW = 0b010 A_WRIST = 0b011 A_WRIST_ROT = 0b100 A_GRIP = 0b101 A_ALL = 0b111 F_ECHO = 0b1000 F_GETPOS = 0b01000000 #TODO: slow motor def __init__(self, device = '/dev/ttyS0'): self.ser = serial.Serial(port = device, baudrate = self.BAUDRATE, timeout = 1) logger.debug(self.ser) self.e_resp = threading.Event() self.e_stopped = threading.Event() self.e_coords_updated = threading.Event() self.t_read = threading.Thread(target = self._read) self.t_read.start() self._write([ self.B_INIT ]) self.e_resp.wait() self._coords = [ 0 ] * 6 self.t_coords_update = threading.Thread(target = self._get_coords) self.t_coords_update.start() logger.info("initialized") def _write(self, a): assert isinstance(a, list) r = array.array('B', a).tobytes() logger.debug(b'<- ' + r) self.ser.write(r) self.ser.flush() def _read(self): logger.info('read robot messages thread started') A_LIST = [ self.A_BODY, self.A_SHOULDER, self.A_ELBOW, self.A_WRIST, self.A_WRIST_ROT, self.A_GRIP ] A_LIST_POS = [ ax | self.F_GETPOS for ax in A_LIST ] A_LIST_ECHO = [ ax | self.F_ECHO for ax in A_LIST ] junk = b'' while not self.e_stopped.is_set(): b = self.ser.read(self.ser.in_waiting) if len(b) == 0: time.sleep(self.SLEEP_READ) continue logger.debug(b'-> ' + b) if self.B_TERM in b: leftoff = self._process_input(b) if leftoff == b and junk: b = junk + b logger.debug(b"retry digesting with former leftoff: " + b) leftoff = self._process_input(b) llo = len(leftoff) while (leftoff != b) and llo: b = leftoff logger.debug(b"go on with the rest: " + b) leftoff = self._process_input(b) llo = len(leftoff) junk = leftoff logger.debug("len leftoff {}".format(llo)) else: junk += b if junk: logger.warning(b'JUNK: ' + junk) logger.info('read robot messages thread stopped') def _process_input(self, b): if b[7] == self.B_TERM and b[0] == self.A_ALL | self.F_GETPOS: self._coords[:] = b[1:] self.e_coords_updated.set() return b[8:] if b[2] == self.B_TERM and b[0] in self.A_LIST_POS: self._coords[b[0] & 0b0111 ] = b[1] self.e_coords_updated.set() return b[3:] if b[1] == self.B_TERM and b[0] in self.RL_INIT: logger.debug("robot echoed") self.e_resp.set() return b[2:] if b[1] == self.B_TERM and b[0] == self.A_ALL | self.F_ECHO: self.e_resp.set() return b[2:] if b[1] == self.B_TERM and b[0] in self.A_LIST_ECHO: self.e_resp.set() return b[2:] logger.debug(b"unable to process: " + b) return b def _get_coords(self): logger.info('request for position thread started') while not self.e_stopped.is_set(): self.e_coords_updated.clear() self._write([ self.A_ALL | self.F_GETPOS, self.B_TERM ]) self.e_coords_updated.wait() logger.info('requsest for positions thread stopped') def _get_coord_ax(self, ax): assert ax in self.A_LIST self.e_coords_updated.clear() self._write([ ax | self.F_GETPOS, self.B_TERM ]) @property def coords(self): self.e_coords_updated.wait() return None if self.e_stopped.is_set() else self._coords def ptp(self, c, echo = False): assert max(c) <= 255 and min(c) >= 0 lc = len(c) if echo: a = self.A_ALL | self.F_ECHO self.e_resp.clear() else: a = self.A_ALL if lc == 5: cmd = list((a, *c, self._grip, self.B_TERM)) elif lc == 6: cmd = list((a, *c, self.B_TERM)) self._grip = c[-1] else: raise AssertionError("len of coords is not okay") self._write(cmd) def rot(self, ax, c, echo = False): assert ax in self.A_LIST assert c >= 0 and c <= 255 if echo: ax |= self.F_ECHO self.e_resp.clear() self._write([ ax, c, self.B_TERM ]) def grip(self, g = None): assert g is None or (g <= 255 and g >= 0) if g is None: gn = 0 if self._coords[-1] else 255 else: gn = g self.rot(self.A_GRIP, gn) def regulation_on(self): self._write([ self.B_MOTOR_REGULATION_ON, self.B_TERM ]) def regulation_off(self): self._write([ self.B_MOTOR_REGULATION_OFF, self.B_TERM ]) def stop(self): self.e_stopped.set() self.e_coords_updated.set() self.t_read.join() self.t_coords_update.join() self.ser.close() logger.info("stopped.") def log_coords(rIF): while not rIF.e_stopped.is_set(): print ("\t", rIF.coords) if __name__ == '__main__': r = rob3IF() tcl = threading.Thread(target = log_coords, args = (r, )) tcl.start() r.ptp([0x80] * 6) time.sleep(4) r.ptp([0x0] * 6) time.sleep(4) # r.grip() # time.sleep(2) # r.grip() # time.sleep(2) # r.grip(0x8f) # time.sleep(2) # r.rot(r.A_BODY, 0xff) # time.sleep(4) # r.rot(r.A_BODY, 0x00) # time.sleep(4) # r.rot(r.A_SHOULDER, 0xff) # time.sleep(4) # r.rot(r.A_SHOULDER, 0x00) # time.sleep(4) # r.rot(r.A_ELBOW, 0xff) # time.sleep(4) # r.rot(r.A_ELBOW, 0x00) # time.sleep(4) # r.rot(r.A_WRIST, 0xff) # time.sleep(4) # r.rot(r.A_WRIST, 0x00) # time.sleep(4) # r.rot(r.A_WRIST_ROT, 0xff) # time.sleep(4) # r.rot(r.A_WRIST_ROT, 0x00) # time.sleep(4) # r.rot(r.A_GRIP, 0xff) # time.sleep(4) # r.rot(r.A_GRIP, 0x00) # time.sleep(10) r.stop() tcl.join()