info_dump.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2010-2011, Cody Jorgensen, Antons Rebguns.
00007 # All rights reserved.
00008 #
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions
00011 # are met:
00012 #
00013 #  * Redistributions of source code must retain the above copyright
00014 #    notice, this list of conditions and the following disclaimer.
00015 #  * Redistributions in binary form must reproduce the above
00016 #    copyright notice, this list of conditions and the following
00017 #    disclaimer in the documentation and/or other materials provided
00018 #    with the distribution.
00019 #  * Neither the name of University of Arizona nor the names of its
00020 #    contributors may be used to endorse or promote products derived
00021 #    from this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 # POSSIBILITY OF SUCH DAMAGE.
00035 
00036 
00037 __author__ = 'Cody Jorgensen, Antons Rebguns'
00038 __copyright__ = 'Copyright (c) 2010-2011 Cody Jorgensen, Antons Rebguns'
00039 
00040 __license__ = 'BSD'
00041 __maintainer__ = 'Antons Rebguns'
00042 __email__ = 'anton@email.arizona.edu'
00043 
00044 
00045 import sys
00046 from optparse import OptionParser
00047 
00048 import roslib
00049 roslib.load_manifest('dynamixel_driver')
00050 
00051 from dynamixel_driver import dynamixel_io
00052 from dynamixel_driver.dynamixel_const import *
00053 
00054 def print_data(values):
00055     ''' Takes a dictionary with all the motor values and does a formatted print.
00056     '''
00057     if values['freespin']:
00058         print '''\
00059     Motor %(id)d is connected:
00060         Freespin: True
00061         Model ------------------- %(model)s
00062         Current Speed ----------- %(speed)d
00063         Current Temperature ----- %(temperature)d%(degree_symbol)sC
00064         Current Voltage --------- %(voltage).1fv
00065         Current Load ------------ %(load)d
00066         Moving ------------------ %(moving)s
00067 ''' %values
00068     else:
00069         print '''\
00070     Motor %(id)d is connected:
00071         Freespin: False
00072         Model ------------------- %(model)s
00073         Min Angle --------------- %(min)d
00074         Max Angle --------------- %(max)d
00075         Current Position -------- %(position)d
00076         Current Speed ----------- %(speed)d
00077         Current Temperature ----- %(temperature)d%(degree_symbol)sC
00078         Current Voltage --------- %(voltage).1fv
00079         Current Load ------------ %(load)d
00080         Moving ------------------ %(moving)s
00081 ''' %values
00082 
00083 if __name__ == '__main__':
00084     usage_msg = 'Usage: %prog [options] IDs'
00085     desc_msg = 'Prints the current status of specified Dynamixel servo motors.'
00086     epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 1 2 3 4 5' % sys.argv[0]
00087     
00088     parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg)
00089     parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0',
00090                       help='motors of specified controllers are connected to PORT [default: %default]')
00091     parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000,
00092                       help='connection to serial port will be established at BAUD bps [default: %default]')
00093                       
00094     (options, args) = parser.parse_args(sys.argv)
00095     
00096     if len(args) < 2:
00097         parser.print_help()
00098         exit(1)
00099         
00100     port = options.port
00101     baudrate = options.baud
00102     motor_ids = args[1:]
00103     
00104     try:
00105         dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
00106     except dynamixel_io.SerialOpenError, soe:
00107         print 'ERROR:', soe
00108     else:
00109         responses = 0
00110         print 'Pinging motors:'
00111         for motor_id in motor_ids:
00112             motor_id = int(motor_id)
00113             print '%d ...' % motor_id,
00114             p = dxl_io.ping(motor_id)
00115             if p:
00116                 responses += 1
00117                 values = dxl_io.get_feedback(motor_id)
00118                 angles = dxl_io.get_angle_limits(motor_id)
00119                 model = dxl_io.get_model_number(motor_id)
00120                 firmware = dxl_io.get_firmware_version(motor_id)
00121                 values['model'] = '%s (firmware version: %d)' % (DXL_MODEL_TO_PARAMS[model]['name'], firmware)
00122                 values['degree_symbol'] = u"\u00B0"
00123                 values['min'] = angles['min']
00124                 values['max'] = angles['max']
00125                 values['voltage'] = values['voltage']
00126                 values['moving'] = str(values['moving'])
00127                 print 'done'
00128                 if angles['max'] == 0 and angles['min'] == 0:
00129                     values['freespin'] = True
00130                 else:
00131                     values['freespin'] = False
00132                 print_data(values)
00133             else:
00134                 print 'error'
00135         if responses == 0:
00136             print 'ERROR: None of the specified motors responded. Make sure to specify the correct baudrate.'
00137 


dynamixel_driver
Author(s): Antons Rebguns, Cody Jorgensen
autogenerated on Fri Jan 3 2014 11:19:42