提交通过IIC红外测距

src/robot_calibration/src/pid_dynamic.cpp

@@ -51,6 +51,7 @@ void dynamic_callback(dynamic_tutorials::TutorialsConfig &config)
         //发布pid参数
         if (client.call(srv))
 	{
+            ROS_INFO("Call service DynamicPID sucessfully.");
 	}
 	else
 	{

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

@@ -87,7 +87,8 @@ class Arduino:
             #test4 = self.detect_distance()#wrong
             #test5 = self.analog_read(1)
             #test6 = self.digital_read(10)
-            #print test4
+            #test7 = self.i2c_get_pidin()
+            #print test7
             if test != self.baudrate:
                 time.sleep(1)
                 test = self.i2c_get_baud()
@@ -316,15 +317,10 @@ class Arduino:
             raise SerialException
             return None
         else:
-            #print values
             return values
 
     def i2c_get_encoder_counts(self):
         #print "IIC Get Encoder count !"
-        ch = ''
-        values = ''
-        cnt = 0
-        cmd = "\r"
 
         try:
             self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('e')))
@@ -336,20 +332,20 @@ class Arduino:
             #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
+            #extract encoder_counts by space
             values=[int(s) for s in result_string.split(" ")]
-            #rospy.loginfo("encoder counts now: "+ str(values))
-            return values
+            if len(values) != 3:
+                rospy.logwarn("Encoder count was not 3")
+                return None
+            else:
+                #rospy.loginfo("Encoder counts now: "+ str(values))
+                return values
         except:
             rospy.logerr(sys.exe_info())
             traceback.print_exc(file=sys.stdout)
             return None
 
-        if len(values) != 3:
-            rospy.logwarn("Encoder count was not 3")
-            return None
-        else:
-            return values
+
 
     def reset_encoders(self):
         ''' Reset the encoder counts to 0 by serial port
@@ -372,7 +368,9 @@ class Arduino:
         #rospy.loginfo("gripper operation ID:" + str(operation))
         if self.is_use_serial:
             return self.execute_ack('z %d' %(operation))
-        else: #control gripper up/down by IIC
+        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])
@@ -385,7 +383,9 @@ class Arduino:
         #rospy.loginfo("gripper operation ID:" + str(operation))
         if self.is_use_serial:
             return self.execute_ack('s 1 %d' %(angle))
-        else:  #control gripper open/close by IIC
+        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])
@@ -492,40 +492,38 @@ class Arduino:
 #                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
+        if self.is_use_serial:
+            return self.execute_array('h')
+        else:
+            try:
+                self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('h')))
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
 
+                result_array2 = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+                #There are result_flag valid bytes.
+                result_flag2 = result_array2.index(120)
+                #rospy.loginfo("Valid bytes: "+str(result_flag2))
+                #extract result_flag bytes as strings
+                result_string2=''.join([chr(ch) for ch in result_array2[0:(result_flag2-1)]])
+                #extract infrared_counts by space
+                values=[float(s) for s in result_string2.split(" ")]
+                #print values
+                if len(values) != 3:
+                    rospy.logwarn("InfraredEncoder count was not 3")
+                    return None
+                else:
+                    #rospy.loginfo("Infrared counts now: "+ str(values))
+                    return values
+            except:
+                return None
 
-    def getBatPercent(self): #获取电池的剩余电量百分比
 
+    def getBatPercent(self):
+        '''get the remaining power percentage
+        '''
         if self.is_use_serial:
             percent = self.execute('g')
-            #rospy.loginfo("bat percent:" + str(percent))
+            #rospy.loginfo("Bat percent:" + str(percent))
             return percent
         else:
             try:
@@ -539,10 +537,12 @@ class Arduino:
             except:
                 return None
 
-    def getCurrentValue(self): #获取电流值
+    def getCurrentValue(self):
+        '''get current value
+        '''
         if self.is_use_serial:
             currentvalue = self.execute('f')
-            #rospy.loginfo("get current value:" + str(currentvalue))
+            #rospy.loginfo("Current value:" + str(currentvalue))
             return currentvalue
         else:
             try:
@@ -556,9 +556,6 @@ class Arduino:
             except:
                 return None
 
-    def light_show(self, value):
-        return self.execute_ack('l %d' %value)
-
     def get_pidin(self):
         values = self.execute_array('i')
         if len(values) != 3:
@@ -570,13 +567,36 @@ class Arduino:
 
     def i2c_get_pidin(self):
         rospy.loginfo("IIC Get Pidin !")
-        #values = self.execute_array('i')
-        #if len(values) != 3:
-        #    print "pidin was not 3"
-        #    raise SerialException
-        #    return None
-        #else:
-        #    return values
+        ch = ''
+        values = ''
+        cnt = 0
+        cmd = "\r"
+
+        try:
+            self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('i')))
+            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)
+            print result_array
+            #There are result_flag valid bytes.
+            result_flag = result_array.index(120)
+            print result_flag
+            #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)])
+            print result_string
+            #extract Pidin_counts by space
+            values=[int(s) for s in result_string.split(" ")]
+            if len(values) != 3:
+                rospy.logwarn("Pidin was not 3")
+                return None
+            else:
+                #rospy.loginfo("Pidin now: "+ str(values))
+                return values
+        except:
+            rospy.logerr(sys.exe_info())
+            traceback.print_exc(file=sys.stdout)
+            return None
 
     def get_pidout(self):
         values = self.execute_array('o')
@@ -589,13 +609,36 @@ class Arduino:
 
     def i2c_get_pidout(self):
         rospy.loginfo("IIC Get Pidout !")
-        #values = self.execute_array('o')
-        #if len(values) != 3:
-        #    print "pidout was not 3"
-        #    raise SerialException
-        #    return None
-        #else:
-        #    return values
+        ch = ''
+        values = ''
+        cnt = 0
+        cmd = "\r"
+
+        try:
+            self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('o')))
+            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)
+            print result_array
+            #There are result_flag valid bytes.
+            result_flag = result_array.index(120)
+            print result_flag
+            #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)])
+            print result_string
+            #extract Pidout_counts by space
+            values=[int(s) for s in result_string.split(" ")]
+            if len(values) != 3:
+                rospy.logwarn("Pidout was not 3")
+                return None
+            else:
+                #rospy.loginfo("Pidout now: "+ str(values))
+                return values
+        except:
+            rospy.logerr(sys.exe_info())
+            traceback.print_exc(file=sys.stdout)
+            return None
 
 """ Basic test for connectivity by serial port or IIC bus"""
 if __name__ == "__main__":