def __init__(self, connection, name, servo_ids): self.name = name self.servos = [ Robotis_Servo(connection, servo_id) for servo_id in servo_ids ] for servo in self.servos: servo.ensure_byte_set( 22, 1) # Make sure 'Resolution divider' is set to 1 self.zero_positions = [0] * len(self.servos)
def find_servos(dyn): ''' Finds all servo IDs on the USB2Dynamixel ''' print 'Scanning for Servos.' servos = [] dyn.servo_dev.setTimeout( 0.03 ) # To make the scan faster for i in xrange(21): try: s = Robotis_Servo( dyn, i ) if i>0 and i<=18: tipo = "body" if i==19 or i==20: tipo = "head" # print '\n FOUND A SERVO @ ID %d\n' % i servos.append( i ) except: pass dyn.servo_dev.setTimeout( 1.0 ) # Restore to original return servos, tipo
def __init__(self): ''' lidar pivot controller constructor ''' # Creates the ROS node. rospy.init_node("lidar_pivot_controller") # Load relevant parameters from ROS parameter server dyn_port = rospy.get_param("ports/dynamixel", "/dev/ttyUSB1") dyn_baud = rospy.get_param("baudrates/dynamixel_baud", 1000000) # Load LIDAR Pivot stuff self.PIVOT_LAYDOWN = rospy.get_param("dynamixel_settings/laying_angle", 20) self.PIVOT_STAND = rospy.get_param("dyanmixel_settings/standing_angle", 0) self.PIVOT_MAX = rospy.get_param("dyanimxel_settings/top_limit", 25) self.PIVOT_MIN = rospy.get_param("dynamixel_settings/bottom_limit", -1) self.target_angle = self.PIVOT_LAYDOWN # Target angle in radians self.move_request = False dyn_id = rospy.get_param("dynamixel_settings/id", DYNAMIXEL_ID) CMDS_TOPIC = rospy.get_param("topics/lidar_pivot_cmds", "lidar_pivot_control") TARGET_ANGLE_TOPIC = rospy.get_param( "topics/lidar_pivot_target_angles", "lidar_pivot_target_angles") POS_SERV = rospy.get_param("services/lidar_pivot_position", "get_lidar_pivot_position") # Attempting to connect to dynamixel self.dyn = None self.servo = None print("Attempting to connect to dynamixel on port: " + str(dyn_port)) while not rospy.is_shutdown(): good_conn = False # Set if successful connection to dynamixel try: self.dyn = USB2Dynamixel_Device(dev_name=dyn_port, baudrate=dyn_baud) except: print("Failed to connect to dynamixel. Will continue trying.") else: good_conn = True if not good_conn: # above connection failed, try again. rospy.sleep(3) continue # Above connection succeeded, try to create servo object try: self.servo = Robotis_Servo(self.dyn, dyn_id) except: print("Failed to create servo object. Will continue trying.") rospy.sleep(3) else: # everything was successful break # Servo Motor Setup self.dyn = USB2Dynamixel_Device(dev_name=dyn_port, baudrate=dyn_baud) self.servo = Robotis_Servo(self.dyn, dyn_id) # Inits the LIDAR pivot controller Subscriber rospy.Subscriber(TARGET_ANGLE_TOPIC, Float32, self.angle_callback) rospy.Subscriber(CMDS_TOPIC, String, self.cmds_callback) # Initialize service that gets the current angle of the lidar self.get_angle_service = rospy.Service( POS_SERV, LidarPivotAngle, self.handle_get_lidar_pivot_position) # Send servo to default position self.servo.move_angle(self.target_angle) self.current_angle = self.servo.read_angle()