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
