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

Added try-except around get_baud()

Browse Source
This commit is contained in:
Patrick Goebel 2016-07-14 06:27:17 -07:00
parent 9231e6c8bd
commit dc6873ee08
1 changed files with 377 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,374 +1,377 @@
#!/usr/bin/env python #!/usr/bin/env python
""" """
A Python driver for the Arduino microcontroller running the A Python driver for the Arduino microcontroller running the
ROSArduinoBridge firmware. ROSArduinoBridge firmware.
Created for the Pi Robot Project: http://www.pirobot.org Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved. Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or the Free Software Foundation; either version 2 of the License, or
(at your option) any later version. (at your option) any later version.
This program is distributed in the hope that it will be useful, This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at: GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html http://www.gnu.org/licenses/gpl.html
""" """
import thread import thread
from math import pi as PI, degrees, radians from math import pi as PI, degrees, radians
import os import os
import time import time
import sys, traceback import sys, traceback
from serial.serialutil import SerialException from serial.serialutil import SerialException
from serial import Serial from serial import Serial
SERVO_MAX = 180 SERVO_MAX = 180
SERVO_MIN = 0 SERVO_MIN = 0
class Arduino: class Arduino:
''' Configuration Parameters ''' Configuration Parameters
''' '''
N_ANALOG_PORTS = 6 N_ANALOG_PORTS = 6
N_DIGITAL_PORTS = 12 N_DIGITAL_PORTS = 12
def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5):
self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller. self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller.
self.PID_INTERVAL = 1000 / 30 self.PID_INTERVAL = 1000 / 30
self.port = port self.port = port
self.baudrate = baudrate self.baudrate = baudrate
self.timeout = timeout self.timeout = timeout
self.encoder_count = 0 self.encoder_count = 0
self.writeTimeout = timeout self.writeTimeout = timeout
self.interCharTimeout = timeout / 30. self.interCharTimeout = timeout / 30.
# Keep things thread safe # Keep things thread safe
self.mutex = thread.allocate_lock() self.mutex = thread.allocate_lock()
# An array to cache analog sensor readings # An array to cache analog sensor readings
self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
# An array to cache digital sensor readings # An array to cache digital sensor readings
self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
def connect(self): def connect(self):
try: try:
print "Connecting to Arduino on port", self.port, "..." print "Connecting to Arduino on port", self.port, "..."
self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
# The next line is necessary to give the firmware time to wake up. # The next line is necessary to give the firmware time to wake up.
time.sleep(1) time.sleep(1)
test = self.get_baud() test = self.get_baud()
if test != self.baudrate: if test != self.baudrate:
time.sleep(1) time.sleep(1)
test = self.get_baud() test = self.get_baud()
if test != self.baudrate: if test != self.baudrate:
raise SerialException raise SerialException
print "Connected at", self.baudrate print "Connected at", self.baudrate
print "Arduino is ready." print "Arduino is ready."
except SerialException: except SerialException:
print "Serial Exception:" print "Serial Exception:"
print sys.exc_info() print sys.exc_info()
print "Traceback follows:" print "Traceback follows:"
traceback.print_exc(file=sys.stdout) traceback.print_exc(file=sys.stdout)
print "Cannot connect to Arduino!" print "Cannot connect to Arduino!"
os._exit(1) os._exit(1)
def open(self): def open(self):
''' Open the serial port. ''' Open the serial port.
''' '''
self.port.open() self.port.open()
def close(self): def close(self):
''' Close the serial port. ''' Close the serial port.
''' '''
self.port.close() self.port.close()
def send(self, cmd): def send(self, cmd):
''' 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.
''' '''
self.port.write(cmd + '\r') self.port.write(cmd + '\r')
def recv(self, timeout=0.5): def recv(self, timeout=0.5):
timeout = min(timeout, self.timeout) 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 = ''
attempts = 0 attempts = 0
while c != '\r': while c != '\r':
c = self.port.read(1) c = self.port.read(1)
value += c value += c
attempts += 1 attempts += 1
if attempts * self.interCharTimeout > timeout: if attempts * self.interCharTimeout > timeout:
return None return None
value = value.strip('\r') value = value.strip('\r')
return value 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
below in a thread safe manner. below in a thread safe manner.
''' '''
ack = self.recv(self.timeout) ack = self.recv(self.timeout)
return ack == 'OK' return ack == 'OK'
def recv_int(self): def recv_int(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
below in a thread safe manner. below in a thread safe manner.
''' '''
value = self.recv(self.timeout) value = self.recv(self.timeout)
try: try:
return int(value) return int(value)
except: except:
return None return None
def recv_array(self): def recv_array(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
below in a thread safe manner. below in a thread safe manner.
''' '''
try: try:
values = self.recv(self.timeout * self.N_ANALOG_PORTS).split() values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
return map(int, values) return map(int, values)
except: except:
return [] return []
def execute(self, cmd): def execute(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning a single integer value. ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
''' '''
self.mutex.acquire() self.mutex.acquire()
try: try:
self.port.flushInput() self.port.flushInput()
except: except:
pass pass
ntries = 1 ntries = 1
attempts = 0 attempts = 0
try: try:
self.port.write(cmd + '\r') self.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 < ntries and (value == '' or value == 'Invalid Command' or value == None):
try: try:
self.port.flushInput() self.port.flushInput()
self.port.write(cmd + '\r') self.port.write(cmd + '\r')
value = self.recv(self.timeout) value = self.recv(self.timeout)
except: except:
print "Exception executing command: " + cmd print "Exception executing command: " + cmd
attempts += 1 attempts += 1
except: except:
self.mutex.release() self.mutex.release()
print "Exception executing command: " + cmd print "Exception executing command: " + cmd
value = None value = None
self.mutex.release() self.mutex.release()
return int(value) return int(value)
def execute_array(self, cmd): def execute_array(self, cmd):
''' 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: try:
self.port.flushInput() self.port.flushInput()
except: except:
pass pass
ntries = 1 ntries = 1
attempts = 0 attempts = 0
try: try:
self.port.write(cmd + '\r') self.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 < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
try: try:
self.port.flushInput() self.port.flushInput()
self.port.write(cmd + '\r') self.port.write(cmd + '\r')
values = self.recv_array() values = self.recv_array()
except: except:
print("Exception executing command: " + cmd) print("Exception executing command: " + cmd)
attempts += 1 attempts += 1
except: except:
self.mutex.release() self.mutex.release()
print "Exception executing command: " + cmd print "Exception executing command: " + cmd
raise SerialException raise SerialException
return [] return []
try: try:
values = map(int, values) values = map(int, values)
except: except:
values = [] values = []
self.mutex.release() self.mutex.release()
return values return values
def execute_ack(self, cmd): def execute_ack(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
''' '''
self.mutex.acquire() self.mutex.acquire()
try: try:
self.port.flushInput() self.port.flushInput()
except: except:
pass pass
ntries = 1 ntries = 1
attempts = 0 attempts = 0
try: try:
self.port.write(cmd + '\r') self.port.write(cmd + '\r')
ack = self.recv(self.timeout) ack = self.recv(self.timeout)
while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
try: try:
self.port.flushInput() self.port.flushInput()
self.port.write(cmd + '\r') self.port.write(cmd + '\r')
ack = self.recv(self.timeout) ack = self.recv(self.timeout)
except: except:
print "Exception executing command: " + cmd print "Exception executing command: " + cmd
attempts += 1 attempts += 1
except: except:
self.mutex.release() self.mutex.release()
print "execute_ack exception when executing", cmd print "execute_ack exception when executing", cmd
print sys.exc_info() print sys.exc_info()
return 0 return 0
self.mutex.release() self.mutex.release()
return ack == 'OK' return ack == 'OK'
def update_pid(self, Kp, Kd, Ki, Ko): def update_pid(self, Kp, Kd, Ki, Ko):
''' Set the PID parameters on the Arduino ''' Set the PID parameters on the Arduino
''' '''
print "Updating PID parameters" print "Updating PID parameters"
cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko) cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
self.execute_ack(cmd) self.execute_ack(cmd)
def get_baud(self): def get_baud(self):
''' Get the current baud rate on the serial port. ''' Get the current baud rate on the serial port.
''' '''
return int(self.execute('b')); try:
return int(self.execute('b'));
def get_encoder_counts(self): except:
values = self.execute_array('e') return None
if len(values) != 2:
print "Encoder count was not 2" def get_encoder_counts(self):
raise SerialException values = self.execute_array('e')
return None if len(values) != 2:
else: print "Encoder count was not 2"
return values raise SerialException
return None
def reset_encoders(self): else:
''' Reset the encoder counts to 0 return values
'''
return self.execute_ack('r') def reset_encoders(self):
''' Reset the encoder counts to 0
def drive(self, right, left): '''
''' Speeds are given in encoder ticks per PID interval return self.execute_ack('r')
'''
return self.execute_ack('m %d %d' %(right, left)) def drive(self, right, left):
''' Speeds are given in encoder ticks per PID interval
def drive_m_per_s(self, right, left): '''
''' Set the motor speeds in meters per second. return self.execute_ack('m %d %d' %(right, left))
'''
left_revs_per_second = float(left) / (self.wheel_diameter * PI) def drive_m_per_s(self, right, left):
right_revs_per_second = float(right) / (self.wheel_diameter * PI) ''' Set the motor speeds in meters per second.
'''
left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) left_revs_per_second = float(left) / (self.wheel_diameter * PI)
right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) right_revs_per_second = float(right) / (self.wheel_diameter * PI)
self.drive(right_ticks_per_loop , left_ticks_per_loop ) left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
def stop(self):
''' Stop both motors. self.drive(right_ticks_per_loop , left_ticks_per_loop )
'''
self.drive(0, 0) def stop(self):
''' Stop both motors.
def analog_read(self, pin): '''
return self.execute('a %d' %pin) self.drive(0, 0)
def analog_write(self, pin, value): def analog_read(self, pin):
return self.execute_ack('x %d %d' %(pin, value)) return self.execute('a %d' %pin)
def digital_read(self, pin): def analog_write(self, pin, value):
return self.execute('d %d' %pin) return self.execute_ack('x %d %d' %(pin, value))
def digital_write(self, pin, value): def digital_read(self, pin):
return self.execute_ack('w %d %d' %(pin, value)) return self.execute('d %d' %pin)
def pin_mode(self, pin, mode): def digital_write(self, pin, value):
return self.execute_ack('c %d %d' %(pin, mode)) return self.execute_ack('w %d %d' %(pin, value))
def servo_write(self, id, pos): def pin_mode(self, pin, mode):
''' Usage: servo_write(id, pos) return self.execute_ack('c %d %d' %(pin, mode))
Position is given in radians and converted to degrees before sending
''' def servo_write(self, id, pos):
return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos))))) ''' Usage: servo_write(id, pos)
Position is given in radians and converted to degrees before sending
def servo_read(self, id): '''
''' Usage: servo_read(id) return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
The returned position is converted from degrees to radians
''' def servo_read(self, id):
return radians(self.execute('t %d' %id)) ''' Usage: servo_read(id)
The returned position is converted from degrees to radians
def ping(self, pin): '''
''' The srf05/Ping command queries an SRF05/Ping sonar sensor return radians(self.execute('t %d' %id))
connected to the General Purpose I/O line pinId for a distance,
and returns the range in cm. Sonar distance resolution is integer based. def ping(self, pin):
''' ''' The srf05/Ping command queries an SRF05/Ping sonar sensor
return self.execute('p %d' %pin); connected to the General Purpose I/O line pinId for a distance,
and returns the range in cm. Sonar distance resolution is integer based.
# def get_maxez1(self, triggerPin, outputPin): '''
# ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar return self.execute('p %d' %pin);
# sensor connected to the General Purpose I/O lines, triggerPin, and
# outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE # def get_maxez1(self, triggerPin, outputPin):
# SURE there's nothing directly in front of the MaxSonar-EZ1 upon # ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar
# power up, otherwise it wont range correctly for object less than 6 # sensor connected to the General Purpose I/O lines, triggerPin, and
# inches away! The sensor reading defaults to use English units # outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE
# (inches). The sensor distance resolution is integer based. Also, the # SURE there's nothing directly in front of the MaxSonar-EZ1 upon
# maxsonar trigger pin is RX, and the echo pin is PW. # power up, otherwise it wont range correctly for object less than 6
# ''' # inches away! The sensor reading defaults to use English units
# return self.execute('z %d %d' %(triggerPin, outputPin)) # (inches). The sensor distance resolution is integer based. Also, the
# maxsonar trigger pin is RX, and the echo pin is PW.
# '''
""" Basic test for connectivity """ # return self.execute('z %d %d' %(triggerPin, outputPin))
if __name__ == "__main__":
if os.name == "posix":
portName = "/dev/ttyACM0" """ Basic test for connectivity """
else: if __name__ == "__main__":
portName = "COM43" # Windows style COM port. if os.name == "posix":
portName = "/dev/ttyACM0"
baudRate = 57600 else:
portName = "COM43" # Windows style COM port.
myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
myArduino.connect() baudRate = 57600
print "Sleeping for 1 second..." myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
time.sleep(1) myArduino.connect()
print "Reading on analog port 0", myArduino.analog_read(0) print "Sleeping for 1 second..."
print "Reading on digital port 0", myArduino.digital_read(0) time.sleep(1)
print "Blinking the LED 3 times"
for i in range(3): print "Reading on analog port 0", myArduino.analog_read(0)
myArduino.digital_write(13, 1) print "Reading on digital port 0", myArduino.digital_read(0)
time.sleep(1.0) print "Blinking the LED 3 times"
#print "Current encoder counts", myArduino.encoders() for i in range(3):
myArduino.digital_write(13, 1)
print "Connection test successful.", time.sleep(1.0)
#print "Current encoder counts", myArduino.encoders()
myArduino.stop()
myArduino.close() print "Connection test successful.",
print "Shutting down Arduino." myArduino.stop()
myArduino.close()
print "Shutting down Arduino."