yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-05 20:14:07 +05:30
Code Issues Projects Releases Wiki Activity

Modified the execute() and recv() functions to be more robust

Browse Source
This commit is contained in:
Patrick Goebel 2016-04-09 06:19:57 -07:00
parent 59488c7b36
commit 03b5666094
1 changed files with 24 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
ros_arduino_python/src/ros_arduino_python/arduino_driver.py
View File

@ -45,7 +45,6 @@ class Arduino:
self.timeout = timeout self.timeout = timeout
self.encoder_count = 0 self.encoder_count = 0
self.writeTimeout = timeout self.writeTimeout = timeout
self.interCharTimeout = timeout / 30.
# Keep things thread safe # Keep things thread safe
self.mutex = thread.allocate_lock() self.mutex = thread.allocate_lock()
@ -105,44 +104,22 @@ class Arduino:
self.serial_port.write(cmd + '\r') self.serial_port.write(cmd + '\r')
def recv(self, timeout=0.5): def recv(self, timeout=0.5):
timeout = min(timeout, self.timeout)
''' This command should not be used on its own: it is called by the execute commands ''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner. Note: we use read() instead of readline() since below in a thread safe manner. Note: we use read() instead of readline() since
readline() tends to return garbage characters from the Arduino readline() tends to return garbage characters from the Arduino
''' '''
c = '' c = ''
value = '' value = ''
start = time.time()
while c != '\r': while c != '\r':
c = self.serial_port.read(1) c = self.serial_port.read()
value += c if c == '':
if time.time() - start > timeout:
return None return None
value += c
value = value.strip('\r') value = value.strip('\r')
return value return value
# def recv(self, timeout=0.5):
# timeout = min(timeout, self.timeout)
# ''' This command should not be used on its own: it is called by the execute commands
# below in a thread safe manner. Note: we use read() instead of readline() since
# readline() tends to return garbage characters from the Arduino
# '''
# c = ''
# value = ''
# attempts = 0
# while c != '\r':
# c = self.serial_port.read(1)
# value += c
# attempts += 1
# if attempts * self.interCharTimeout > timeout:
# return None
#
# value = value.strip('\r')
#
# return value
def recv_ack(self): def recv_ack(self):
''' This command should not be used on its own: it is called by the execute commands ''' This command should not be used on its own: it is called by the execute commands
@ -165,31 +142,25 @@ class Arduino:
''' This command should not be used on its own: it is called by the execute commands ''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner. below in a thread safe manner.
''' '''
try: return self.recv(self.timeout).split()
values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
return map(int, values)
except:
return []
def execute(self, cmd): def execute(self, cmd, max_attempts=5):
''' Thread safe execution of "cmd" on the Arduino returning a single integer value. ''' Thread safe execution of "cmd" on the Arduino returning a single value.
''' '''
self.mutex.acquire() self.mutex.acquire()
try: self.serial_port.flushInput()
self.serial_port.flushInput() self.serial_port.flushOutput()
except:
pass
ntries = 1
attempts = 0 attempts = 0
try: try:
self.serial_port.write(cmd + '\r') self.serial_port.write(cmd + '\r')
value = self.recv(self.timeout) value = self.recv(self.timeout)
while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): while attempts < max_attempts and (value == '' or value == 'Invalid Command' or value == None):
try: try:
self.serial_port.flushInput() self.serial_port.flushInput()
self.serial_port.flushOutput()
self.serial_port.write(cmd + '\r') self.serial_port.write(cmd + '\r')
value = self.recv(self.timeout) value = self.recv(self.timeout)
except: except:
@ -201,27 +172,26 @@ class Arduino:
value = None value = None
self.mutex.release() self.mutex.release()
return int(value)
def execute_array(self, cmd): return value
def execute_array(self, cmd, max_attempts=5):
''' Thread safe execution of "cmd" on the Arduino returning an array. ''' Thread safe execution of "cmd" on the Arduino returning an array.
''' '''
self.mutex.acquire() self.mutex.acquire()
try: self.serial_port.flushInput()
self.serial_port.flushInput() self.serial_port.flushOutput()
except:
pass
ntries = 1
attempts = 0 attempts = 0
try: try:
self.serial_port.write(cmd + '\r') self.serial_port.write(cmd + '\r')
values = self.recv_array() values = self.recv_array()
while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): while attempts < max_attempts and (values == '' or values == 'Invalid Command' or values == [] or values == None):
try: try:
self.serial_port.flushInput() self.serial_port.flushInput()
self.serial_port.flushOutput()
self.serial_port.write(cmd + '\r') self.serial_port.write(cmd + '\r')
values = self.recv_array() values = self.recv_array()
except: except:
@ -232,13 +202,9 @@ class Arduino:
print "Exception executing command: " + cmd print "Exception executing command: " + cmd
raise SerialException raise SerialException
return [] return []
try:
values = map(int, values)
except:
values = []
self.mutex.release() self.mutex.release()
return values return values
def execute_ack(self, cmd): def execute_ack(self, cmd):
@ -285,7 +251,7 @@ class Arduino:
''' Get the current baud rate on the serial port. ''' Get the current baud rate on the serial port.
''' '''
try: try:
return int(self.execute('b')) return int(self.execute('b', max_attempts=5))
except: except:
print "Failed to determine baud rate of Arduino so aborting!" print "Failed to determine baud rate of Arduino so aborting!"
os._exit(1) os._exit(1)
@ -297,13 +263,13 @@ class Arduino:
raise SerialException raise SerialException
return None return None
else: else:
return values return map(int, values)
def reset_encoders(self): def reset_encoders(self):
''' Reset the encoder counts to 0 ''' Reset the encoder counts to 0
''' '''
return self.execute_ack('r') return self.execute_ack('r')
def drive(self, right, left): def drive(self, right, left):
''' Speeds are given in encoder ticks per PID interval ''' Speeds are given in encoder ticks per PID interval
''' '''
@ -329,13 +295,13 @@ class Arduino:
return self.execute_ack('c A%d %d' %(pin, mode)) return self.execute_ack('c A%d %d' %(pin, mode))
def analog_read(self, pin): def analog_read(self, pin):
return self.execute('a %d' %pin) return int(self.execute('a %d' %pin))
def analog_write(self, pin, value): def analog_write(self, pin, value):
return self.execute_ack('x %d %d' %(pin, value)) return self.execute_ack('x %d %d' %(pin, value))
def digital_read(self, pin): def digital_read(self, pin):
return self.execute('d %d' %pin) return int(self.execute('d %d' %pin))
def digital_write(self, pin, value): def digital_write(self, pin, value):
return self.execute_ack('w %d %d' %(pin, value)) return self.execute_ack('w %d %d' %(pin, value))