提交IIC蜂鸣器

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

@@ -95,7 +95,13 @@ class Arduino:
                 if test != self.baudrate:
                     raise Exception
 
-            self.beep_ring(1)
+            self.beep_ring(2)
+            rospy.sleep(0.1)
+            self.beep_ring(3)
+            rospy.sleep(0.05)
+            self.beep_ring(2)
+            rospy.sleep(0.1)
+            self.beep_ring(3)
             rospy.loginfo("Arduino is connected by IIC !")
         except Exception:
             rospy.logerr(sys.exc_info())
@@ -111,7 +117,9 @@ class Arduino:
     def close(self):
         ''' Close the serial port or i2c bus connect.
         '''
-        self.beep_ring(0)
+        self.beep_ring(2)
+        rospy.sleep(0.5)
+        self.beep_ring(3)
         if self.is_use_serial:
             self.serial_port.close()
         else:
@@ -482,21 +490,21 @@ class Arduino:
                 return None
 
     def beep_ring(self, value):
-#        if self.is_use_serial:
+        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
+        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:
             try:
-                self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('h')))
+                self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('j')))
                 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)

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -51,6 +51,7 @@ class Sensor(object):
             if self.direction == "input":
                 try:
                     self.value = self.read_value()
+                    #rospy.loginfo("read value: "+str(self.value))
                 except:
                     rospy.logerr("Sensor read value error !")
                     return

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

@@ -173,11 +173,11 @@ class BaseController:
                     aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts()
                 else:
                     aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.i2c_get_encoder_counts()
+                    #rospy.loginfo("get encoder counts: "+str(self.arduino.i2c_get_encoder_counts()))
             except:
                 self.bad_encoder_count += 1
                 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
                 return
-            #rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))
 
             # Calculate odometry
             dist_A = (aWheel_enc - self.enc_A)/self.ticks_per_meter