00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import serial
00035 import time
00036 import thread
00037 import sys, optparse
00038 import math
00039
00040 class USB2Dynamixel_Device():
00041 ''' Class that manages serial port contention between servos on same bus
00042 '''
00043 def __init__( self, dev_name = '/dev/ttyUSB0', baudrate = 57600 ):
00044 try:
00045 self.dev_name = string.atoi( dev_name )
00046 except:
00047 self.dev_name = dev_name
00048
00049 self.mutex = thread.allocate_lock()
00050 self.servo_dev = None
00051
00052 self.acq_mutex()
00053 self._open_serial( baudrate )
00054 self.rel_mutex()
00055
00056 def acq_mutex(self):
00057 self.mutex.acquire()
00058
00059 def rel_mutex(self):
00060 self.mutex.release()
00061
00062 def send_serial(self, msg):
00063
00064 self.servo_dev.write( msg )
00065
00066 def read_serial(self, nBytes=1):
00067
00068 rep = self.servo_dev.read( nBytes )
00069 return rep
00070
00071 def _open_serial(self, baudrate):
00072 try:
00073 self.servo_dev = serial.Serial(self.dev_name, baudrate, timeout=1.0)
00074
00075
00076 self.servo_dev.close()
00077 self.servo_dev.setParity('N')
00078 self.servo_dev.setStopbits(1)
00079 self.servo_dev.open()
00080
00081 self.servo_dev.flushOutput()
00082 self.servo_dev.flushInput()
00083
00084 except (serial.serialutil.SerialException), e:
00085 raise RuntimeError('lib_robotis: Serial port not found!\n')
00086 if(self.servo_dev == None):
00087 raise RuntimeError('lib_robotis: Serial port not found!\n')
00088
00089
00090
00091
00092
00093 class Robotis_Servo():
00094 ''' Class to use a robotis RX-28 or RX-64 servo.
00095 '''
00096 def __init__(self, USB2Dynamixel, servo_id, series = None ):
00097 ''' USB2Dynamixel - USB2Dynamixel_Device object to handle serial port.
00098 Handles threadsafe operation for multiple servos
00099 servo_id - servo ids connected to USB2Dynamixel 1,2,3,4 ... (1 to 253)
00100 [0 is broadcast if memory serves]
00101 series - Just a convenience for defining "good" defaults on MX series.
00102 When set to "MX" it uses these values, otherwise it uses values
00103 better for AX / RX series. Any of the defaults can be overloaded
00104 on a servo-by-servo bases in servo_config.py
00105 '''
00106
00107
00108 if series == 'MX':
00109 defaults = {
00110 'home_encoder': 0x7FF,
00111 'max_encoder': 0xFFF,
00112 'rad_per_enc': 2*math.pi / 0xFFF,
00113 'max_ang': math.pi,
00114 'min_ang': -math.pi,
00115 'flipped': False,
00116 'max_speed': math.radians(180)
00117 }
00118 else:
00119 defaults = {
00120 'home_encoder': 0x200,
00121 'max_encoder': 0x3FF,
00122 'rad_per_enc': math.radians(300.0) / 1024.0,
00123 'max_ang': math.radians(148),
00124 'min_ang': math.radians(-148),
00125 'flipped': False,
00126 'max_speed': math.radians(180)
00127 }
00128
00129
00130 if USB2Dynamixel == None:
00131 raise RuntimeError('lib_robotis: Robotis Servo requires USB2Dynamixel!\n')
00132 else:
00133 self.dyn = USB2Dynamixel
00134
00135
00136 self.servo_id = servo_id
00137 try:
00138 self.read_address(3)
00139 except:
00140 raise RuntimeError('lib_robotis: Error encountered. Could not find ID (%d) on bus (%s), or USB2Dynamixel 3-way switch in wrong position.\n' %
00141 ( servo_id, self.dyn.dev_name ))
00142
00143
00144 data = self.read_address( 0x05, 1)
00145 self.return_delay = data[0] * 2e-6
00146
00147
00148 self.settings = {}
00149 try:
00150 import servo_config as sc
00151 if sc.servo_param.has_key( self.servo_id ):
00152 self.settings = sc.servo_param[ self.servo_id ]
00153 else:
00154 print 'Warning: servo_id ', self.servo_id, ' not found in servo_config.py. Using defaults.'
00155 except:
00156 print 'Warning: servo_config.py not found. Using defaults.'
00157
00158
00159 for key in defaults.keys():
00160 if self.settings.has_key( key ):
00161 pass
00162 else:
00163 self.settings[ key ] = defaults[ key ]
00164
00165 def init_cont_turn(self):
00166 '''sets CCW angle limit to zero and allows continuous turning (good for wheels).
00167 After calling this method, simply use 'set_angvel' to command rotation. This
00168 rotation is proportional to torque according to Robotis documentation.
00169 '''
00170 self.write_address(0x08, [0,0])
00171
00172 def kill_cont_turn(self):
00173 '''resets CCW angle limits to allow commands through 'move_angle' again
00174 '''
00175 self.write_address(0x08, [255, 3])
00176
00177 def is_moving(self):
00178 ''' returns True if servo is moving.
00179 '''
00180 data = self.read_address( 0x2e, 1 )
00181 return data[0] != 0
00182
00183 def read_voltage(self):
00184 ''' returns voltage (Volts)
00185 '''
00186 data = self.read_address( 0x2a, 1 )
00187 return data[0] / 10.
00188
00189 def read_temperature(self):
00190 ''' returns the temperature (Celcius)
00191 '''
00192 data = self.read_address( 0x2b, 1 )
00193 return data[0]
00194
00195 def read_load(self):
00196 ''' number proportional to the torque applied by the servo.
00197 sign etc. might vary with how the servo is mounted.
00198 '''
00199 data = self.read_address( 0x28, 2 )
00200 load = data[0] + (data[1] >> 6) * 256
00201 if data[1] >> 2 & 1 == 0:
00202 return -1.0 * load
00203 else:
00204 return 1.0 * load
00205
00206 def read_encoder(self):
00207 ''' returns position in encoder ticks
00208 '''
00209 data = self.read_address( 0x24, 2 )
00210 enc_val = data[0] + data[1] * 256
00211 return enc_val
00212
00213 def read_angle(self):
00214 ''' returns the current servo angle (radians)
00215 '''
00216 ang = (self.read_encoder() - self.settings['home_encoder']) * self.settings['rad_per_enc']
00217 if self.settings['flipped']:
00218 ang = ang * -1.0
00219 return ang
00220
00221 def move_angle(self, ang, angvel=None, blocking=True):
00222 ''' move to angle (radians)
00223 '''
00224 if angvel == None:
00225 angvel = self.settings['max_speed']
00226
00227 if angvel > self.settings['max_speed']:
00228 print 'lib_robotis.move_angle: angvel too high - %.2f deg/s' % (math.degrees(angvel))
00229 print 'lib_robotis.ignoring move command.'
00230 return
00231
00232 if ang > self.settings['max_ang'] or ang < self.settings['min_ang']:
00233 print 'lib_robotis.move_angle: angle out of range- ', math.degrees(ang)
00234 print 'lib_robotis.ignoring move command.'
00235 return
00236
00237 self.set_angvel(angvel)
00238
00239 if self.settings['flipped']:
00240 ang = ang * -1.0
00241 enc_tics = int(round( ang / self.settings['rad_per_enc'] ))
00242 enc_tics += self.settings['home_encoder']
00243 self.move_to_encoder( enc_tics )
00244
00245 if blocking == True:
00246 while(self.is_moving()):
00247 continue
00248
00249 def move_to_encoder(self, n):
00250 ''' move to encoder position n
00251 '''
00252
00253
00254 n = min( max( n, 0 ), self.settings['max_encoder'] )
00255 hi,lo = n / 256, n % 256
00256 return self.write_address( 0x1e, [lo,hi] )
00257
00258 def enable_torque(self):
00259 return self.write_address(0x18, [1])
00260
00261 def disable_torque(self):
00262 return self.write_address(0x18, [0])
00263
00264 def set_angvel(self, angvel):
00265 ''' angvel - in rad/sec
00266 '''
00267 rpm = angvel / (2 * math.pi) * 60.0
00268 angvel_enc = int(round( rpm / 0.111 ))
00269 if angvel_enc<0:
00270 hi,lo = abs(angvel_enc) / 256 + 4, abs(angvel_enc) % 256
00271 else:
00272 hi,lo = angvel_enc / 256, angvel_enc % 256
00273
00274 return self.write_address( 0x20, [lo,hi] )
00275
00276 def write_id(self, id):
00277 ''' changes the servo id
00278 '''
00279 return self.write_address( 0x03, [id] )
00280
00281 def __calc_checksum(self, msg):
00282 chksum = 0
00283 for m in msg:
00284 chksum += m
00285 chksum = ( ~chksum ) % 256
00286 return chksum
00287
00288 def read_address(self, address, nBytes=1):
00289 ''' reads nBytes from address on the servo.
00290 returns [n1,n2 ...] (list of parameters)
00291 '''
00292 msg = [ 0x02, address, nBytes ]
00293 return self.send_instruction( msg, self.servo_id )
00294
00295 def write_address(self, address, data):
00296 ''' writes data at the address.
00297 data = [n1,n2 ...] list of numbers.
00298 return [n1,n2 ...] (list of return parameters)
00299 '''
00300 msg = [ 0x03, address ] + data
00301 return self.send_instruction( msg, self.servo_id )
00302
00303 def send_instruction(self, instruction, id):
00304 msg = [ id, len(instruction) + 1 ] + instruction
00305 chksum = self.__calc_checksum( msg )
00306 msg = [ 0xff, 0xff ] + msg + [chksum]
00307
00308 self.dyn.acq_mutex()
00309 try:
00310 self.send_serial( msg )
00311 data, err = self.receive_reply()
00312 except:
00313 self.dyn.rel_mutex()
00314 raise
00315 self.dyn.rel_mutex()
00316
00317 if err != 0:
00318 self.process_err( err )
00319
00320 return data
00321
00322 def process_err( self, err ):
00323 raise RuntimeError('lib_robotis: An error occurred: %d\n' % err)
00324
00325 def receive_reply(self):
00326 start = self.dyn.read_serial( 2 )
00327 if start != '\xff\xff':
00328 raise RuntimeError('lib_robotis: Failed to receive start bytes\n')
00329 servo_id = self.dyn.read_serial( 1 )
00330 if ord(servo_id) != self.servo_id:
00331 raise RuntimeError('lib_robotis: Incorrect servo ID received: %d\n' % ord(servo_id))
00332 data_len = self.dyn.read_serial( 1 )
00333 err = self.dyn.read_serial( 1 )
00334 data = self.dyn.read_serial( ord(data_len) - 2 )
00335 checksum = self.dyn.read_serial( 1 )
00336 return [ord(v) for v in data], ord(err)
00337
00338
00339 def send_serial(self, msg):
00340 """ sends the command to the servo
00341 """
00342 out = ''
00343 for m in msg:
00344 out += chr(m)
00345 self.dyn.send_serial( out )
00346
00347
00348
00349
00350
00351 def find_servos(dyn):
00352 ''' Finds all servo IDs on the USB2Dynamixel '''
00353 print 'Scanning for Servos.'
00354 servos = []
00355 dyn.servo_dev.setTimeout( 0.03 )
00356 for i in xrange(254):
00357 try:
00358 s = Robotis_Servo( dyn, i )
00359 print '\n FOUND A SERVO @ ID %d\n' % i
00360 servos.append( i )
00361 except:
00362 pass
00363 dyn.servo_dev.setTimeout( 1.0 )
00364 return servos
00365
00366
00367 def recover_servo(dyn):
00368 ''' Recovers a bricked servo by booting into diagnostic bootloader and resetting '''
00369 raw_input('Make sure only one servo connected to USB2Dynamixel Device [ENTER]')
00370 raw_input('Disconnect power from the servo, but leave USB2Dynamixel connected to USB. [ENTER]')
00371
00372 dyn.servo_dev.setBaudrate( 57600 )
00373
00374 print 'Get Ready. Be ready to reconnect servo power when I say \'GO!\''
00375 print 'After a second, the red LED should become permanently lit.'
00376 print 'After that happens, Ctrl + C to kill this program.'
00377 print
00378 print 'Then, you will need to use a serial terminal to issue additional commands.',
00379 print 'Here is an example using screen as serial terminal:'
00380 print
00381 print 'Command Line: screen /dev/robot/servo_left 57600'
00382 print 'Type: \'h\''
00383 print 'Response: Command : L(oad),G(o),S(ystem),A(pplication),R(eset),D(ump),C(lear)'
00384 print 'Type: \'C\''
00385 print 'Response: * Clear EEPROM '
00386 print 'Type: \'A\''
00387 print 'Response: * Application Mode'
00388 print 'Type: \'G\''
00389 print 'Response: * Go'
00390 print
00391 print 'Should now be able to reconnect to the servo using ID 1'
00392 print
00393 print
00394 raw_input('Ready to reconnect power? [ENTER]')
00395 print 'GO!'
00396
00397 while True:
00398 s.write('#')
00399 time.sleep(0.0001)
00400
00401
00402 if __name__ == '__main__':
00403 p = optparse.OptionParser()
00404 p.add_option('-d', action='store', type='string', dest='dev_name',
00405 help='Required: Device string for USB2Dynamixel. [i.e. /dev/ttyUSB0 for Linux, \'0\' (for COM1) on Windows]')
00406 p.add_option('--scan', action='store_true', dest='scan', default=False,
00407 help='Scan the device for servo IDs attached.')
00408 p.add_option('--recover', action='store_true', dest='recover', default=False,
00409 help='Recover from a bricked servo (restores to factory defaults).')
00410 p.add_option('--ang', action='store', type='float', dest='ang',
00411 help='Angle to move the servo to (degrees).')
00412 p.add_option('--ang_vel', action='store', type='float', dest='ang_vel',
00413 help='angular velocity. (degrees/sec) [default = 50]', default=50)
00414 p.add_option('--id', action='store', type='int', dest='id',
00415 help='id of servo to connect to, [default = 1]', default=1)
00416 p.add_option('--baud', action='store', type='int', dest='baud',
00417 help='baudrate for USB2Dynamixel connection [default = 57600]', default=57600)
00418
00419 opt, args = p.parse_args()
00420
00421 if opt.dev_name == None:
00422 p.print_help()
00423 sys.exit(0)
00424
00425 dyn = USB2Dynamixel_Device(opt.dev_name, opt.baud)
00426
00427 if opt.scan:
00428 find_servos( dyn )
00429
00430 if opt.recover:
00431 recover_servo( dyn )
00432
00433 if opt.ang != None:
00434 servo = Robotis_Servo( dyn, opt.id )
00435 servo.move_angle( math.radians(opt.ang), math.radians(opt.ang_vel) )
00436