在走直线pid调节加入限制范围使小车摆动幅度很小

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -81,4 +81,5 @@ yaw_kd: 0.11
 correction_yaw_flag_timeout: 2
 correction_forward_err_yaw_data: 0.012
 correction_forward_rate: 1
+correction_th_threshold: 0.1
 

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

@@ -108,6 +108,7 @@ class BaseController:
         self.correction_forward_err_yaw_data = rospy.get_param("~correction_forward_err_yaw_data",0.017)
         self.correction_forward_rate = rospy.get_param("~correction_forward_rate",1)
         self.correction_yaw_flag_timeout = rospy.get_param("~correction_yaw_flag_timeout",2)
+        self.correction_th_threshold = rospy.get_param("~correction_th_threshold",0.1)
         self.pid_controller = SimplePID(self.yaw_target, self.yaw_kp, self.yaw_ki, self.yaw_kd)
         self.correction_yaw_flag = 0
         self.last_yaw_data = 0
@@ -494,6 +495,10 @@ 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)
+                    if th > self.correction_th_threshold:
+                        th = self.correction_th_threshold
+                    if th < -self.correction_th_threshold:
+                        th = -self.correction_th_threshold
                     #print("err_yaw_data is ",err_yaw_data,"th is ",th)
         else:
             self.correction_yaw_flag = 0
@@ -512,4 +517,4 @@ class BaseController:
         #rospy.loginfo("cmdCallback(): A v_des:"+str(self.v_des_AWheel)+",B v_des:"+str(self.v_des_BWheel)+",C v_des:"+str(self.v_des_CWheel))
 
     def yawCallback(self,req):
-        self.yaw_topic_data = req.data
+        self.yaw_topic_data = req.data