停止时重置pid控制走直线参数

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -77,6 +77,7 @@ yaw_ki: 0.2
 yaw_kd: 0.11
 
 # Forward Yaw Error and Rate
+correction_yaw_flag_timeout: 2
 correction_forward_err_yaw_data: 0.00
 correction_forward_rate: 1
 

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

@@ -106,6 +106,7 @@ class BaseController:
         self.yaw_kd = rospy.get_param("~yaw_kd",1)
         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.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
@@ -371,6 +372,7 @@ class BaseController:
             if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
                 self.stop()
                 self.correction_yaw_flag = 0
+                self.reset_pid_controller()
             else:
                 self.send_motor_speed()
 
@@ -464,6 +466,11 @@ class BaseController:
         except rospy.ServiceException, e:            
             print "Service call failed: %s"%e
 
+    def reset_pid_controller(self):
+        self.pid_controller.integrator = 0
+        self.pid_controller.last_error = 0
+        self.pid_controller.timeOfLastCall = None
+
     def cmdVelCallback(self, req):
         
         # Handle velocity-based movement requests
@@ -474,7 +481,7 @@ class BaseController:
         th = req.angular.z # rad/s
 
         if x != 0 and y == 0 and th == 0:
-            if self.correction_yaw_flag < 2:
+            if self.correction_yaw_flag < self.correction_yaw_flag_timeout:
                 self.stop()
                 self.last_yaw_data = self.get_yaw()
                 self.correction_yaw_flag += 1
@@ -486,7 +493,8 @@ class BaseController:
                     th = self.pid_controller.update(err_yaw_data)
                     #print("err_yaw_data is ",err_yaw_data,"th is ",th)
         else:
-            self.correction_yaw_flag = 0  
+            self.correction_yaw_flag = 0
+            self.reset_pid_controller()  
 
         #Inverse Kinematic
         tmpX = sqrt(3)/2.0