def get_pololu_power(self, port, channel): try: m = Maestro(port); pos = m.getPosition(channel) m.close() if pos > 100: return 0 return 1 except: return 1
def publish_motor_states(self): if self.idle: return if self._sync == 'on': for i, m in self._motors.items(): try: if self._dynamic_speed == "on": # Get speed required and normalize it speed = Maestro.calculateSpeed( m.last_pulse, m.pulse, 1.0 / COMMAND_RATE, 1.0 / self._servo_rate) / 512.0 m.last_pulse = m.pulse else: speed = m.speed self.set_speed(m.id, speed) self.set_pulse(m.id, m.pulse) self.set_acceleration(m.id, 0) except Exception as ex: logger.error("Error %s" % ex) time.sleep(0.01) self.controller.clean() self.controller.clean() # Publish the states msg = JointState() for i, m in self._motors.items(): msg.name.append(m.name) msg.position.append(m.get_angle()) self.states_pub.publish(msg)
def publish_motor_states(self): if self.idle: return if self._sync == 'on': for i, m in self._motors.items(): try: if self._dynamic_speed == "on": # Get speed required and normalize it speed = Maestro.calculateSpeed(m.last_pulse, m.pulse, 1.0/COMMAND_RATE, 1.0/self._servo_rate) / 512.0 m.last_pulse = m.pulse else: speed = m.speed self.set_speed(m.id, speed) self.set_pulse(m.id, m.pulse) self.set_acceleration(m.id, 0) except Exception as ex: logger.error("Error %s" % ex) time.sleep(0.01) self.controller.clean() self.controller.clean() # Publish the states msg = JointState() for i, m in self._motors.items(): msg.name.append(m.name) msg.position.append(m.get_angle()) self.states_pub.publish(msg)
def __init__(self): port = rospy.get_param("~port_name") topic_prefix = rospy.get_param("~topic_prefix", "pololu/") topic_name = rospy.get_param("~topic_name", "command") # Use safety proxy to filter motor comamnds safety = rospy.get_param("~safety", False) # Use specific rate to publish motors commands self._origin_sync = self._sync = rospy.get_param("~sync", "off") self._dynamic_speed = rospy.get_param("~dyn_speed", "off") self._servo_rate = rospy.get_param("~servo_rate", 50) self._controller_type = rospy.get_param("~controller", "Maestro") self._hw_fail = rospy.get_param("~hw_required", False) self._motors = {} self._inputs = {} self.idle = False if rospy.has_param("~pololu_motors_yaml"): config_yaml = rospy.get_param("~pololu_motors_yaml") try: yaml_stream = open(config_yaml) config = yaml.load(yaml_stream) except: logger.warn("Error loading config files") # Get existing motors config and update those configs if callibration data changed for name, cfg in config.items(): if 'input_name' in cfg.keys(): self._inputs[name] = cfg self._inputs[name]['topic'] = rospy.Publisher( 'inputs/{}'.format(name), Int32, queue_size=1) else: self._motors[name] = PololuMotor(name, cfg) rospy.logerr(self._inputs) try: if self._controller_type == 'Maestro': self.controller = Maestro(port, writeTimeout=1.0) if self._controller_type == 'MicroSSC': self.controller = MicroSSC(port) except Exception as ex: if self._hw_fail: rospy.logfatal("Hardware needs to be attached") rospy.signal_shutdown("HW Failure") return logger.warn("Error creating the motor controller") logger.warn(ex) self.idle = True rospy.set_param(topic_prefix.strip("/") + "_enabled", False) return rospy.set_param(topic_prefix.strip("/") + "_enabled", True) # Listen for outputs from proxy self.states_pub = rospy.Publisher(topic_prefix + "motors_states", JointState, queue_size=10) if safety: topic_prefix = 'safe/' + topic_prefix rospy.Subscriber(topic_prefix + topic_name, MotorCommand, self.motor_command_callback) logger.info("ros_pololu Subscribed to %s" % (topic_prefix + topic_name)) # Topic to enable disable continou rospy.Subscriber('pololu_sync', String, self.sync_callback)
def __init__(self): port = rospy.get_param("~port_name") topic_prefix = rospy.get_param("~topic_prefix", "pololu/") topic_name = rospy.get_param("~topic_name", "command") # Use safety proxy to filter motor comamnds safety = rospy.get_param("~safety", False) # Use specific rate to publish motors commands self._sync = rospy.get_param("~sync", "off") self._dynamic_speed = rospy.get_param("~dyn_speed", "off") self._servo_rate = rospy.get_param("~servo_rate", 50) self._controller_type = rospy.get_param("~controller", "Maestro") self._motors = {} self.idle = False if rospy.has_param("~pololu_motors_yaml"): config_yaml = rospy.get_param("~pololu_motors_yaml") try: yaml_stream = open(config_yaml) config = yaml.load(yaml_stream) except: logger.warn("Error loading config files") # Get existing motors config and update those configs if callibration data changed motors = rospy.get_param('motors', []) for name, cfg in config.items(): self._motors[name] = PololuMotor(name, cfg) # cfg = self._motors[name].get_calibrated_config() # cfg['topic'] = topic_prefix.strip("/") # cfg['hardware'] = 'pololu' # for i, m in enumerate(motors): # if m['name'] == name: # motors[i] = cfg # break # else: # motors.append(cfg) # rospy.set_param('motors', motors) try: if self._controller_type == 'Maestro': self.controller = Maestro(port) if self._controller_type == 'MicroSSC': self.controller = MicroSSC(port) except Exception as ex: logger.warn("Error creating the motor controller") logger.warn(ex) self.idle = True rospy.set_param(topic_prefix.strip("/") + "_enabled", False) return rospy.set_param(topic_prefix.strip("/") + "_enabled", True) # Listen for outputs from proxy self.states_pub = rospy.Publisher(topic_prefix + "motors_states", JointState, queue_size=10) if safety: topic_prefix = 'safe/' + topic_prefix rospy.Subscriber(topic_prefix + topic_name, MotorCommand, self.motor_command_callback) logger.info("ros_pololu Subscribed to %s" % (topic_prefix + topic_name))
def test_port_io(self): controller = Maestro(self.port0) reader = PololuSerialReader(self.port1) for i in range(10): pulses = [6143, 6122, 5899, 5085, 5352, 5810, 6075, 6143] * 100 for pulse in pulses: controller.setAcceleration(12, 255) controller.setSpeed(12, 256) controller.setTarget(12, pulse) pulses_got = [] for i in range(3 * len(pulses)): id, cmd, value = reader.read() if cmd == 'speed': self.assertEqual(value, 256) if cmd == 'accelaration': self.assertEqual(value, 255) if cmd == 'position': pulses_got.append(value) self.assertListEqual(pulses, pulses_got)
def test_port_io(self): controller = Maestro(self.port0) reader = PololuSerialReader(self.port1) for i in range(10): pulses = [6143, 6122, 5899, 5085, 5352, 5810, 6075, 6143]*100 for pulse in pulses: controller.setAcceleration(12, 255) controller.setSpeed(12, 256) controller.setTarget(12, pulse) pulses_got = [] for i in range(3*len(pulses)): id, cmd, value = reader.read() if cmd == 'speed': self.assertEqual(value, 256) if cmd == 'accelaration': self.assertEqual(value, 255) if cmd == 'position': pulses_got.append(value) self.assertListEqual(pulses, pulses_got)
import time from pololu.motors import Maestro def set_position(motors, pos_series): active_motors = pos_series.index.tolist() for motor in motors: if motor['name'] in active_motors: pos = pos_series[motor['name']] if not np.isnan(pos): motor['pos'] = pos print "set {} to {}".format(motor['name'], pos) if __name__ == '__main__': signal_file = 'signal.csv' signal_df = pd.read_csv(signal_file) motors = get_motors('head.yaml') init_position(motors) DEVICE = '/dev/ttyACM0' controller = None if os.path.exists(DEVICE): controller = Maestro(DEVICE) for index, row in signal_df.iterrows(): set_position(motors, row) if controller: set_servos(controller, motors) time.sleep(0.5)