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 import usb
00030 import pickle
00031 import time
00032 import numpy as np
00033
00034 import math
00035 import struct
00036
00037 class Mecanum_Properties():
00038 def __init__( self ):
00039 self.r1 = .2032/2.
00040 self.r2 = .2032/2.
00041
00042 self.R = 4*2.54/100
00043
00044 self.la = .6223/2
00045 self.lb = .33655/2
00046
00047 class Mecanum( Mecanum_Properties ):
00048 def __init__( self ):
00049 self.segway_front = Segway( side='front')
00050 self.segway_back = Segway( side='back' )
00051
00052 Mecanum_Properties.__init__(self)
00053 self.get_status()
00054
00055 def set_velocity( self, xvel, yvel, avel ):
00056 """xvel and yvel should be in m/s and avel should be rad/s"""
00057 yvel = -yvel
00058
00059 R = self.R
00060 la = self.la
00061 lb = self.lb
00062
00063
00064
00065
00066 J = 1/R * np.matrix([-1,1, (la + lb),
00067 1,1,-(la + lb),
00068 -1,1,-(la + lb),
00069 1,1, (la + lb) ]).reshape(4,3)
00070
00071
00072 dP = np.matrix( [ -yvel, -xvel, avel] ).T
00073
00074 w = J*dP
00075
00076
00077
00078 flw = w[1,0]
00079 frw = w[0,0]
00080 blw = w[2,0]
00081 brw = w[3,0]
00082
00083 front_cmd = self.segway_front.send_wheel_velocities( flw, frw )
00084 back_cmd = self.segway_back.send_wheel_velocities( blw, brw )
00085
00086 if front_cmd == False or back_cmd == False:
00087 print 'Velocities out of spec: ',flw,frw,blw,brw
00088 print 'Perhaps we should consider sending the max command so motion doesn\'t hang.'
00089
00090 def stop(self):
00091 self.set_velocity(0.,0.,0.)
00092
00093 def get_status( self ):
00094 pass
00095
00096 class Segway_Properties():
00097 def __init__( self ):
00098 self._integrated_wheel_displacement = 24372/(2*math.pi)
00099
00100
00101 vv = 39
00102 tv = 22
00103 self._V_vel = vv /self._integrated_wheel_displacement
00104 self._T_vel = tv /self._integrated_wheel_displacement
00105
00106
00107
00108
00109 a = self._V_vel
00110 b = self._T_vel
00111 c = self._V_vel
00112 d = -self._T_vel
00113 self.A = np.matrix([a,b,c,d]).reshape(2,2)
00114 self.A_inv = 1/(a*b-c*d) * np.matrix([ d, -b, -c, a ]).reshape(2,2)
00115
00116
00117
00118
00119
00120
00121 self._power_base_battery_voltage = 1/4.
00122 self._ui_battery_voltage = .0125
00123 self._ui_battery_voltage_offset = 1.4
00124
00125 class Segway( Segway_Properties ):
00126
00127 def __init__( self, side='front' ):
00128 """side should be 'front' or 'back'"""
00129 Segway_Properties.__init__(self)
00130
00131 self.side = side
00132 self.segway = None
00133 self.connect()
00134
00135 self.pitch_ang = 0
00136 self.pitch_rate = 0
00137 self.yaw_ang = 0
00138 self.yaw_rate = 0
00139 self.LW_vel = 0
00140 self.RW_vel = 0
00141 self.yaw_rate = 0
00142 self.servo_frames = 0
00143 self.integrated_wheel_displacement_left = 0
00144 self.integrated_wheel_displacement_right = 0
00145 self.integrated_for_aft_displacement = 0
00146 self.integrated_yaw_displacement = 0
00147 self.left_motor_torque = 0
00148 self.right_motor_torque = 0
00149 self.operational_mode = 0
00150 self.controller_gain_schedule = 0
00151 self.ui_battery_voltage = 0
00152 self.power_base_battery_voltage = 0
00153 self.velocity_commanded = 0
00154 self.turn_commanded = 0
00155
00156 def connect(self):
00157 buses = usb.busses()
00158 segway_handle_list = []
00159 for bus in buses:
00160 for device in bus.devices:
00161 if device.idVendor == 1027 and device.idProduct == 59177:
00162 h = device.open()
00163 serial_num = int(h.getString(3,10))
00164 if serial_num == 215:
00165 if self.side == 'front':
00166 print 'Connected to front segway'
00167 self.segway = h
00168 self.segway.claimInterface(0)
00169 elif serial_num == 201:
00170 if self.side == 'back':
00171 print 'Connected to rear segway'
00172 self.segway = h
00173 self.segway.claimInterface(0)
00174 else:
00175 raise RuntimeError( 'Unknown_segway connected.' +
00176 ' Serial Number is ',serial_num )
00177
00178
00179 def calc_checksum(self, msg):
00180 checksum = 0
00181 for byt in msg:
00182 checksum = (checksum+byt)%65536
00183 checksum_hi = checksum >> 8
00184 checksum &= 0xff
00185 checksum = (checksum+checksum_hi)%65536
00186 checksum_hi = checksum >> 8
00187 checksum &= 0xff
00188 checksum = (checksum+checksum_hi)%65536
00189 checksum = (~checksum+1)&0xff
00190 return checksum
00191
00192 def compose_velocity_cmd(self,linvel,angvel):
00193 byt_hi = 0x04
00194 byt_lo = 0x13
00195
00196 if self.side == 'back':
00197 linvel = -linvel
00198 linvel_counts = int(linvel)
00199 angvel_counts = int(angvel)
00200
00201 if abs(linvel_counts)>1176:
00202 print 'connect.compose_velocity_cmd: Linear velocity is too high. counts: %d'%linvel
00203 return []
00204
00205 if abs(angvel_counts)>1024:
00206 print 'connect.compose_velocity_cmd: Angular velocity is too high. counts: %d'%angvel
00207 return []
00208
00209 usb_msg_fixed = [0xf0,0x55,0x00,0x00,0x00,0x00,byt_hi,byt_lo,0x00]
00210 can_vel_msg = [(linvel_counts>>8)&0xff,linvel_counts&0xff,(angvel_counts>>8)&0xff,angvel_counts&0xff,0x00,0x00,0x00,0x00]
00211 msg_send = usb_msg_fixed + can_vel_msg
00212 msg_send.append(self.calc_checksum(msg_send))
00213
00214 return msg_send
00215
00216
00217 def send_command(self, linvel0, angvel0 ):
00218 msg0 = self.compose_velocity_cmd(linvel0,angvel0)
00219 if msg0 == []:
00220 return False
00221
00222 for i in range(1):
00223 self.segway.bulkWrite(0x02,msg0)
00224
00225 def send_wheel_velocities( self, lvel, rvel ):
00226 w = np.matrix([lvel,rvel]).T
00227
00228 V = self.A_inv*w
00229
00230
00231 xv = V[0,0]
00232 tv = V[1,0]
00233
00234
00235
00236
00237
00238
00239 return self.send_command( xv, tv )
00240
00241 def parse_usb_cmd(self,msg):
00242 if len(msg) < 18:
00243 return
00244
00245 if self.calc_checksum(msg[:-1]) != msg[-1]:
00246
00247 return
00248
00249 id = ((msg[4]<<3)|((msg[5]>>5)&7))&0xfff
00250
00251 data = msg[9:17]
00252 if id == 0x400:
00253
00254 pass
00255 elif id == 0x401:
00256 self.pitch_ang = self._2_bytes( data[0], data[1] )
00257 self.pitch_rate = self._2_bytes( data[2], data[3] )
00258 self.yaw_ang = self._2_bytes( data[4], data[5] )
00259 self.yaw_rate = self._2_bytes( data[6], data[7] )
00260 elif id == 0x402:
00261 self.LW_vel = self._2_bytes( data[0], data[1] )
00262 self.RW_vel = self._2_bytes( data[2], data[3] )
00263 self.yaw_rate = self._2_bytes( data[4], data[5] )
00264 self.servo_frames = self._2_bytes_unsigned( data[6], data[7] )
00265 elif id == 0x403:
00266 self.integrated_wheel_displacement_left = self._4_bytes(data[2],data[3],data[0],data[1])
00267 self.integrated_wheel_displacement_right = self._4_bytes(data[6],data[7],data[4],data[5])
00268 pass
00269 elif id == 0x404:
00270 self.integrated_for_aft_displacement = self._4_bytes(data[2],data[3],data[0],data[1])
00271 self.integrated_yaw_displacement = self._4_bytes(data[6],data[7],data[4],data[5])
00272 elif id == 0x405:
00273 self.left_motor_torque = self._2_bytes( data[0], data[1] )
00274 self.right_motor_torque = self._2_bytes( data[2], data[3] )
00275 elif id == 0x406:
00276 self.operational_mode = self._2_bytes( data[0], data[1] )
00277 self.controller_gain_schedule = self._2_bytes( data[2], data[3] )
00278 self.ui_battery_voltage = self._2_bytes( data[4], data[5] )*self._ui_battery_voltage + self._ui_battery_voltage_offset
00279 self.power_base_battery_voltage = self._2_bytes( data[6], data[7] )*self._power_base_battery_voltage
00280 elif id == 0x407:
00281 self.velocity_commanded = self._2_bytes( data[0], data[1] )
00282 self.turn_commanded = self._2_bytes( data[2], data[3] )
00283 elif msg[1] == 0x00:
00284
00285 pass
00286 elif id == 0x680:
00287
00288 pass
00289 else:
00290
00291 pass
00292
00293 def _2_bytes( self,high, low ):
00294 return struct.unpack('>h',chr(high)+chr(low))[0]
00295
00296 def _2_bytes_unsigned( self,high, low ):
00297 return struct.unpack('>H',chr(high)+chr(low))[0]
00298
00299 def _4_bytes( self,highhigh, lowhigh, highlow, lowlow ):
00300 return struct.unpack('>l',chr(highhigh)+chr(lowhigh)+chr(highlow)+chr(lowlow))[0]
00301
00302 def clear_read(self):
00303 rd = self.segway.bulkRead(0x81,1000)
00304
00305 def read(self):
00306 before = time.time()
00307 rd = self.segway.bulkRead(0x81,9*(18+2))
00308 msg = [(a & 0xff) for a in rd]
00309
00310 i=0
00311 while 1:
00312 try:
00313 msg.pop(i*62)
00314 msg.pop(i*62)
00315 i += 1
00316 except IndexError:
00317 break
00318
00319
00320 idx1 = msg.index(0xf0)
00321
00322 if msg[idx1+18] != 0xf0:
00323
00324 msg = msg[idx1+1:]
00325 else:
00326
00327 while len(msg) > 17:
00328 try:
00329 single_msg = msg[idx1:idx1+18]
00330 if (single_msg[1] == 0x55 and single_msg[2] == 0xaa) or single_msg[1] == 0x00:
00331 self.parse_usb_cmd(single_msg)
00332 msg = msg[18:]
00333 except IndexError:
00334 break
00335
00336 def print_feedback( self ):
00337 print 'self.integrated_wheel_displacement_left = ',self.integrated_wheel_displacement_left
00338 print 'self.integrated_wheel_displacement_right = ',self.integrated_wheel_displacement_right
00339 print 'self.LW_vel = ',self.LW_vel
00340 print 'self.RW_vel = ',self.RW_vel
00341 print 'frame = ',self.servo_frames
00342 print 'self.velocity_commanded = ',self.velocity_commanded
00343 print 'self.turn_commanded = ',self.turn_commanded
00344
00345
00346
00347 if __name__ == '__main__':
00348
00349 import segway
00350 seg= segway.Segway()
00351 seg.clear_read()
00352
00353 rrates = []
00354 lrates = []
00355
00356 for vel in range(31):
00357 linvel = 2000.*(vel)/30 - 1000.
00358 angvel = 0.
00359
00360 start = time.time()
00361 last = start
00362 lastwrite = start
00363 lastread = start
00364 while 1:
00365
00366
00367 if time.time() - lastwrite > 0.1:
00368 print 'loop 1',time.time() - start
00369
00370 lastwrite = time.time()
00371
00372 seg.send_wheel_velocities(1.,1.)
00373 if time.time() - lastread > 0.01:
00374 seg.read()
00375 lastread = time.time()
00376
00377 if time.time() - start > 2.0:
00378 break
00379
00380 left_start = seg.integrated_wheel_displacement_left
00381 right_start = seg.integrated_wheel_displacement_right
00382 first_points = time.time()
00383
00384 while 1:
00385
00386
00387 if time.time() - lastwrite > 0.1:
00388 print 'loop 1.5',time.time() - start
00389
00390 lastwrite = time.time()
00391
00392 seg.send_wheel_velocities(1.,1.)
00393 if time.time() - lastread > 0.01:
00394 seg.read()
00395 lastread = time.time()
00396
00397 if time.time() - start > 5.0:
00398 break
00399
00400 left_stop = seg.integrated_wheel_displacement_left
00401 right_stop = seg.integrated_wheel_displacement_right
00402 last_points = time.time()
00403
00404 diff = last_points - first_points
00405 print 'Time: ',diff
00406 print 'left rate: ',(left_stop - left_start)/diff
00407 print 'right rate: ',(right_stop - right_start)/diff
00408
00409 rrates.append(((right_stop - right_start)/diff,linvel))
00410 lrates.append(((left_stop - left_start)/diff,linvel))
00411
00412 while 1:
00413 if time.time() - lastread > 0.01:
00414 seg.read()
00415 lastread = time.time()
00416
00417 if seg.LW_vel ==0 and seg.RW_vel == 0:
00418 break
00419
00420 print 'rrates:',rrates
00421 print 'lrates:',lrates
00422
00423
00424 import pylab
00425 x = []
00426 y = []
00427 x1 = []
00428 y1 = []
00429 for a, b in rrates:
00430 x.append(b)
00431 y.append(a)
00432 for a, b in lrates:
00433 x1.append(b)
00434 y1.append(a)
00435 pylab.plot(x,y,x1,y1)
00436 pylab.show()
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464