提交可用IIC，除红外传感器和蜂鸣器

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

@@ -82,6 +82,12 @@ class Arduino:
         try:
             rospy.loginfo("Connecting to Arduino on IIC addr:"+str(self.i2c_slave_addr))
             test = self.i2c_get_baud()
+            #test2 = self.getCurrentValue()
+            #test3 = self.getBatPercent()
+            #test4 = self.detect_distance()#wrong
+            #test5 = self.analog_read(1)
+            #test6 = self.digital_read(10)
+            #print test4
             if test != self.baudrate:
                 time.sleep(1)
                 test = self.i2c_get_baud()
@@ -288,15 +294,19 @@ class Arduino:
         ''' 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'))
+            #rospy.loginfo("i2c_slave_addr:"+str(self.i2c_slave_addr))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('b'))
+            #if the program ends here, maybe I/O error, please check $ sudo i2cdetect -y -a 1
+            #if 0x08 does not exist all the time or the refresh is so slow, please reset arduino DUE
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
             time.sleep(0.1)
-            values = self.i2c_bus.read_i2c_block_data(self.i2c_addr, 0, 5)
+            values = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 5)
             values = map(chr, values)
             ret = ''.join(values)
             #print "ret:"+ret
             return int(ret)
         except:
+            rospy.loginfo("get baud failed !")
             return None
 
     def get_encoder_counts(self):
@@ -306,6 +316,7 @@ class Arduino:
             raise SerialException
             return None
         else:
+            print values
             return values
 
     def i2c_get_encoder_counts(self):
@@ -316,12 +327,19 @@ class Arduino:
         cmd = "\r"
 
         try:
-            self.i2c_bus.write_byte(self.i2c_addr, int(ord('e')))
-            self.i2c_bus.write_byte(self.i2c_addr, ord('\r'))
-
-            result_string = ''.join([chr(e) for e in self.i2c_bus.read_i2c_block_data(self.i2c_addr, 0x06)])
-            result_flag = result_string.index('\r')
-            values=[int(e) for e in ''.join(result_string[:result_flag]).split('')]
+            self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('e')))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+
+            result_array = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+            #There are result_flag valid bytes.
+            result_flag = result_array.index(120) 
+            #rospy.loginfo("Valid bytes: "+str(result_flag))
+            #extract result_flag bytes as strings
+            result_string=''.join([chr(e) for e in self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08, result_flag)])
+            #extract by space
+            values=[int(s) for s in result_string.split(" ")]
+            #rospy.loginfo("encoder counts now: "+ str(values))
+            return values
         except:
             rospy.logerr(sys.exe_info())
             traceback.print_exc(file=sys.stdout)
@@ -342,22 +360,37 @@ class Arduino:
         ''' 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'))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('r'))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+            rospy.loginfo("Reset encoders sucessfully")
         except:
             return None
 
     def gripper_control(self, operation):
-        ''' control gripper: 1-open gripper down 2:stop 3:close gripper up 4:open gripper 5:close gripper
+        ''' control gripper: 1:down 2:stop 3:up
         '''
         #rospy.loginfo("gripper operation ID:" + str(operation))
-        return self.execute_ack('z %d' %(operation))
-
+        if self.is_use_serial:
+            return self.execute_ack('z %d' %(operation))
+        else: #control gripper up/down by IIC
+            cmd = (' %d\r' %(operation))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('z'), [ord(c) for c in cmd])
+            except:
+                return None
+        
     def gripper_angle(self, angle):
         ''' control gripper angle
         '''
         #rospy.loginfo("gripper operation ID:" + str(operation))
-        return self.execute_ack('s 1 %d' %(angle))
+        if self.is_use_serial:
+            return self.execute_ack('s 1 %d' %(angle))
+        else:  #control gripper open/close by IIC
+            cmd = (' 1 %d\r' %(angle))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('s'), [ord(c) for c in cmd])
+            except:
+                return None
 
     def drive(self, AWheel, BWheel, CWheel):
         ''' Speeds are given in encoder ticks per PID interval
@@ -371,7 +404,7 @@ class Arduino:
         #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])
+            self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('m'), [ord(c) for c in cmd])
         except:
             return None
 
@@ -383,40 +416,145 @@ class Arduino:
     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])
+            self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('m'), [ord(c) for c in cmd])
         except:
             return None
 
-    def analog_read(self, pin):
-        return self.execute('a %d' %pin)
+    def analog_read(self, pin):        
+        if self.is_use_serial:
+            return self.execute('a %d' %pin)
+        else:
+            cmd = (' %d\r' %(pin))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('a'), [ord(c) for c in cmd])
+                pin = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 3)
+                pin = map(chr, pin)
+                ret = ''.join(pin)
+                #print "ret:"+ret
+                return int(ret)
+            except:
+                return None
 
-    def analog_write(self, pin, value):
-        return self.execute_ack('x %d %d' %(pin, value))
+    def analog_write(self, pin, value):      
+        if self.is_use_serial:
+            return self.execute_ack('x %d %d' %(pin, value))
+        else:
+            cmd = (' %d %d\r' %(pin, value))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('x'), [ord(c) for c in cmd])
+            except:
+                return None
 
-    def digital_read(self, pin):
-        return self.execute('d %d' %pin)
+    def digital_read(self, pin):        
+        if self.is_use_serial:
+            return self.execute('d %d' %pin)
+        else:
+            cmd = (' %d\r' %(pin))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('d'), [ord(c) for c in cmd])
+                pin = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 2)
+                pin = map(chr, pin)
+                print pin
+                ret = ''.join(pin)
+                print "ret:"+ret
+                return int(ret)
+            except:
+                return None
 
-    def digital_write(self, pin, value):
-        return self.execute_ack('w %d %d' %(pin, value))
+    def digital_write(self, pin, value):      
+        if self.is_use_serial:
+            return self.execute_ack('w %d %d' %(pin, value))
+        else:
+            cmd = (' %d %d\r' %(pin, value))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('w'), [ord(c) for c in cmd])
+            except:
+                return None
 
-    def pin_mode(self, pin, mode):
-        return self.execute_ack('c %d %d' %(pin, mode))
+    def pin_mode(self, pin, mode):        
+        if self.is_use_serial:
+            return self.execute_ack('c %d %d' %(pin, mode))
+        else:
+            cmd = (' %d %d\r' %(pin, mode))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('c'), [ord(c) for c in cmd])
+            except:
+                return None
 
-    def beep_ring(self, value):
-        return self.execute_ack('p %d' %value)
+    def beep_ring(self, value):           
+#        if self.is_use_serial:
+            return self.execute_ack('p %d' %value)
+#        else:
+#            cmd = (' %d\r' %(value))
+#            try:
+#                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('p'), [ord(c) for c in cmd])
+#            except:
+#                return None
 
     def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
+#        if self.is_use_serial:
         return self.execute_array('h')
+#        else: 
+#            ch = ''
+#            values = ''
+#            cnt = 0
+#            cmd = "\r"
+#            try:
+#                self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('p')))
+#                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+#                rospy.sleep(2)
+#                print 111111111111111
+#                result_array = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+#                result_array = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 20)
+#                print 222222222
+#                print self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+#                #There are result_flag valid bytes.
+#                result_flag = result_array.index(120) 
+#                #rospy.loginfo("Valid bytes: "+str(result_flag))
+#                #extract result_flag bytes as strings
+#                result_string=''.join([chr(e) for e in self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08, result_flag)])
+#                #extract by space
+#                values=[int(s) for s in result_string.split(" ")]
+#                #rospy.loginfo("encoder counts now: "+ str(values))
+#                return values
+#            except:
+#                return None
+
 
     def getBatPercent(self): #获取电池的剩余电量百分比
-        percent = self.execute('g')
-        #rospy.loginfo("bat percent:" + str(percent))
-        return percent
+        
+        if self.is_use_serial:
+            percent = self.execute('g')
+            #rospy.loginfo("bat percent:" + str(percent))
+            return percent
+        else: 
+            try:
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('g'))
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+                percent = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 3)
+                percent = map(chr, percent)
+                ret = ''.join(percent)
+                #print "ret:"+ret
+                return int(ret)
+            except:
+                return None
 
     def getCurrentValue(self): #获取电流值
-        currentvalue = self.execute('f')
-        #rospy.loginfo("get current value:" + str(currentvalue))
-        return currentvalue
+        if self.is_use_serial:
+            currentvalue = self.execute('f')
+            #rospy.loginfo("get current value:" + str(currentvalue))
+            return currentvalue
+        else: 
+            try:
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('f'))
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+                currentvalue = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 5)
+                currentvalue = map(chr, currentvalue)
+                ret = ''.join(currentvalue)
+                #print "ret:"+ret
+                return float(ret)
+            except:
+                return None
 
     def light_show(self, value):
         return self.execute_ack('l %d' %value)