更新电机pwm控制,当左右电机向x轴正向运动时,pwm为正,编码器计数值也为正

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -35,16 +35,16 @@ linear_scale_correction: 1.00
 angular_scale_correction: 1.00
 
 #是否开启平衡车模式,直立pd控制
-balance_mode: True   #开启时设置为True
-middle_val: 4.26     #小车物理平衡时的俯仰角度
-motor_stop_delay: 11 #单位ms
-check_down_pitch: 18 #当倾斜角超过该度数,认为小车已经倒下,停止电机转动
-pitch_tolerance: 0.2 #当平衡模式时,该角度倾斜范围内不进行调平
+balance_mode: False     #开启时设置为True
+middle_val: 4.24       #小车物理平衡时的俯仰角度
+motor_stop_delay: 11   #单位ms
+check_down_pitch: 18   #当倾斜角超过该度数,认为小车已经倒下,停止电机转动
+pitch_tolerance: 0.2   #当平衡模式时,该角度倾斜范围内不进行调平
 
-balance_kp: 10.0   #直立环kp参数,25最大值,调试值17,17*0.6=10.2为正常值
-balance_kd: 0.64   #直立环kd参数,2.0最大值,调试值1.2,1.2*0.6=0.65
-speed_kp: 15.0     #速度环的kp参数
-speed_ki: 0.075    #速度环的ki参数,一般情况ki=kp/200
+balance_kp: 10.2   #直立环kp参数,25最大值,调试值17,17*0.6=10.2为正常值
+balance_kd: 0.65   #直立环kd参数,2.0最大值,调试值1.2,1.2*0.6=0.65
+speed_kp: 2.0      #速度环的kp参数,最大值3.2
+speed_ki: 0.01    #速度环的ki参数,一般情况ki=kp/200
 
 # PID parameters
 debugPID: False

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/balance_mode.py

@@ -27,7 +27,7 @@ class BalanceMode:
         self.downPitchData = 0   #pitch角度超过该值,认为小车倒下,停止电机转动
 
         self.middle_val  = rospy.get_param("~middle_val", 4.3)
-        self.downPitchData = rospy.get_param("~check_down_pitch", 18)
+        self.downPitchData   = rospy.get_param("~check_down_pitch", 18)
         self.pitch_tolerance = rospy.get_param("~pitch_tolerance", 0.2)
         self.balance_kp  = rospy.get_param("~balance_kp", 17)
         self.balance_kd  = rospy.get_param("~balance_kd", 1)
@@ -53,12 +53,11 @@ class BalanceMode:
 
     #平衡小车速度环,PI控制器
     def keepVelocity(self, encoder_A, encoder_B, target_speed):
-        encoder_err = encoder_A + encoder_B
+        encoder_err = encoder_A + encoder_B 
 
         #电机编码器计数进行一阶低通滤波
         self.encoder_lowOut = (1-self.encoderFilterPer)*encoder_err + \
                             self.encoderFilterPer*self.encoder_lowout
-
         self.encoder_integral += self.encoder_lowOut    #速度偏差进行积分
 
         self.encoder_integral -= target_speed
@@ -70,7 +69,7 @@ class BalanceMode:
         
         #计算控制量pwm
         motorPWM = self.speed_kp*self.encoder_lowOut + self.speed_ki*self.encoder_integral
-        #rospy.loginfo("encoder A:B="+str(encoder_A)+":"+str(encoder_B)+" speed pwm=" + str(motorPWM))
+        #rospy.loginfo("integral="+str(self.encoder_integral)+" pwm="+str(motorPWM))
         return motorPWM
 
     #检查小车是否已经倒下,倒下后停止电机转动

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

@@ -147,6 +147,7 @@ class BaseController:
             self.arduino.i2c_update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
                                         self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,)
 
+    #对电机pwm进行限幅,保持其正负号
     def pwm_ctrl(self, pwmA, pwmB):
         if pwmA > 255:
             pwmA = 255
@@ -160,7 +161,7 @@ class BaseController:
 
         return int(pwmA), int(pwmB)
 
-    #底盘运动控制的主循环,执行频率由arduino_node中的频率决定,默认50hz
+    #底盘运动控制的主循环,执行频率由arduino_node中的频率决定,默认100hz
     def poll(self):
         #发送调试pid参数的数据
         self.send_debug_pid()
@@ -183,10 +184,6 @@ class BaseController:
                 return
         #rospy.loginfo("Encoder A:B=" + str(aWheel_enc) + ":" + str(bWheel_enc))
 
-        #计算编码器计数差值,当前值减去上次的计数值
-        encoder_diff_A = aWheel_enc - self.enc_A
-        encoder_diff_B = bWheel_enc - self.enc_B
-
         #当读取到最新的编码器计数为0,说明下位机已经将计数清零,那上位机保存的上一次编码器计数也得清零
         if aWheel_enc == 0 and bWheel_enc == 0:
             self.enc_A = 0
@@ -195,6 +192,14 @@ class BaseController:
                 self.balanceControl.encoder_integral = 0
                 self.balanceControl.encoder_lowout = 0
 
+        #计算编码器计数差值,当前值减去上次记录的计数值
+        encoder_diff_A = aWheel_enc - self.enc_A
+        encoder_diff_B = bWheel_enc - self.enc_B
+
+        #保存当前编码器计数值,用于下次计算
+        self.enc_A = aWheel_enc
+        self.enc_B = bWheel_enc
+
         #是否开启平衡车模式,在控制周期内计算控制电机的pwm值
         if self.isBalance:
             #计算小车当前的倾角,用于直立环和判断小车是否倒下
@@ -202,7 +207,8 @@ class BaseController:
 
             #启动直立环控制,PD控制器,控制pitch角度和角速度
             stand_pwm = self.balanceControl.keepBalance(pitch_err)
-
+            #stand_pwm = 0
+            
             #启动速度环控制,PI控制器
             #speed_pwm = self.balanceControl.keepVelocity(encoder_diff_A, encoder_diff_B, self.target_speed)
             speed_pwm = 0
@@ -211,13 +217,13 @@ class BaseController:
             turn_pwm = 0
 
             #计算最终发送给电机的pwm
-            pwm_A_tmp = -(stand_pwm + speed_pwm - turn_pwm)
+            pwm_A_tmp = stand_pwm + speed_pwm - turn_pwm
             pwm_B_tmp = stand_pwm + speed_pwm + turn_pwm
             
             #将计算得到的pwm值进行限幅,最大pwm值为255,保持正负号
             pwm_A, pwm_B = self.pwm_ctrl(pwm_A_tmp, pwm_B_tmp)
             pwm_A, pwm_B = self.balanceControl.checkIfDown(pitch_err, pwm_A, pwm_B)
-            #rospy.loginfo("pwm_A:pwm_B= " + str(pwm_A) + " : " + str(pwm_B))
+            
             if self.use_serial:
                 self.arduino.pwm_drive(pwm_A, pwm_B)
             else:
@@ -227,10 +233,6 @@ class BaseController:
         dist_A = encoder_diff_A/self.ticks_per_meter
         dist_B = encoder_diff_B/self.ticks_per_meter
 
-        #保存当前编码器计数值,用于下次计算
-        self.enc_A = aWheel_enc
-        self.enc_B = bWheel_enc
-
         time_now = rospy.Time.now()
         dt = time_now - self.old_time
         self.old_time = time_now
@@ -325,8 +327,8 @@ class BaseController:
         th = req.angular.z  #z轴转动角速度rad/s
 
         #两轮小车逆运动学模型,计算得到左右电机转速
-        vA = -(x + 0.5*self.wheel_track*th)
-        vB =  (x - 0.5*self.wheel_track*th)
+        vA = x + 0.5*self.wheel_track*th
+        vB = x - 0.5*self.wheel_track*th
 
         self.v_des_AWheel = int(vA * self.ticks_per_meter / self.PID_rate)
         self.v_des_BWheel = int(vB * self.ticks_per_meter / self.PID_rate)