def motor_states(self, motors, robot_name):
# Dynamixel monitor start
if not self.monitor_started:
for i, m in enumerate(motors):
if m['hardware'] == 'pololu':
self.pololu_boards[m['topic']] = {}
self.start_motors_monitor(robot_name)
self.monitor_started = True
# Sleep some time so first results if motors are alive will have time to return
time.sleep(0.5)
status = {}
pololu_boards = {}
for i, m in enumerate(motors):
if m['hardware'] == 'pololu':
if not m['topic'] in self.pololu_boards.keys():
self.pololu_boards[m['topic']] = {}
if m['name'] in self.pololu_boards[m['topic']].keys():
m['motor_state'] = {
'position': self.pololu_boards[m['topic']][m['name']]
}
m['error'] = 2
else:
m['error'] = 1
motor = PololuMotor(m['name'], m)
#Conert to angles
m['init'] = round(math.degrees(motor.get_angle(m['init'] * 4)))
m['min'] = round(math.degrees(motor.get_angle(m['min'] * 4)))
m['max'] = round(math.degrees(motor.get_angle(m['max'] * 4)))
#Dynamixel motors
else:
if m['motor_id'] in self.dynamixel_motors_states.keys():
motors[i]['error'] = 0
motors[i]['motor_state'] = self.dynamixel_motors_states[
m['motor_id']]
else:
# Motor is not on
motors[i]['error'] = 1
m['max'] = round(
math.degrees(Configs.dynamixel_angle(m, m['max'])))
m['min'] = round(
math.degrees(Configs.dynamixel_angle(m, m['min'])))
# Init has to be replaced last, because calculation depends on it
m['init'] = round(
math.degrees(Configs.dynamixel_angle(m, m['init'])))
return motors
def motor_states(self, motors, robot_name):
# Dynamixel monitor start
if not self.monitor_started:
for i,m in enumerate(motors):
if m['hardware'] == 'pololu':
self.pololu_boards[m['topic']] = {}
self.start_motors_monitor(robot_name)
self.monitor_started = True
# Sleep some time so first results if motors are alive will have time to return
time.sleep(0.5)
status = {}
pololu_boards = {}
for i, m in enumerate(motors):
if m['hardware'] == 'pololu':
if not m['topic'] in self.pololu_boards.keys():
self.pololu_boards[m['topic']] = {}
if m['name'] in self.pololu_boards[m['topic']].keys():
m['motor_state'] = {'position': self.pololu_boards[m['topic']][m['name']]}
m['error'] = 2
else:
m['error'] = 1
motor = PololuMotor(m['name'], m)
#Conert to angles
m['init'] = round(math.degrees(motor.get_angle(m['init']*4)))
m['min'] = round(math.degrees(motor.get_angle(m['min']*4)))
m['max'] = round(math.degrees(motor.get_angle(m['max']*4)))
#Dynamixel motors
else:
if m['motor_id'] in self.dynamixel_motors_states.keys():
motors[i]['error'] = 0
motors[i]['motor_state'] = self.dynamixel_motors_states[m['motor_id']]
else:
# Motor is not on
motors[i]['error'] = 1
m['max'] = round(math.degrees(Configs.dynamixel_angle(m, m['max'])))
m['min'] = round(math.degrees(Configs.dynamixel_angle(m, m['min'])))
# Init has to be replaced last, because calculation depends on it
m['init'] = round(math.degrees(Configs.dynamixel_angle(m, m['init'])))
return motors
