dynamixel_hardware_interface: set_servo

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, 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__ = 'Antons Rebguns'
00038 __copyright__ = 'Copyright (c) 2010-2011 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_hardware_interface')
00050 
00051 import dynamixel_io
00052 
00053 if __name__ == '__main__':
00054     usage_msg = 'Usage: %prog [options] MOTOR_IDs'
00055     desc_msg = 'Sets various configuration options of specified Dynamixel servo motor.'
00056     epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 --baud-rate=1 --return-delay=1 5 9 23' % sys.argv[0]
00057     
00058     parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg)
00059     parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0',
00060                       help='motors of specified controllers are connected to PORT [default: %default]')
00061     parser.add_option('-b', '--baud', metavar='BAUD', type='int', default=1000000,
00062                       help='connection to serial port will be established at BAUD bps [default: %default]')
00063     parser.add_option('-r', '--baud-rate', type='int', metavar='RATE', dest='baud_rate',
00064                       help='set servo motor communication speed')
00065     parser.add_option('-d', '--return-delay', type='int', metavar='DELAY', dest='return_delay',
00066                       help='set servo motor return packet delay time')
00067     parser.add_option('--cw-angle-limit', type='int', metavar='CW_ANGLE', dest='cw_angle_limit',
00068                       help='set servo motor CW angle limit')
00069     parser.add_option('--ccw-angle-limit', type='int', metavar='CCW_ANGLE', dest='ccw_angle_limit',
00070                       help='set servo motor CCW angle limit')
00071     parser.add_option('--min-voltage-limit', type='int', metavar='MIN_VOLTAGE', dest='min_voltage_limit',
00072                       help='set servo motor minimum voltage limit')
00073     parser.add_option('--max-voltage-limit', type='int', metavar='MAX_VOLTAGE', dest='max_voltage_limit',
00074                       help='set servo motor maximum voltage limit')
00075     parser.add_option('--cw-compliance-margin', type='int', metavar='CW_MARGIN', dest='cw_compliance_margin',
00076                       help='set servo motor CW compliance margin')
00077     parser.add_option('--ccw-compliance-margin', type='int', metavar='CCW_MARGIN', dest='ccw_compliance_margin',
00078                       help='set servo motor CCW compliance margin')
00079     parser.add_option('--cw-compliance-slope', type='int', metavar='CW_SLOPE', dest='cw_compliance_slope',
00080                       help='set servo motor CW compliance slope')
00081     parser.add_option('--ccw-compliance-slope', type='int', metavar='CCW_SLOPE', dest='ccw_compliance_slope',
00082                       help='set servo motor CCW compliance slope')
00083                       
00084     (options, args) = parser.parse_args(sys.argv)
00085     #print options
00086     
00087     if len(args) < 2:
00088         parser.print_help()
00089         exit(1)
00090         
00091     port = options.port
00092     baudrate = options.baud
00093     motor_ids = args[1:]
00094     
00095     try:
00096         dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
00097     except RuntimeError, soe:
00098         print 'ERROR:', soe
00099     else:
00100         for motor_id in motor_ids:
00101             motor_id = int(motor_id)
00102             print 'Configuring Dynamixel motor with ID %d' % motor_id
00103             
00104             if dxl_io.ping(motor_id):
00105                 # check if baud rate needs to be changed
00106                 if options.baud_rate:
00107                     valid_rates = (1,3,4,7,9,16,34,103,207,250,251,252)
00108                     
00109                     if options.baud_rate not in valid_rates:
00110                         print 'Requested baud rate is invalid, please use one of the following: %s' % str(valid_rates)
00111                         
00112                     if options.baud_rate <= 207:
00113                         print 'Setting baud rate to %d bps' % int(2000000.0/(options.baud_rate + 1))
00114                     elif options.baud_rate == 250:
00115                         print 'Setting baud rate to %d bps' % 2250000
00116                     elif options.baud_rate == 251:
00117                         print 'Setting baud rate to %d bps' % 2500000
00118                     elif options.baud_rate == 252:
00119                         print 'Setting baud rate to %d bps' % 3000000
00120                         
00121                     dxl_io.set_baud_rate(motor_id, options.baud_rate)
00122                     
00123                 # check if return delay time needs to be changed
00124                 if options.return_delay is not None:
00125                     if options.return_delay < 0 or options.return_delay > 254:
00126                         print 'Requested return delay time is out of valie range (0 - 254)'
00127                         
00128                     print 'Setting return delay time to %d us' % (options.return_delay * 2)
00129                     dxl_io.set_return_delay_time(motor_id, options.return_delay)
00130                     
00131                 # check if CW angle limit needs to be changed
00132                 if options.cw_angle_limit is not None:
00133                     print 'Setting CW angle limit to %d' % options.cw_angle_limit
00134                     dxl_io.set_cw_angle_limit(motor_id, options.cw_angle_limit)
00135                     
00136                 # check if CCW angle limit needs to be changed
00137                 if options.ccw_angle_limit is not None:
00138                     print 'Setting CCW angle limit to %d' % options.ccw_angle_limit
00139                     dxl_io.set_ccw_angle_limit(motor_id, options.ccw_angle_limit)
00140                     
00141                 # check if minimum voltage limit needs to be changed
00142                 if options.min_voltage_limit:
00143                     print 'Setting minimum voltage limit to %d' % options.min_voltage_limit
00144                     dxl_io.set_min_voltage_limit(motor_id, options.min_voltage_limit)
00145                     
00146                 # check if maximum voltage limit needs to be changed
00147                 if options.max_voltage_limit:
00148                     print 'Setting maximum voltage limit to %d' % options.max_voltage_limit
00149                     dxl_io.set_max_voltage_limit(motor_id, options.max_voltage_limit)
00150                     
00151                 # check if CW compliance margin needs to be changed
00152                 if options.cw_compliance_margin is not None:
00153                     print 'Setting CW compliance margin to %d' % options.cw_compliance_margin
00154                     dxl_io.set_cw_compliance_margin(motor_id, options.cw_compliance_margin)
00155                     
00156                 # check if CCW compliance margin needs to be changed
00157                 if options.ccw_compliance_margin is not None:
00158                     print 'Setting CCW compliance margin to %d' % options.ccw_compliance_margin
00159                     dxl_io.set_ccw_compliance_margin(motor_id, options.ccw_compliance_margin)
00160                     
00161                 # check if CW compliance slope needs to be changed
00162                 if options.cw_compliance_slope is not None:
00163                     print 'Setting CW compliance slope to %d' % options.cw_compliance_slope
00164                     dxl_io.set_cw_compliance_slope(motor_id, options.cw_compliance_slope)
00165                     
00166                 # check if CCW compliance slope needs to be changed
00167                 if options.ccw_compliance_slope is not None:
00168                     print 'Setting CCW compliance slope to %d' % options.ccw_compliance_slope
00169                     dxl_io.set_ccw_compliance_slope(motor_id, options.ccw_compliance_slope)
00170                     
00171                 print 'done'
00172             else:
00173                 print 'Unable to connect to Dynamixel motor with ID %d on port %s at %d bps' % (motor_id, port, baudrate)