arduino_driver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     A Python driver for the Arduino microcontroller running the
00005     ROSArduinoBridge firmware.
00006     
00007     Created for the Pi Robot Project: http://www.pirobot.org
00008     Copyright (c) 2012 Patrick Goebel.  All rights reserved.
00009 
00010     This program is free software; you can redistribute it and/or modify
00011     it under the terms of the GNU General Public License as published by
00012     the Free Software Foundation; either version 2 of the License, or
00013     (at your option) any later version.
00014     
00015     This program is distributed in the hope that it will be useful,
00016     but WITHOUT ANY WARRANTY; without even the implied warranty of
00017     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018     GNU General Public License for more details at:
00019     
00020     http://www.gnu.org/licenses/gpl.html
00021 
00022 """
00023 
00024 import thread
00025 from math import pi as PI, degrees, radians
00026 import os
00027 import time
00028 import sys, traceback
00029 from serial.serialutil import SerialException
00030 from serial import Serial
00031 
00032 SERVO_MAX = 180
00033 SERVO_MIN = 0
00034 
00035 class Arduino:
00036     ''' Configuration Parameters
00037     '''    
00038     N_ANALOG_PORTS = 6
00039     N_DIGITAL_PORTS = 12
00040     
00041     def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5):
00042         
00043         self.PID_RATE = 30 # Do not change this!  It is a fixed property of the Arduino PID controller.
00044         self.PID_INTERVAL = 1000 / 30
00045         
00046         self.port = port
00047         self.baudrate = baudrate
00048         self.timeout = timeout
00049         self.encoder_count = 0
00050         self.writeTimeout = timeout
00051         self.interCharTimeout = timeout / 30.
00052     
00053         # Keep things thread safe
00054         self.mutex = thread.allocate_lock()
00055             
00056         # An array to cache analog sensor readings
00057         self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
00058         
00059         # An array to cache digital sensor readings
00060         self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
00061     
00062     def connect(self):
00063         try:
00064             print "Connecting to Arduino on port", self.port, "..."
00065             self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
00066             # The next line is necessary to give the firmware time to wake up.
00067             time.sleep(1)
00068             test = self.get_baud()
00069             if test != self.baudrate:
00070                 time.sleep(1)
00071                 test = self.get_baud()   
00072                 if test != self.baudrate:
00073                     raise SerialException
00074             print "Connected at", self.baudrate
00075             print "Arduino is ready."
00076 
00077         except SerialException:
00078             print "Serial Exception:"
00079             print sys.exc_info()
00080             print "Traceback follows:"
00081             traceback.print_exc(file=sys.stdout)
00082             print "Cannot connect to Arduino!"
00083             os._exit(1)
00084 
00085     def open(self): 
00086         ''' Open the serial port.
00087         '''
00088         self.port.open()
00089 
00090     def close(self): 
00091         ''' Close the serial port.
00092         '''
00093         self.port.close() 
00094     
00095     def send(self, cmd):
00096         ''' This command should not be used on its own: it is called by the execute commands
00097             below in a thread safe manner.
00098         '''
00099         self.port.write(cmd + '\r')
00100 
00101     def recv(self, timeout=0.5):
00102         timeout = min(timeout, self.timeout)
00103         ''' This command should not be used on its own: it is called by the execute commands   
00104             below in a thread safe manner.  Note: we use read() instead of readline() since
00105             readline() tends to return garbage characters from the Arduino
00106         '''
00107         c = ''
00108         value = ''
00109         attempts = 0
00110         while c != '\r':
00111             c = self.port.read(1)
00112             value += c
00113             attempts += 1
00114             if attempts * self.interCharTimeout > timeout:
00115                 return None
00116 
00117         value = value.strip('\r')
00118 
00119         return value
00120             
00121     def recv_ack(self):
00122         ''' This command should not be used on its own: it is called by the execute commands
00123             below in a thread safe manner.
00124         '''
00125         ack = self.recv(self.timeout)
00126         return ack == 'OK'
00127 
00128     def recv_int(self):
00129         ''' This command should not be used on its own: it is called by the execute commands
00130             below in a thread safe manner.
00131         '''
00132         value = self.recv(self.timeout)
00133         try:
00134             return int(value)
00135         except:
00136             return None
00137 
00138     def recv_array(self):
00139         ''' This command should not be used on its own: it is called by the execute commands
00140             below in a thread safe manner.
00141         '''
00142         try:
00143             values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
00144             return map(int, values)
00145         except:
00146             return []
00147 
00148     def execute(self, cmd):
00149         ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
00150         '''
00151         self.mutex.acquire()
00152         
00153         try:
00154             self.port.flushInput()
00155         except:
00156             pass
00157         
00158         ntries = 1
00159         attempts = 0
00160         
00161         try:
00162             self.port.write(cmd + '\r')
00163             value = self.recv(self.timeout)
00164             while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
00165                 try:
00166                     self.port.flushInput()
00167                     self.port.write(cmd + '\r')
00168                     value = self.recv(self.timeout)
00169                 except:
00170                     print "Exception executing command: " + cmd
00171                 attempts += 1
00172         except:
00173             self.mutex.release()
00174             print "Exception executing command: " + cmd
00175             value = None
00176         
00177         self.mutex.release()
00178         return int(value)
00179 
00180     def execute_array(self, cmd):
00181         ''' Thread safe execution of "cmd" on the Arduino returning an array.
00182         '''
00183         self.mutex.acquire()
00184         
00185         try:
00186             self.port.flushInput()
00187         except:
00188             pass
00189         
00190         ntries = 1
00191         attempts = 0
00192         
00193         try:
00194             self.port.write(cmd + '\r')
00195             values = self.recv_array()
00196             while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
00197                 try:
00198                     self.port.flushInput()
00199                     self.port.write(cmd + '\r')
00200                     values = self.recv_array()
00201                 except:
00202                     print("Exception executing command: " + cmd)
00203                 attempts += 1
00204         except:
00205             self.mutex.release()
00206             print "Exception executing command: " + cmd
00207             raise SerialException
00208             return []
00209         
00210         try:
00211             values = map(int, values)
00212         except:
00213             values = []
00214 
00215         self.mutex.release()
00216         return values
00217         
00218     def execute_ack(self, cmd):
00219         ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
00220         '''
00221         self.mutex.acquire()
00222         
00223         try:
00224             self.port.flushInput()
00225         except:
00226             pass
00227         
00228         ntries = 1
00229         attempts = 0
00230         
00231         try:
00232             self.port.write(cmd + '\r')
00233             ack = self.recv(self.timeout)
00234             while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
00235                 try:
00236                     self.port.flushInput()
00237                     self.port.write(cmd + '\r')
00238                     ack = self.recv(self.timeout)
00239                 except:
00240                     print "Exception executing command: " + cmd
00241             attempts += 1
00242         except:
00243             self.mutex.release()
00244             print "execute_ack exception when executing", cmd
00245             print sys.exc_info()
00246             return 0
00247         
00248         self.mutex.release()
00249         return ack == 'OK'   
00250     
00251     def update_pid(self, Kp, Kd, Ki, Ko):
00252         ''' Set the PID parameters on the Arduino
00253         '''
00254         print "Updating PID parameters"
00255         cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
00256         self.execute_ack(cmd)                          
00257 
00258     def get_baud(self):
00259         ''' Get the current baud rate on the serial port.
00260         '''
00261         return int(self.execute('b'));
00262 
00263     def get_encoder_counts(self):
00264         values = self.execute_array('e')
00265         if len(values) != 2:
00266             print "Encoder count was not 2"
00267             raise SerialException
00268             return None
00269         else:
00270             return values
00271 
00272     def reset_encoders(self):
00273         ''' Reset the encoder counts to 0
00274         '''
00275         return self.execute_ack('r')
00276     
00277     def drive(self, right, left):
00278         ''' Speeds are given in encoder ticks per PID interval
00279         '''
00280         return self.execute_ack('m %d %d' %(right, left))
00281     
00282     def drive_m_per_s(self, right, left):
00283         ''' Set the motor speeds in meters per second.
00284         '''
00285         left_revs_per_second = float(left) / (self.wheel_diameter * PI)
00286         right_revs_per_second = float(right) / (self.wheel_diameter * PI)
00287 
00288         left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
00289         right_ticks_per_loop  = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
00290 
00291         self.drive(right_ticks_per_loop , left_ticks_per_loop )
00292         
00293     def stop(self):
00294         ''' Stop both motors.
00295         '''
00296         self.drive(0, 0)
00297             
00298     def analog_read(self, pin):
00299         return self.execute('a %d' %pin)
00300     
00301     def analog_write(self, pin, value):
00302         return self.execute_ack('x %d %d' %(pin, value))
00303     
00304     def digital_read(self, pin):
00305         return self.execute('d %d' %pin)
00306     
00307     def digital_write(self, pin, value):
00308         return self.execute_ack('w %d %d' %(pin, value))
00309     
00310     def pin_mode(self, pin, mode):
00311         return self.execute_ack('c %d %d' %(pin, mode))
00312 
00313     def servo_write(self, id, pos):
00314         ''' Usage: servo_write(id, pos)
00315             Position is given in radians and converted to degrees before sending
00316         '''        
00317         return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
00318     
00319     def servo_read(self, id):
00320         ''' Usage: servo_read(id)
00321             The returned position is converted from degrees to radians
00322         '''        
00323         return radians(self.execute('t %d' %id))
00324 
00325     def ping(self, pin):
00326         ''' The srf05/Ping command queries an SRF05/Ping sonar sensor
00327             connected to the General Purpose I/O line pinId for a distance,
00328             and returns the range in cm.  Sonar distance resolution is integer based.
00329         '''
00330         return self.execute('p %d' %pin);
00331     
00332 #    def get_maxez1(self, triggerPin, outputPin):
00333 #        ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar
00334 #            sensor connected to the General Purpose I/O lines, triggerPin, and
00335 #            outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE
00336 #            SURE there's nothing directly in front of the MaxSonar-EZ1 upon
00337 #            power up, otherwise it wont range correctly for object less than 6
00338 #            inches away! The sensor reading defaults to use English units
00339 #            (inches). The sensor distance resolution is integer based. Also, the
00340 #            maxsonar trigger pin is RX, and the echo pin is PW.
00341 #        '''
00342 #        return self.execute('z %d %d' %(triggerPin, outputPin)) 
00343  
00344 
00345 """ Basic test for connectivity """
00346 if __name__ == "__main__":
00347     if os.name == "posix":
00348         portName = "/dev/ttyACM0"
00349     else:
00350         portName = "COM43" # Windows style COM port.
00351         
00352     baudRate = 57600
00353 
00354     myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
00355     myArduino.connect()
00356      
00357     print "Sleeping for 1 second..."
00358     time.sleep(1)   
00359     
00360     print "Reading on analog port 0", myArduino.analog_read(0)
00361     print "Reading on digital port 0", myArduino.digital_read(0)
00362     print "Blinking the LED 3 times"
00363     for i in range(3):
00364         myArduino.digital_write(13, 1)
00365         time.sleep(1.0)
00366     #print "Current encoder counts", myArduino.encoders()
00367     
00368     print "Connection test successful.",
00369     
00370     myArduino.stop()
00371     myArduino.close()
00372     
00373     print "Shutting down Arduino."
00374     


ros_arduino_python
Author(s): Patrick Goebel
autogenerated on Wed Aug 26 2015 15:57:04