1 importos2 importtime3 importsys, traceback4 from serial.serialutil importSerialException5 from serial importSerial6
7 importstruct8 importbinascii9
10
11 classArduino:12 def __init__(self, port="/dev/ttyUSB0", baudrate=115200, timeout=0.5):13
14 self.port =port15 self.baudrate =baudrate16 self.timeout =timeout17 self.encoder_count =018 self.writeTimeout =timeout19 self.interCharTimeout = timeout / 30.20
21 self.WAITING_FF =022 self.WAITING_AA = 1
23 self.RECEIVE_LEN = 2
24 self.RECEIVE_PACKAGE = 3
25 self.RECEIVE_CHECK = 4
26 self.HEADER0 = 0xff
27 self.HEADER1 = 0xaa
28 self.REC_CMD = 5
29 self.count =030 self.data1 =031 self.data2 =032 self.error_flag =033 self.SUCCESS =034 self.FAIL = -1
35
36 self.receive_state_ =self.WAITING_FF37 self.receive_check_sum_ =038 self.payload_command = ''
39 self.payload_ack = ''
40 self.payload_args = ''
41 self.payload_len =042 self.byte_count_ =043 self.receive_message_length_ =044
45 self.mutex =threading.Thread.allocate_lock()46
47 #An array to cache analog sensor readings
48 self.analog_sensor_cache = [None] *self.N_ANALOG_PORTS49
50 #An array to cache digital sensor readings
51 self.digital_sensor_cache = [None] *self.N_DIGITAL_PORTS52
53 defconnect(self):54 try:55 print "Connecting to Arduino on port", self.port, "..."
56 self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)57 #The next line is necessary to give the firmware time to wake up.
58 time.sleep(1)59 state_, val =self.get_baud()60 if val !=self.baudrate:61 time.sleep(1)62 state_, val =self.get_baud()63 if val !=self.baudrate:64 raiseSerialException65 print "Connected at", self.baudrate66 print "Arduino is ready."
67
68 exceptSerialException:69 print "Serial Exception:"
70 printsys.exc_info()71 print "Traceback follows:"
72 traceback.print_exc(file=sys.stdout)73 print "Cannot connect to Arduino!"
74 os._exit(1)75
76 defopen(self):77 self.port.open()78
79 defclose(self):80 self.port.close()81
82 defsend(self, cmd):83 self.port.write(cmd)84
85 defreceiveFiniteStates(self, rx_data):86 if self.receive_state_ ==self.WAITING_FF:87 #print str(binascii.b2a_hex(rx_data))
88 if rx_data == '\xff':89 self.receive_state_ =self.WAITING_AA90 self.receive_check_sum_ =091 self.receive_message_length_ =092 self.byte_count_=093 self.payload_ack = ''
94 self.payload_args = ''
95 self.payload_len =096 self.count =097
98 elif self.receive_state_ ==self.WAITING_AA :99 if rx_data == '\xaa':100 self.receive_state_ =self.REC_CMD101 else:102 self.receive_state_ =self.WAITING_FF103 elif self.receive_state_ ==self.REC_CMD:104 self.count+=1
105 if self.count == 1:106 self.data1,=struct.unpack("B",rx_data)107 elif self.count == 2:108 self.data2,=struct.unpack("B",rx_data)109 self.receive_state_ =self.RECEIVE_LEN110 if self.error_flag == 0 and self.data1 !=0:111 self.error_flag = 1
112 if self.data2 != 0 and self.error_flag ==0:113 self.error_flag = 1
114 elif self.receive_state_ ==self.RECEIVE_LEN:115 self.receive_message_length_, = struct.unpack("B",rx_data)116 self.receive_state_ =self.RECEIVE_PACKAGE117 elif self.receive_state_ ==self.RECEIVE_PACKAGE:118 if self.byte_count_==0:119 self.payload_ack =rx_data120 else:121 self.payload_args +=rx_data122 self.byte_count_ +=1
123 #print "byte:"+str(byte_count_) +","+ "rece_len:"+str(receive_message_length_)
124 if self.byte_count_ >=self.receive_message_length_:125 self.receive_state_ =self.RECEIVE_CHECK126
127 elif self.receive_state_ ==self.RECEIVE_CHECK:128 #if(rx_data == (unsigned char)receive_check_sum_)
129 if 1:130 self.receive_state_ =self.WAITING_FF131 #print str(binascii.b2a_hex(value))
132 #left, right, = struct.unpack('hh', value)
133 #print "left:"+str(left)+", right:"+str(right)
134 return 1
135 else:136 self.receive_state_ =self.WAITING_FF137 else:138 self.receive_state_ =self.WAITING_FF;139 return0140
141 def recv(self, timeout=0.5):142 timeout =min(timeout, self.timeout)143 '''This command should not be used on its own: it is called by the execute commands144 below in a thread safe manner. Note: we use read() instead of readline() since145 readline() tends to return garbage characters from the Arduino146 '''
147 c = ''
148 value = ''
149 attempts =0150 c = self.port.read(1)151 #print str(binascii.b2a_hex(c))
152 while self.receiveFiniteStates(c) != 1:153 c = self.port.read(1)154 #print str(binascii.b2a_hex(c))
155 attempts += 1
156 if attempts * self.interCharTimeout >timeout:157 return0158 return 1
159
160 defrecv_ack(self):161 ack =self.recv(self.timeout)162 return ack == 'OK'
163
164 defexecute(self, cmd):165 self.mutex.acquire()166
167 try:168 self.port.flushInput()169 except:170 pass
171
172 ntries = 1
173 attempts =0174
175 try:176 self.port.write(cmd)177 res =self.recv(self.timeout)178 while attempts < ntries and res !=1:179 try:180 self.port.flushInput()181 self.port.write(cmd)182 res =self.recv(self.timeout)183 except:184 print "Exception executing command:" +str(binascii.b2a_hex(cmd))185 attempts += 1
186 except:187 self.mutex.release()188 print "Exception executing command:" +str(binascii.b2a_hex(cmd))189 return0190
191 self.mutex.release()192 return 1
193
194 defget_baud(self):195 '''Get the current baud rate on the serial port.196 '''
197 cmd_str=struct.pack("4B", self.HEADER0, self.HEADER1, 0x01, 0x00) + struct.pack("B", 0x01)198 if (self.execute(cmd_str))==1 and self.payload_ack == '\x00':199 val, = struct.unpack('I', self.payload_args)200 returnself.SUCCESS, val201 else:202 returnself.FAIL, 0203
204 defget_check_sum(self,list):205 list_len =len(list)206 cs =0207 for i inrange(list_len):208 #print i, list[i]
209 cs +=list[i]210 cs=cs%255
211 returncs212
213 defstop(self):214 '''Stop both motors.215 '''
216 self.drive(0, 0)