添加更新注释信息;电池电量改为每100s获取一次

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -8,6 +8,7 @@
 # Author: corvin
 # History:
 #   20200330:init this file.
+#   20201119:新增pid调速走直线参数.
 
 is_use_serial: True
 
@@ -63,7 +64,7 @@ CWheel_Ko: 10
 # Sensor definitions (case sensitive!):
 sensors: {
   GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5, direction: input},
-  batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1, direction: input},
+  batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.01, direction: input},
 }
 
 # Yaw Service

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

@@ -9,6 +9,7 @@
     20200329:增加获取电池剩余电量百分比函数.
     20200412:增加发布红外测距信息的服务.
     20200423:增加获取电流传感器信息
+    20201119:修复i2c获取pid,编码器,电量问题.
 """
 from serial.serialutil import SerialException
 import thread, smbus, rospy, time, os
@@ -332,9 +333,11 @@ class Arduino:
             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)
+            if result_flag == 0:
+                result_array = result_array[1:(len(result_array)-1)]
+                result_flag = result_array.index(120)
+            result_string=''.join([chr(ch) for ch in result_array[0:(result_flag-1)]])
             #rospy.loginfo("Valid bytes: "+str(result_flag))
-            #extract result_flag bytes as strings
-            result_string=''.join([chr(ec) for ec in result_array[0:(result_flag-1)]])
             #extract encoder_counts by space
             values=[int(s) for s in result_string.split(" ")]
             if len(values) != 3:
@@ -531,6 +534,8 @@ class Arduino:
                 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)
+                if percent[0] == 120:
+                    percent.remove(percent[0])
                 percent = map(chr, percent)
                 ret = ''.join(percent)
                 #print "ret:"+ret

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

@@ -6,8 +6,9 @@
   Description:  A base controller class for the Arduino DUE microcontroller
   Author: corvin
   History:
-      20200704:当超过给下位机发送控制移动命令时间后,不需要再发送pwm控制0的命令.
+    20200704:当超过给下位机发送控制移动命令时间后,不需要再发送pwm控制0的命令.
         因为在arduino代码会根据超时自己会执行pwm清零停止电机移动的命令.
+    20201119:新增pid调速走直线,但横向位移仍未解决.
 """
 import os
 import rospy
@@ -483,7 +484,7 @@ class BaseController:
                 err_yaw_data = self.now_yaw_data - self.last_yaw_data
                 if abs(err_yaw_data) > self.correction_forward_err_yaw_data:
                     th = self.pid_controller.update(err_yaw_data)
-                    print("err_yaw_data is ",err_yaw_data,"th is ",th)
+                    #print("err_yaw_data is ",err_yaw_data,"th is ",th)
         else:
             self.correction_yaw_flag = 0