优化小车平衡模式,使用pwm直接控制电机,稳定性增加

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -36,9 +36,9 @@ angular_scale_correction: 1.00
 
 #是否开启平衡车模式,直立pd控制
 balance_mode: False  #开启时设置为True
-middle_val: 4.05     #小车物理平衡时的俯仰角度
+middle_val: 4.28     #小车物理平衡时的俯仰角度
 motor_stop_delay: 11 #单位ms
-balance_kp: 30.0  #30
+balance_kp: 35    #35
 balance_kd: 2.3   #2.3
 
 # PID parameters

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -27,10 +27,10 @@ class ArduinoROS():
         self.serial_baud    = int(rospy.get_param("~serial_baud", 57600))
         self.i2c_smbus_num  = rospy.get_param("~i2c_smbus_num", 1)
         self.i2c_slave_addr = rospy.get_param("~i2c_slave_addr", 8)
-        self.timeout    = rospy.get_param("~connect_timeout", 0.5)
-        self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
-        self.cmd_topic  = rospy.get_param("~cmd_topic", "cmd_vel")
-        self.balance_mode = rospy.get_param("~balance_mode", False)
+        self.timeout        = rospy.get_param("~connect_timeout", 0.5)
+        self.base_frame     = rospy.get_param("~base_frame", 'base_footprint')
+        self.cmd_topic      = rospy.get_param("~cmd_topic", "cmd_vel")
+        self.balance_mode   = rospy.get_param("~balance_mode", False)
         self.motor_stop_delay = rospy.get_param("~motor_stop_delay", 11)
 
         #根据参数中配置的频率来设置主循环频率

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

@@ -370,6 +370,16 @@ class Arduino:
         except:
             return None
 
+    def pwm_drive(self, AWheel, BWheel):
+        return self.execute_ack('n %d %d' %(AWheel, BWheel))
+
+    def i2c_pwm_drive(self, AWheel, BWheel):
+        cmd = (' %d %d\r' %(AWheel, BWheel))
+        try:
+            self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('n'), [ord(c) for c in cmd])
+        except:
+            return None
+
     #使用串口发送停止电机转动命令
     def stop(self):
         self.drive(0, 0)

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

@@ -18,12 +18,11 @@ from sensor_msgs.msg import Imu
 class BalanceMode:
     def __init__(self):
         self.angularVal = 57.2957
-        self.motorPulse = 0
 
         self.pitchData = 0
         self.angularPitch = 0
 
-        self.middle_val = rospy.get_param("~middle_val", 0.0733)
+        self.middle_val = rospy.get_param("~middle_val", 4.3)
         self.balance_kp = rospy.get_param("~balance_kp", 1)
         self.balance_kd = rospy.get_param("~balance_kd", 1)
 
@@ -31,16 +30,21 @@ class BalanceMode:
         rospy.Subscriber("/imu_data", Imu, self.angularProc)
 
     def pitchDataProc(self, msg):
-        self.pitchData = msg.data * self.angularVal
+        self.pitchData = msg.data*self.angularVal
 
     def angularProc(self, msg):
-        self.angularPitch = msg.angular_velocity.y * self.angularVal
+        self.angularPitch = msg.angular_velocity.y*self.angularVal
 
     def keepBalance(self):
-        pitch_err = self.pitchData-self.middle_val
-        self.motorPulse = self.balance_kp * pitch_err \
-                          + self.balance_kd * self.angularPitch
+        pitch_err = self.pitchData - self.middle_val
+        motorPWM = int(self.balance_kp*pitch_err + self.balance_kd*self.angularPitch)
+                          
+        #将计算得到的pwm值进行限幅,最大pwm值为255
+        if motorPWM > 255:
+            motorPWM = 255
+        elif motorPWM < -255:
+            motorPWM = -255
 
         rospy.loginfo("pitch=" + str(self.pitchData) + \
-                      " angular=" + str(self.angularPitch) + \
-                      " motorPulse=" + str(self.motorPulse))
+                      " angular=" + str(self.angularPitch) + " PWM=" + str(motorPWM))
+        return motorPWM

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

@@ -151,15 +151,13 @@ class BaseController:
         #发送调试pid参数的数据
         self.send_debug_pid()
 
-        #是否开启平衡车模式,计算pid周期内的脉冲数
+        #是否开启平衡车模式,计算控制周期内控制电机的pwm值
         if self.isBalance:
-            self.balanceControl.keepBalance()
-            data = self.balanceControl.motorPulse
-            
+            pwm = self.balanceControl.keepBalance()
             if self.use_serial:
-                self.arduino.drive(-data, data)
+                self.arduino.pwm_drive(-pwm, pwm)
             else:
-                self.arduino.i2c_drive(-data, data)
+                self.arduino.i2c_pwm_drive(-pwm, pwm)
 
         #读取编码器计数值,用于计算里程计信息
         try:
@@ -173,6 +171,8 @@ class BaseController:
                 #rospy.logwarn("Failed to get encoder counts, try again. ")
                 if self.use_serial == False:
                     aWheel_enc, bWheel_enc = self.arduino.i2c_get_encoder_counts()
+                else:
+                    aWheel_enc, bWheel_enc = self.arduino.get_encoder_counts()
             except:
                 rospy.logerr("Encoder exception count !")
                 return

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -69,7 +69,7 @@ def getKey():
     return key
 
 def vels(speed, turn):
-    return "当前控制速度:  线速度: %s  角速度: %s" %(speed,turn)
+    return "当前控制速度: 线速度: %s m/s 转弯角速度: %s rad/s" %(speed,turn)
 
 def angles(angle):
     return "当前角度:  %s " % (angle)
@@ -80,7 +80,7 @@ def client_srv(ser_id, ser_angle):
     try:
         camera_client = rospy.ServiceProxy("mobilebase_arduino/camera_control", CameraControl)
         resp = camera_client.call(ser_id, ser_angle)
-    except (rospy.ServiceException, rospy.ROSException), e:
+    except (rospy.ServiceException, rospy.ROSException) as e:
         rospy.logwarn("Service call failed: %s"%e)
 
 if __name__=="__main__":
@@ -89,8 +89,8 @@ if __name__=="__main__":
     pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
     rospy.init_node('teleop_twist_keyboard')
 
-    #初始化控制命令参数,线速度0.3m/s，角速度1.0rad/s
-    speed = 0.30
+    #初始化控制命令参数,线速度0.2m/s，角速度1.0rad/s
+    speed = 0.2
     turn = 1.0
 
     x = 0