在ros_arduino_python中增加可以iic与arduino通信的代码

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -3,8 +3,12 @@
 # For a wireless connection like XBee, the port is typically
 # /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.
 
-port: /dev/ttyAMA0
-baud: 57600
+is_use_serial: True
+
+serial_port: /dev/ttyACM0
+serial_baud: 57600
+i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
+
 timeout: 0.5
 
 rate: 25
@@ -21,9 +25,9 @@ odom_name: odom
 
 # === Robot drivetrain parameters
 wheel_diameter: 0.058
-wheel_track: 0.126   #L value
+wheel_track: 0.126     #L value
 encoder_resolution: 11 #12V DC motors
-gear_reduction: 103
+gear_reduction: 46     #empty payload rpm 130r/m
 debugPID: False
 linear_scale_correction: 1.00 
 angular_scale_correction: 1.00

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -2,21 +2,6 @@
 
 """
     A ROS Node for the Arduino microcontroller
-
-    Created for the Pi Robot Project: http://www.pirobot.org
-    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
-
-    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
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details at:
-
-    http://www.gnu.org/licenses/gpl.html
 """
 
 import os
@@ -38,9 +23,12 @@ class ArduinoROS():
 
         # Cleanup when termniating the node
         rospy.on_shutdown(self.shutdown)
+        
+        self.is_use_serial = rospy.get_param("~is_use_serial", True)
 
-        self.port = rospy.get_param("~port", "/dev/ttyAMA0")
-        self.baud = int(rospy.get_param("~baud", 57600))
+        self.serial_port = rospy.get_param("~serial_port", "/dev/ttyACM0")
+        self.serial_baud = int(rospy.get_param("~serial_baud", 57600))
+        self.i2c_slave_addr = rospy.get_param("~i2c_slave_addr", 8)
         self.timeout    = rospy.get_param("~timeout", 0.7)
         self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
 
@@ -88,11 +76,11 @@ class ArduinoROS():
         rospy.Service('~light_show', LightShow, self.LightShowHandler)
 
         # Initialize the controlller
-        self.controller = Arduino(self.port, self.baud, self.timeout)
+        self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_slave_addr, self.timeout)
 
         # Make the connection
         self.controller.connect()
-        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
+        #rospy.loginfo("Connected to Arduino on port " + self.serial_port + " at " + str(self.serial_baud) + " serial baud !")
 
         # Reserve a thread lock
         mutex = thread.allocate_lock()
@@ -130,7 +118,7 @@ class ArduinoROS():
                 rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
 
         # Initialize the base controller
-        self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller")
+        self.myBaseController = BaseController(self.is_use_serial, self.controller, self.base_frame, self.name + "_base_controller")
 
         # Start polling the sensors and base controller
         while not rospy.is_shutdown():
@@ -216,4 +204,7 @@ if __name__ == '__main__':
     except SerialException:
         rospy.logerr("Serial exception trying to open port.")
         os._exit(0)
+    else:
+        rospy.logerr("Get other unknown error !")
+        os._exit(0)
 

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -37,13 +37,19 @@ class Arduino:
     N_ANALOG_PORTS  = 10
     N_DIGITAL_PORTS = 54
 
-    def __init__(self, port="/dev/ttyAMA0", baudrate=57600, timeout=0.5):
+    def __init__(self, is_use_serial, serial_port="/dev/ttyACM0", baudrate=57600, i2c_slave_addr=8, timeout=0.5):
         self.PID_RATE = 40 # Do not change this!  It is a fixed property of the Arduino PID controller.
         self.PID_INTERVAL = 25
 
-        self.port     = port
+        self.is_use_serial = is_use_serial
+        self.serial_port   = serial_port
         self.baudrate = baudrate
+        
+        self.i2c_bus = smbus.SMBus(1)
+        self.i2c_slave_addr = i2c_slave_addr
+
         self.timeout  = timeout
+
         self.encoder_count    = 0
         self.writeTimeout     = timeout
         self.interCharTimeout = timeout/30.
@@ -58,9 +64,15 @@ class Arduino:
         self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
 
     def connect(self):
+        if self.is_use_serial:
+            self.serial_connect()
+        else:
+            self.i2c_connect()
+
+    def serial_connect(self):
         try:
-            print "Connecting to Arduino on port", self.port, "..."
-            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
+            print "Connecting to Arduino on port", self.serial_port, "..."
+            self.serial_port = Serial(port=self.serial_port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
             # The next line is necessary to give the firmware time to wake up.
             time.sleep(1)
             test = self.get_baud()
@@ -73,7 +85,7 @@ class Arduino:
 
             self.beep_ring(1)
             print "Connected at", self.baudrate
-            print "Arduino is ready."
+            print "Arduino is ready !"
         except SerialException:
             print "Serial Exception:"
             print sys.exc_info()
@@ -82,22 +94,46 @@ class Arduino:
             print "Cannot connect to Arduino!"
             os._exit(1)
 
+    def i2c_connect(self):
+        try:
+            
+            print "Connecting to Arduino on IIC addr ", self.i2c_slave_addr, "..."
+            test = self.i2c_get_baud()
+            if test != self.baudrate:
+                time.sleep(1)
+                print "Connecting ..."
+                test = self.i2c_get_baud()
+                if test != self.baudrate:
+                    raise Exception
+
+            self.beep_ring(1)
+            print "Connected at", self.baudrate
+            print "Arduino is ready !"
+        except Exception:
+            print sys.exc_info()
+            print "Traceback follows:"
+            traceback.print_exc(file=sys.stdout)
+            print "Cannot connect to Arduino IIC slave addr !"
+            os._exit(1)
+
+
     def open(self):
         ''' Open the serial port.
         '''
-        self.port.open()
+        self.serial_port.open()
 
     def close(self):
         ''' Close the serial port.
         '''
         self.beep_ring(0)
-        self.port.close()
+        self.serial_port.close()
+        self.i2c_bus.close()
 
     def send(self, cmd):
         ''' This command should not be used on its own: it is called by the execute commands
             below in a thread safe manner.
         '''
-        self.port.write(cmd + '\r')
+        self.serial_port.write(cmd + '\r')
 
     def recv(self, timeout=0.5):
         timeout = min(timeout, self.timeout)
@@ -109,7 +145,7 @@ class Arduino:
         value = ''
         attempts = 0
         while c != '\r':
-            c = self.port.read(1)
+            c = self.serial_port.read(1)
             value += c
             attempts += 1
             if attempts * self.interCharTimeout > timeout:
@@ -151,7 +187,7 @@ class Arduino:
         self.mutex.acquire()
 
         try:
-            self.port.flushInput()
+            self.serial_port.flushInput()
         except:
             pass
 
@@ -159,12 +195,12 @@ class Arduino:
         attempts = 0
 
         try:
-            self.port.write(cmd + '\r')
+            self.serial_port.write(cmd + '\r')
             value = self.recv(self.timeout)
             while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
                 try:
-                    self.port.flushInput()
-                    self.port.write(cmd + '\r')
+                    self.serial_port.flushInput()
+                    self.serial_port.write(cmd + '\r')
                     value = self.recv(self.timeout)
                 except:
                     print "Exception executing command: " + cmd
@@ -182,19 +218,19 @@ class Arduino:
         '''
         self.mutex.acquire()
         try:
-            self.port.flushInput()
+            self.serial_port.flushInput()
         except:
             pass
 
         ntries   = 1
         attempts = 0
         try:
-            self.port.write(cmd + '\r')
+            self.serial_port.write(cmd + '\r')
             values = self.recv_array()
             while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
                 try:
-                    self.port.flushInput()
-                    self.port.write(cmd + '\r')
+                    self.serial_port.flushInput()
+                    self.serial_port.write(cmd + '\r')
                     values = self.recv_array()
                 except:
                     print("Exception executing command: " + cmd)
@@ -219,7 +255,7 @@ class Arduino:
         self.mutex.acquire()
 
         try:
-            self.port.flushInput()
+            self.serial_port.flushInput()
         except:
             pass
 
@@ -227,12 +263,12 @@ class Arduino:
         attempts = 0
 
         try:
-            self.port.write(cmd + '\r')
+            self.serial_port.write(cmd + '\r')
             ack = self.recv(self.timeout)
             while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
                 try:
-                    self.port.flushInput()
-                    self.port.write(cmd + '\r')
+                    self.serial_port.flushInput()
+                    self.serial_port.write(cmd + '\r')
                     ack = self.recv(self.timeout)
                 except:
                     print "Exception executing command: " + cmd
@@ -255,6 +291,11 @@ class Arduino:
         cmd = 'u '+str(AWheel_Kp)+':'+str(AWheel_Kd)+':'+str(AWheel_Ki)+':'+str(AWheel_Ko)+':'+str(BWheel_Kp)+':'+str(BWheel_Kd)+':'+str(BWheel_Ki)+':'+str(BWheel_Ko)+':'+str(CWheel_Kp)+':'+str(CWheel_Kd)+':'+str(CWheel_Ki)+':'+str(CWheel_Ko)
         self.execute_ack(cmd)
 
+    def i2c_update_pid(self, AWheel_Kp, AWheel_Kd, AWheel_Ki, AWheel_Ko,
+                         BWheel_Kp, BWheel_Kd, BWheel_Ki, BWheel_Ko,
+                         CWheel_Kp, CWheel_Kd, CWheel_Ki, CWheel_Ko ):
+        print "Updating PID parameters by IIC"        
+
     def get_baud(self):
         ''' Get the current baud rate on the serial port.
         '''
@@ -263,6 +304,21 @@ class Arduino:
         except:
             return None
 
+    def i2c_get_baud(self):
+        ''' Get the current baud rate on the arduino IIC addr.
+        '''
+        try:
+            self.i2c_bus.write_byte(self.i2c_addr, ord('b'))
+            self.i2c_bus.write_byte(self.i2c_addr, ord('\r'))
+            time.sleep(0.1)
+            values = self.i2c_bus.read_i2c_block_data(self.i2c_addr, 0, 5)
+            values = map(chr, values)
+            ret = ''.join(values)
+            #print "ret:"+ret
+            return int(ret)
+        except:
+            return None
+
     def get_encoder_counts(self):
         values = self.execute_array('e')
         if len(values) != 3:
@@ -272,16 +328,44 @@ class Arduino:
         else:
             return values
 
+    def i2c_get_encoder_counts(self):
+        #values = self.execute_array('e')
+        #if len(values) != 3:
+        #    print "Encoder count was not 3"
+        #    raise SerialException
+        #    return None
+        #else:
+        #    return values
+
     def reset_encoders(self):
-        ''' Reset the encoder counts to 0
+        ''' Reset the encoder counts to 0 by serial port
         '''
         return self.execute_ack('r')
 
+    def i2c_reset_encoders(self):
+        ''' Reset the encoder counts to 0 by IIC
+        '''
+        try:
+            self.i2c_bus.write_byte(self.i2c_addr, ord('r'))
+            self.i2c_bus.write_byte(self.i2c_addr, ord('\r'))
+        except:
+            return None
+
     def drive(self, AWheel, BWheel, CWheel):
         ''' Speeds are given in encoder ticks per PID interval
         '''
         #rospy.loginfo("drive() A:" + str(AWheel) + ",B:" + str(BWheel) + ",C:" + str(CWheel))
         return self.execute_ack('m %d %d %d' %(AWheel, BWheel, CWheel))
+    
+    def i2c_drive(self, AWheel, BWheel, CWheel):
+        ''' Speeds are given in encoder ticks per PID interval
+        '''
+        #rospy.loginfo("drive() A:" + str(AWheel) + ",B:" + str(BWheel) + ",C:" + str(CWheel))
+        cmd = (' %d %d %d\r' %(AWheel, BWheel, CWheel))
+        try:
+            self.i2c_bus.write_i2c_block_data(self.i2c_addr, ord('m'), [ord(c) for c in cmd])
+        except:
+            return None
 
     def drive_m_per_s(self, AWheel, BWheel, CWheel):
         ''' Set the motor speeds in meters per second.
@@ -301,6 +385,13 @@ class Arduino:
         '''
         self.drive(0, 0, 0)
 
+    def i2c_stop(self):
+        cmd = (' 0 0 0\r')
+        try:
+            self.i2c_bus.write_i2c_block_data(self.i2c_addr, ord('m'), [ord(c) for c in cmd])
+        except:
+            return None
+
     def analog_read(self, pin):
         return self.execute('a %d' %pin)
 
@@ -340,6 +431,15 @@ class Arduino:
         else:
             return values
 
+    def i2c_get_pidin(self):
+        #values = self.execute_array('i')
+        #if len(values) != 3:
+        #    print "pidin was not 3"
+        #    raise SerialException
+        #    return None
+        #else:
+        #    return values
+
     def get_pidout(self):
         values = self.execute_array('o')
         if len(values) != 3:
@@ -349,14 +449,18 @@ class Arduino:
         else:
             return values
 
+    def i2c_get_pidout(self):
+        #values = self.execute_array('o')
+        #if len(values) != 3:
+        #    print "pidout was not 3"
+        #    raise SerialException
+        #    return None
+        #else:
+        #    return values
 
 """ Basic test for connectivity """
 if __name__ == "__main__":
-    if os.name == "posix":
-        portName = "/dev/ttyACM0"
-    else:
-        portName = "COM43" # Windows style COM port.
-
+    portName = "/dev/ttyACM0"
     baudRate = 57600
 
     myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -2,23 +2,6 @@
 
 """
     A base controller class for the Arduino microcontroller
-
-    Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code.
-
-    Created for the Pi Robot Project: http://www.pirobot.org
-    Copyright (c) 2010 Patrick Goebel.  All rights reserved.
-
-    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
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details at:
-
-    http://www.gnu.org/licenses
 """
 import os
 import rospy
@@ -33,7 +16,8 @@ from std_msgs.msg import Int32
 
 """ Class to receive Twist commands and publish wheel Odometry data """
 class BaseController:
-    def __init__(self, arduino, base_frame, name="base_controllers_node"):
+    def __init__(self, is_use_serial, arduino, base_frame, name="base_controllers_node"):
+        self.use_serial = is_use_serial
         self.arduino    = arduino
         self.name       = name
         self.base_frame = base_frame
@@ -108,7 +92,10 @@ class BaseController:
         rospy.Subscriber(self.cmd_topic, Twist, self.cmdVelCallback)
 
         # Clear any old odometry info
-        self.arduino.reset_encoders()
+        if self.use_serial:
+            self.arduino.reset_encoders()
+        else:
+            self.arduino.i2c_reset_encoders()
 
         # Set up the wheel odometry broadcaster
         self.odomPub = rospy.Publisher(self.odom_name, Odometry, queue_size=10)
@@ -162,10 +149,14 @@ class BaseController:
         self.CWheel_Ki = pid_params['CWheel_Ki']
         self.CWheel_Ko = pid_params['CWheel_Ko']
 
-        self.arduino.update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
-                               self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,
-                               self.CWheel_Kp, self.CWheel_Kd, self.CWheel_Ki, self.CWheel_Ko,)
-
+        if self.use_serial:
+            self.arduino.update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
+                                    self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,
+                                    self.CWheel_Kp, self.CWheel_Kd, self.CWheel_Ki, self.CWheel_Ko,)
+        else:
+            self.arduino.i2c_update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
+                                        self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,
+                                        self.CWheel_Kp, self.CWheel_Kd, self.CWheel_Ki, self.CWheel_Ko,)
 
     # main loop, always run
     def poll(self):
@@ -174,12 +165,15 @@ class BaseController:
         time_now = rospy.Time.now()
         if time_now > self.t_next:
             #rospy.logwarn("Voltage: %f, Current: %f", float(vol), float(current))
-            vol = self.arduino.detect_voltage()
-            current = self.arduino.detect_current()
-            rospy.logwarn("Voltage: %f, Current: %f", vol, current)
+            #vol = self.arduino.detect_voltage()
+            #current = self.arduino.detect_current()
+            #rospy.logwarn("Voltage: %f, Current: %f", vol, current)
             # Read the encoders
             try:
-                aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts()
+                if self.use_serial:
+                    aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts()
+                else:
+                    aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.i2c_get_encoder_counts()
             except:
                 self.bad_encoder_count += 1
                 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
@@ -271,7 +265,10 @@ class BaseController:
     def send_debug_pid(self):
         if self.debugPID:
             try:
-                A_pidin, B_pidin, C_pidin = self.arduino.get_pidin()
+                if self.use_serial:
+                    A_pidin, B_pidin, C_pidin = self.arduino.get_pidin()
+                else:
+                    A_pidin, B_pidin, C_pidin = self.arduino.i2c_get_pidin()
                 self.AEncoderPub.publish(A_pidin)
                 self.BEncoderPub.publish(B_pidin)
                 self.CEncoderPub.publish(C_pidin)
@@ -280,7 +277,10 @@ class BaseController:
                 return
 
             try:
-                A_pidout, B_pidout, C_pidout = self.arduino.get_pidout()
+                if self.use_serial:
+                    A_pidout, B_pidout, C_pidout = self.arduino.get_pidout()
+                else:
+                    A_pidout, B_pidout, C_pidout = self.arduino.i2c_get_pidout()
                 self.APidoutPub.publish(A_pidout)
                 self.BPidoutPub.publish(B_pidout)
                 self.CPidoutPub.publish(C_pidout)
@@ -318,7 +318,11 @@ class BaseController:
               self.v_C = self.v_des_CWheel
 
         # send to arduino to drive motor
-        self.arduino.drive(self.v_A, self.v_B, self.v_C)
+        if self.use_serial:
+            self.arduino.drive(self.v_A, self.v_B, self.v_C)
+        else:
+            self.arduino.i2c_drive(self.v_A, self.v_B, self.v_C)
+
         if self.debugPID:
             self.AVelPub.publish(self.v_A)
             self.BVelPub.publish(self.v_B)
@@ -333,7 +337,10 @@ class BaseController:
         self.v_des_AWheel = 0
         self.v_des_BWheel = 0
         self.v_des_CWheel = 0
-        self.arduino.drive(0, 0, 0)
+        if self.use_serial:
+            self.arduino.drive(0, 0, 0)
+        else:
+            self.arduino.i2c_drive(0, 0, 0)
         #rospy.logwarn("stop mobilebase move!!!!")
 
     def cmdVelCallback(self, req):