增加使用IIC读取小车编码器的函数

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

@@ -315,14 +315,30 @@ class Arduino:
             return values
 
     def i2c_get_encoder_counts(self):
-        print "IIC Get Encoder count !"
-        #values = self.execute_array('e')
-        #if len(values) != 3:
-        #    print "Encoder count was not 3"
-        #    raise SerialException
-        #    return None
-        #else:
-        #    return values
+        #print "IIC Get Encoder count !"
+        ch = ''
+        values = ''
+        cnt = 0
+        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('')]
+        except:
+            print sys.exe_info()
+            traceback.print_exc(file=sys.stdout)
+            return None
+
+        if len(values) != 3:
+            print "Encoder count was not 3"
+            return None
+        else:
+            return values
+
 
     def reset_encoders(self):
         ''' Reset the encoder counts to 0 by serial port