增加平衡模式直立环控制代码,现在小车的平衡控制间隔20-30ms,需要减小该值

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -20,7 +20,7 @@ i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
 
 connect_timeout: 0.5
 
-main_rate: 50
+main_rate: 100
 cmd_topic: cmd_vel
 
 base_frame: base_footprint
@@ -34,6 +34,13 @@ gear_reduction: 30     #empty payload rpm 330 r/m,if rpm=530,gear_reduction=19
 linear_scale_correction: 1.00
 angular_scale_correction: 1.00
 
+#是否开启平衡车模式,直立pd控制
+balance_mode: False  #开启时设置为True
+middle_val: 4.05     #小车物理平衡时的俯仰角度
+motor_stop_delay: 11 #单位ms
+balance_kp: 30.0  #30
+balance_kd: 2.3   #2.3
+
 # PID parameters
 debugPID: False
 

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -22,10 +22,7 @@ class ArduinoROS():
     def __init__(self):
         rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
 
-        #当节点退出的时候执行的回调函数
-        rospy.on_shutdown(self.shutdown)
-
-        self.is_use_serial  = rospy.get_param("~is_use_serial", True)
+        self.is_serial  = rospy.get_param("~is_use_serial", True)
         self.serial_port    = rospy.get_param("~serial_port", "/dev/ttyACM0")
         self.serial_baud    = int(rospy.get_param("~serial_baud", 57600))
         self.i2c_smbus_num  = rospy.get_param("~i2c_smbus_num", 1)
@@ -33,6 +30,8 @@ class ArduinoROS():
         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)
 
         #根据参数中配置的频率来设置主循环频率
         self.rate = int(rospy.get_param("~main_rate", 50))
@@ -64,9 +63,12 @@ class ArduinoROS():
 
         # A service to return infrared sensor distance
         rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
+        
+        #当节点退出的时候执行的回调函数
+        rospy.on_shutdown(self.shutdown)
 
         # Initialize the controlller
-        self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
+        self.controller = Arduino(self.is_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
 
         #与下位机建立连接
         self.controller.connect()
@@ -103,7 +105,13 @@ class ArduinoROS():
                 rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
 
         #初始化底盘控制器
-        self.myBaseController = BaseController(self.is_use_serial, self.controller, self.base_frame)
+        self.myBaseController = BaseController(self.is_serial,self.controller,self.base_frame,self.cmd_topic,self.balance_mode)
+
+        #配置电机停止转动的延时,从配置文件中读取延迟参数
+        if self.balance_mode:
+            self.controller.update_motor_stop_delay(self.motor_stop_delay)
+        else:
+            self.controller.update_motor_stop_delay(100)
 
         #开始主循环,控制底盘转动和发布各传感器信息
         while not rospy.is_shutdown():
@@ -138,10 +146,6 @@ class ArduinoROS():
         value = self.controller.analog_read(req.pin)
         return AnalogReadResponse(value)
 
-    def LightShowHandler(self, req):
-        self.controller.light_show(req.value)
-        return LightShowResponse()
-
     def CameraControlHandler(self, req):
         self.controller.camera_angle(req.servo_id, req.servo_angle)
         return CameraControlResponse()

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

@@ -22,13 +22,10 @@ class Arduino:
     N_ANALOG_PORTS  = 10
     N_DIGITAL_PORTS = 54
 
-    def __init__(self, is_use_serial,serial_port="/dev/ttyACM0",baudrate=57600,i2c_smbus_num=1,i2c_slave_addr=8,timeout=0.5):
-        self.PID_RATE = 50   #Don't change this ! It is a fixed property of the Arduino PID controller.
-        self.PID_INTERVAL = 20  #单位ms
-
+    def __init__(self,is_use_serial,serial_port,baudrate,i2c_smbus_num,i2c_slave_addr,timeout):
         self.is_use_serial = is_use_serial #与下位机arduino的通信方式是否使用串口还是IIC
-        self.serial_port = serial_port
-        self.baudrate    = baudrate
+        self.serial_port   = serial_port
+        self.baudrate      = baudrate
 
         self.i2c_smbus_num  = i2c_smbus_num
         self.i2c_slave_addr = i2c_slave_addr
@@ -366,8 +363,6 @@ class Arduino:
         return self.execute_ack('m %d %d' %(AWheel, BWheel))
 
     def i2c_drive(self, AWheel, BWheel):
-        ''' Speeds are given in encoder ticks per PID interval
-        '''
         #rospy.loginfo("drive() A:" + str(AWheel) + ",B:" + str(BWheel))
         cmd = (' %d %d\r' %(AWheel, BWheel))
         try:
@@ -447,6 +442,7 @@ class Arduino:
             except:
                 return None
 
+    #控制蜂鸣器发出各种声音
     def beep_ring(self, value):
         if self.is_use_serial:
             return self.execute_ack('p %d' %value)
@@ -457,6 +453,17 @@ class Arduino:
             except:
                 return None
 
+    #电机多长时间没收到命令将停止转动
+    def update_motor_stop_delay(self, value):
+        if self.is_use_serial:
+            return self.execute_ack('l %d' %value)
+        else:
+            cmd = (' %d\r' %(value))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('l'), [ord(c) for c in cmd])
+            except:
+                return None
+
     def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
         if self.is_use_serial:
             return self.execute_array('h')
@@ -585,25 +592,3 @@ class Arduino:
             rospy.logerr(sys.exe_info())
             traceback.print_exc(file=sys.stdout)
             return None
-
-""" Basic test for connectivity by serial port """
-if __name__ == "__main__":
-    is_use_serial = True
-    portName = "/dev/ttyACM0"
-    baudRate = 57600
-
-    iic_smbus = 1
-    iic_num = 8
-
-    myArduino = Arduino(is_use_serial, serial_port=portName, baudrate=baudRate, i2c_smbus_num=iic_smbus, i2c_slave_addr=iic_num, timeout=0.5)
-    myArduino.connect()
-
-    print("Test Beep ring 3 times...")
-    for i in range(3):
-        myArduino.beep_ring(1)
-        time.sleep(2.0)
-    print("Did you heard the beep ring ?")
-
-    myArduino.stop()
-    myArduino.close()
-    print("Shutting down Arduino !")

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

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+#-*- coding:utf-8 -*-
+
+"""
+  Copyright: 2016-2021 www.corvin.cn ROS小课堂
+  Description:  使两轮小车保持平衡模式,可以去掉支撑轮直立保持平衡和移动.
+  Author: corvin
+  History:
+    20211005:初始化代码.
+"""
+
+import os
+import rospy
+import roslib; roslib.load_manifest('ros_arduino_python')
+from std_msgs.msg import Float32
+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.balance_kp = rospy.get_param("~balance_kp", 1)
+        self.balance_kd = rospy.get_param("~balance_kd", 1)
+
+        rospy.Subscriber("/pitch_data", Float32, self.pitchDataProc)
+        rospy.Subscriber("/imu_data", Imu, self.angularProc)
+
+    def pitchDataProc(self, msg):
+        self.pitchData = msg.data * self.angularVal
+
+    def angularProc(self, msg):
+        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
+
+        rospy.loginfo("pitch=" + str(self.pitchData) + \
+                      " angular=" + str(self.angularPitch) + \
+                      " motorPulse=" + str(self.motorPulse))

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

@@ -20,18 +20,22 @@ from geometry_msgs.msg import Quaternion, Twist
 from nav_msgs.msg import Odometry
 from tf.broadcaster import TransformBroadcaster
 from std_msgs.msg import Int32
+from ros_arduino_python.balance_mode import BalanceMode
 
 
 """ Class to receive Twist commands and publish wheel Odometry data """
 class BaseController:
-    def __init__(self, is_use_serial, arduino, base_frame):
+    def __init__(self, is_use_serial, arduino, base_frame, cmd_topic, balance_mode):
         self.use_serial = is_use_serial
         self.arduino    = arduino
         self.base_frame = base_frame
+        self.isBalance  = balance_mode
+        self.cmd_topic  = cmd_topic
+        self.PID_rate   = 100  #下位机中pid固定频率100hz
+
         self.odom_name  = rospy.get_param("~odom_name", "odom")
-        self.cmd_topic  = rospy.get_param("~cmd_topic", "cmd_vel")
         self.debugPID   = rospy.get_param('/dynamic_tutorials/debugPID', False)
-
+        
         pid_params = dict()
         pid_params['wheel_diameter']     = rospy.get_param("~wheel_diameter", 0.067)
         pid_params['wheel_track']        = rospy.get_param("~wheel_track", 0.126)
@@ -51,8 +55,6 @@ class BaseController:
         self.linear_scale_correction  = rospy.get_param("~linear_scale_correction",  1.0)
         self.angular_scale_correction = rospy.get_param("~angular_scale_correction", 1.0)
 
-        #rospy.loginfo("pid_params_dict: "+pid_params)
-
         # Set up PID parameters and check for missing values
         self.setup_pid(pid_params)
 
@@ -66,7 +68,7 @@ class BaseController:
         now = rospy.Time.now()
         self.old_time = now  # time for determining dx/dy
 
-        #用于保存读取的左右电机的编码器计数值
+        #用于保存读取的电机编码器计数值
         self.enc_A = 0
         self.enc_B = 0
 
@@ -74,17 +76,21 @@ class BaseController:
         self.y  = 0  #在平面上Y轴方向位移
         self.th = 0  #在平面上朝向
 
-        self.v_A = 0
+        self.v_A = 0 #真正发送给下位机的脉冲数
         self.v_B = 0
 
-        self.v_des_AWheel = 0  # cmd_vel setpoint
+        self.v_des_AWheel = 0  #计算出来应该发送的脉冲数
         self.v_des_BWheel = 0
 
         self.last_cmd_time = now
 
-        # Subscriptions main comtrol board to send control cmd
+        #设置订阅控制移动话题的回调函数
         rospy.Subscriber(self.cmd_topic, Twist, self.cmdVelCallback)
 
+        #只有开启了平衡车模式才初始化平衡模式
+        if self.isBalance:
+            self.balanceControl = BalanceMode()
+
         #初始化时,将电机编码器计数值清零
         if self.use_serial:
             self.arduino.reset_encoders()
@@ -104,7 +110,7 @@ class BaseController:
             self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=10)
             self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=10)
 
-        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m with " + str(self.encoder_resolution) + " ticks per rev")
+        rospy.loginfo("Started base controller of " + str(self.wheel_track) + "m with " + str(self.encoder_resolution) + " ticks per rev")
 
     def setup_pid(self, pid_params):
         # Check to see if any PID parameters are missing
@@ -118,7 +124,7 @@ class BaseController:
             os._exit(1)
 
         self.encoder_resolution = pid_params['encoder_resolution']
-        self.gear_reduction     = pid_params['gear_reduction']
+        self.gear_reduction = pid_params['gear_reduction']
         self.wheel_diameter = pid_params['wheel_diameter']
         self.wheel_track    = pid_params['wheel_track']
         self.wheel_track    = self.wheel_track/self.angular_scale_correction
@@ -140,11 +146,21 @@ 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,)
 
-    #底盘运动控制的主循环,执行频率由arduino_node中的频率决定
+    #底盘运动控制的主循环,执行频率由arduino_node中的频率决定,默认50hz
     def poll(self):
         #发送调试pid参数的数据
         self.send_debug_pid()
 
+        #是否开启平衡车模式,计算pid周期内的脉冲数
+        if self.isBalance:
+            self.balanceControl.keepBalance()
+            data = self.balanceControl.motorPulse
+            
+            if self.use_serial:
+                self.arduino.drive(-data, data)
+            else:
+                self.arduino.i2c_drive(-data, data)
+
         #读取编码器计数值,用于计算里程计信息
         try:
             if self.use_serial:
@@ -261,18 +277,19 @@ class BaseController:
             self.AVelPub.publish(self.v_A)
             self.BVelPub.publish(self.v_B)
 
+    #控制小车移动话题的回调函数,用于计算得到每个pid周期内电机转动的脉冲数
     def cmdVelCallback(self, req):
         self.last_cmd_time = rospy.Time.now()
 
-        x  = req.linear.x   # x轴方向线速度　m/s
-        th = req.angular.z  # z轴转动角速度　rad/s
+        x  = req.linear.x   #x轴方向线速度m/s
+        th = req.angular.z  #z轴转动角速度rad/s
 
-        #两轮小车逆运动学模型,得到左右电机转速
+        #两轮小车逆运动学模型,计算得到左右电机转速
         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.arduino.PID_RATE)
-        self.v_des_BWheel = int(vB * self.ticks_per_meter / self.arduino.PID_RATE)
+        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)
         #rospy.loginfo("cmdCallback(): A v_des:"+str(self.v_des_AWheel)+",B v_des:"+str(self.v_des_BWheel))
 
     #用于调试电机PID参数