def __init__(self, port_name, baud): self.mae = MaestroSerial(port_name, baud)
class Legs: def __init__(self, port_name, baud): self.mae = MaestroSerial(port_name, baud) def get_status(self, status): is_moving = self.mae.get_moving() if is_moving: status.moving_bitmask = 0xFFFFFFFF else: status.moving_bitmask = 0x00000000 status.RLA = int(self.mae.get_position(0)) status.RLB = int(self.mae.get_position(1)) status.RLC = int(self.mae.get_position(2)) status.MLA = int(self.mae.get_position(3)) status.MLB = int(self.mae.get_position(4)) status.MLC = int(self.mae.get_position(5)) status.FLA = int(self.mae.get_position(6)) status.FLB = int(self.mae.get_position(7)) status.FLC = int(self.mae.get_position(8)) status.FRA = int(self.mae.get_position(9)) status.FRB = int(self.mae.get_position(10)) status.FRC = int(self.mae.get_position(11)) status.MRA = int(self.mae.get_position(12)) status.MRB = int(self.mae.get_position(13)) status.MRC = int(self.mae.get_position(14)) status.RRA = int(self.mae.get_position(15)) status.RRB = int(self.mae.get_position(16)) status.RRC = int(self.mae.get_position(17)) return status def set_all(self,speed,ls): for i in range(23): self.mae.set_speed(i,speed) self.mae.set_multiple(0,( ls.RLA, ls.RLB, ls.RLC, ls.MLA, ls.MLB, ls.MLC, ls.FLA, ls.FLB, ls.FLC, ls.FRA, ls.FRB, ls.FRC, ls.MRA, ls.MRB, ls.MRC, ls.RRA, ls.RRB, ls.RRC )) def set_all_sync(self,speed,ls, status): ms = self.get_status(status) target = [] dist = [] target.append(ls.RLA); dist.append(abs(ls.RLA-ms.RLA)); target.append(ls.RLB); dist.append(abs(ls.RLB-ms.RLB)); target.append(ls.RLC); dist.append(abs(ls.RLC-ms.RLC)); target.append(ls.MLA); dist.append(abs(ls.MLA-ms.MLA)); target.append(ls.MLB); dist.append(abs(ls.MLB-ms.MLB)); target.append(ls.MLC); dist.append(abs(ls.MLC-ms.MLC)); target.append(ls.FLA); dist.append(abs(ls.FLA-ms.FLA)); target.append(ls.FLB); dist.append(abs(ls.FLB-ms.FLB)); target.append(ls.FLC); dist.append(abs(ls.FLC-ms.FLC)); target.append(ls.FRA); dist.append(abs(ls.FRA-ms.FRA)); target.append(ls.FRB); dist.append(abs(ls.FRB-ms.FRB)); target.append(ls.FRC); dist.append(abs(ls.FRC-ms.FRC)); target.append(ls.MRA); dist.append(abs(ls.MRA-ms.MRA)); target.append(ls.MRB); dist.append(abs(ls.MRB-ms.MRB)); target.append(ls.MRC); dist.append(abs(ls.MRC-ms.MRC)); target.append(ls.RRA); dist.append(abs(ls.RRA-ms.RRA)); target.append(ls.RRB); dist.append(abs(ls.RRB-ms.RRB)); target.append(ls.RRC); dist.append(abs(ls.RRC-ms.RRC)); max_dist = 0 for d in dist: if d > max_dist: max_dist = d for i in range(18): if max_dist != 0: sp = int(dist[i]/max_dist * speed) else: sp = 0 if sp != 0 and sp < 25: sp = 25 self.mae.set_speed(i,sp) self.mae.set_multiple(0,( ls.RLA, ls.RLB, ls.RLC, ls.MLA, ls.MLB, ls.MLC, ls.FLA, ls.FLB, ls.FLC, ls.FRA, ls.FRB, ls.FRC, ls.MRA, ls.MRB, ls.MRC, ls.RRA, ls.RRB, ls.RRC )) return ms