37 __author__ =
'Cody Jorgensen, Antons Rebguns' 38 __copyright__ =
'Copyright (c) 2010-2011 Cody Jorgensen, Antons Rebguns' 41 __maintainer__ =
'Antons Rebguns' 42 __email__ =
'anton@email.arizona.edu' 46 from optparse
import OptionParser
49 roslib.load_manifest(
'dynamixel_driver')
51 from dynamixel_driver
import dynamixel_io
55 ''' Takes a dictionary with all the motor values and does a formatted print. 57 if values[
'freespin']:
59 Motor %(id)d is connected: 61 Model ------------------- %(model)s 62 Current Speed ----------- %(speed)d 63 Current Temperature ----- %(temperature)d%(degree_symbol)sC 64 Current Voltage --------- %(voltage).1fv 65 Current Load ------------ %(load)d 66 Moving ------------------ %(moving)s 70 Motor %(id)d is connected: 72 Model ------------------- %(model)s 73 Min Angle --------------- %(min)d 74 Max Angle --------------- %(max)d 75 Current Position -------- %(position)d 76 Current Speed ----------- %(speed)d 77 Current Temperature ----- %(temperature)d%(degree_symbol)sC 78 Current Voltage --------- %(voltage).1fv 79 Current Load ------------ %(load)d 80 Moving ------------------ %(moving)s 83 if __name__ ==
'__main__':
84 usage_msg =
'Usage: %prog [options] IDs' 85 desc_msg =
'Prints the current status of specified Dynamixel servo motors.' 86 epi_msg =
'Example: %s --port=/dev/ttyUSB1 --baud=57600 1 2 3 4 5' % sys.argv[0]
88 parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg)
89 parser.add_option(
'-p',
'--port', metavar=
'PORT', default=
'/dev/ttyUSB0',
90 help=
'motors of specified controllers are connected to PORT [default: %default]')
91 parser.add_option(
'-b',
'--baud', metavar=
'BAUD', type=
"int", default=1000000,
92 help=
'connection to serial port will be established at BAUD bps [default: %default]')
94 (options, args) = parser.parse_args(sys.argv)
101 baudrate = options.baud
105 dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
106 except dynamixel_io.SerialOpenError, soe:
110 print 'Pinging motors:' 111 for motor_id
in motor_ids:
112 motor_id = int(motor_id)
113 print '%d ...' % motor_id,
114 p = dxl_io.ping(motor_id)
117 values = dxl_io.get_feedback(motor_id)
118 angles = dxl_io.get_angle_limits(motor_id)
119 model = dxl_io.get_model_number(motor_id)
120 firmware = dxl_io.get_firmware_version(motor_id)
121 values[
'model'] =
'%s (firmware version: %d)' % (DXL_MODEL_TO_PARAMS[model][
'name'], firmware)
122 values[
'degree_symbol'] =
u"\u00B0" 123 values[
'min'] = angles[
'min']
124 values[
'max'] = angles[
'max']
125 values[
'voltage'] = values[
'voltage']
126 values[
'moving'] = str(values[
'moving'])
128 if angles[
'max'] == 0
and angles[
'min'] == 0:
129 values[
'freespin'] =
True 131 values[
'freespin'] =
False 136 print 'ERROR: None of the specified motors responded. Make sure to specify the correct baudrate.'