优化小车平衡模式代码

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -13,10 +13,8 @@ add_message_files(FILES
                  )
 
 add_service_files(FILES
-                  AnalogWrite.srv
                   AnalogSensorWrite.srv
                   AnalogFloatSensorWrite.srv
-                  AnalogPinMode.srv
                   AnalogRead.srv
                   AnalogSensorRead.srv
                   AnalogFloatSensorRead.srv
@@ -27,8 +25,6 @@ add_service_files(FILES
                   DigitalSensorPinMode.srv
                   DigitalWrite.srv
                   DigitalSensorWrite.srv
-                  Enable.srv
-                  Relax.srv
                   AnalogSensorRead.srv
                   ServoRead.srv
                   ServoWrite.srv

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogPinMode.srv

@@ -1,3 +0,0 @@
-uint8 pin
-bool direction
----

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogWrite.srv

@@ -1,3 +0,0 @@
-uint8 pin
-uint16 value
----

src/ros_arduino_bridge/ros_arduino_msgs/srv/Enable.srv

@@ -1,3 +0,0 @@
-bool enable
----
-bool state

src/ros_arduino_bridge/ros_arduino_msgs/srv/Relax.srv

@@ -1,2 +0,0 @@
-
----

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -5,9 +5,12 @@
 #   serial_baud:串口通信时波特率
 #   i2c_smbus_num:系统管理总线号,树莓派为1
 #   isc_slave_addr:arduino的IIC设备地址
+#   main_rate:主循环的频率
+#   cmd_topic:底盘订阅的控制移动的话题名称
 # Author: corvin
 # History:
 #   20200706:init this file.
+#   20211016:增加两轮小车平衡模式下的参数.
 
 is_use_serial: True
 
@@ -18,8 +21,6 @@ serial_baud: 57600
 i2c_smbus_num: 1   #raspberryPi:1, huawei altals 200DK: 2
 i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
 
-connect_timeout: 0.5
-
 main_rate: 100
 cmd_topic: cmd_vel
 
@@ -35,7 +36,7 @@ linear_scale_correction: 1.00
 angular_scale_correction: 1.00
 
 #是否开启平衡车模式,直立pd控制
-balance_mode: False     #开启时设置为True
+balance_mode: False    #开启时设置为True
 middle_val: 4.24       #小车物理平衡时的俯仰角度
 motor_stop_delay: 11   #单位ms
 check_down_pitch: 18   #当倾斜角超过该度数,认为小车已经倒下,停止电机转动
@@ -44,7 +45,7 @@ pitch_tolerance: 0.2   #当平衡模式时,该角度倾斜范围内不进行调
 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
+speed_ki: 0.01     #速度环的ki参数,一般情况ki=kp/200
 
 # PID parameters
 debugPID: False

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -8,9 +8,8 @@
   History:
     20200706:init this file.
 """
-import os
 import rospy
-import thread
+import os, thread
 from ros_arduino_msgs.srv import *
 from geometry_msgs.msg import Twist
 from serial.serialutil import SerialException
@@ -22,40 +21,36 @@ class ArduinoROS():
     def __init__(self):
         rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
 
-        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)
-        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.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)
+        self.i2c_slave_addr   = rospy.get_param("~i2c_slave_addr", 8)
+        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))
+        self.rate = int(rospy.get_param("~main_rate", 100))
         loop_rate = rospy.Rate(self.rate)
 
         #初始化控制移动的Twist发布者,用于断开连接时停止小车移动
         self.cmd_vel_pub = rospy.Publisher(self.cmd_topic, Twist, queue_size=1)
 
-        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
+        #设置数字引脚的输入输出模式 (0 = input, 1 = output)
         rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
 
-        # A service to turn a digital sensor on or off
+        #设置数字引脚的高低电平
         rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
 
-        # A service to read the value of a digital sensor
+        #读取数字引脚高低电平的服务
         rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
 
-        # A service to set pwm values for the pins
-        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
-
-        # A service to read the value of an analog sensor
+        #读取模拟引脚数值的服务
         rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
 
-        # A service to control camera angle
+        #控制摄像头转动的服务
         rospy.Service('~camera_control', CameraControl, self.CameraControlHandler)
 
         # A service to get new pid params
@@ -68,12 +63,12 @@ class ArduinoROS():
         rospy.on_shutdown(self.shutdown)
 
         # Initialize the controlller
-        self.controller = Arduino(self.is_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.controller.connect()
 
-        # Reserve a thread lock
+        #初始化一个线程锁对象
         mutex = thread.allocate_lock()
 
         #获取传感器列表
@@ -138,10 +133,6 @@ class ArduinoROS():
         value = self.controller.digital_read(req.pin)
         return DigitalReadResponse(value)
 
-    def AnalogWriteHandler(self, req):
-        self.controller.analog_write(req.pin, req.value)
-        return AnalogWriteResponse()
-
     def AnalogReadHandler(self, req):
         value = self.controller.analog_read(req.pin)
         return AnalogReadResponse(value)
@@ -152,7 +143,7 @@ class ArduinoROS():
 
     def GetInfraredDistanceHandler(self, req):
         value = self.controller.detect_distance()
-        return GetInfraredDistanceResponse(value[0]/100.0,value[1]/100.0,value[2]/100.0)
+        return GetInfraredDistanceResponse(value[0]/100.0, value[1]/100.0, value[2]/100.0)
 
     def DynamicPIDHandler(self, req):
         self.controller.update_pid(req.value1, req.value2, 0, 50,
@@ -166,12 +157,13 @@ class ArduinoROS():
         self.controller.close()
         os._exit(0)
 
+
 if __name__ == '__main__':
     try:
         myArduino = ArduinoROS()
     except SerialException:
         rospy.logerr("ERROR trying to open serial port.")
         os._exit(1)
-    else:
+    except Exception:
         rospy.logerr("Get other unknown error !")
         os._exit(2)

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

@@ -2,7 +2,7 @@
 #-*- coding:utf-8 -*-
 
 """
-  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Description: A Python driver for the Arduino microcontroller.
   Author: corvin, jally
   History:
@@ -13,33 +13,28 @@ from serial.serialutil import SerialException
 import thread, smbus, rospy, time, os
 from serial import Serial
 import sys, traceback
-import rospy
 
 
 class Arduino:
-    ''' Configuration Arduino DUE Board Parameters
-    '''
-    N_ANALOG_PORTS  = 10
-    N_DIGITAL_PORTS = 54
 
-    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
+    def __init__(self, is_use_serial, serial_port, baudrate, i2c_smbus_num, i2c_slave_addr):
+        self.is_use_serial = is_use_serial  #与下位机arduino的通信方式是否使用串口还是IIC
         self.serial_port   = serial_port
         self.baudrate      = baudrate
 
         self.i2c_smbus_num  = i2c_smbus_num
         self.i2c_slave_addr = i2c_slave_addr
 
-        self.timeout = timeout
-        self.writeTimeout     = timeout
-        self.interCharTimeout = timeout/30.0
+        self.timeout          = 0.5
+        self.writeTimeout     = 0.5
+        self.interCharTimeout = 0.02
 
-        # Keep things thread safe
+        #初始化一个线程锁对象
         self.mutex = thread.allocate_lock()
 
     #与下位机arduino建立连接,根据串口或IIC做区分
     def connect(self):
-        if self.is_use_serial:
+        if self.is_use_serial:  #使用串口建立连接
             self.serial_connect()
         else:
             self.i2c_bus = smbus.SMBus(self.i2c_smbus_num)
@@ -48,7 +43,6 @@ class Arduino:
     #使用串口建立连接,进行通信
     def serial_connect(self):
         try:
-            rospy.loginfo("Connecting to Arduino on port: " + self.serial_port)
             self.serial_port = Serial(port=self.serial_port,baudrate=self.baudrate,timeout=self.timeout,writeTimeout=self.writeTimeout)
             time.sleep(1)
             test = self.get_baud()
@@ -57,8 +51,7 @@ class Arduino:
                 test = self.get_baud()
                 if test != self.baudrate:
                     raise SerialException
-            self.beep_ring(1) #连接串口成功的提示音
-            rospy.loginfo("Arduino connected baudrate: "+str(self.baudrate))
+            self.beep_ring(1)  #连接串口成功的提示音
         except SerialException:
             rospy.logerr("Cannot connect to Arduino !")
             rospy.logerr(sys.exc_info())
@@ -98,12 +91,10 @@ class Arduino:
             rospy.logerr("Cannot connect to Arduino IIC slave addr !")
             os._exit(1)
 
+    #断开上下位机建立的连接,串口或者IIC连接
     def close(self):
-        ''' Close the serial port or i2c bus connect.
-        '''
-        self.beep_ring(2)
-        rospy.sleep(0.5)
-        self.beep_ring(3)
+        self.beep_ring(0)  #断开连接时的提示音
+        rospy.sleep(1)
         if self.is_use_serial:
             self.serial_port.close()
         else:
@@ -156,7 +147,7 @@ class Arduino:
             below in a thread safe manner.
         '''
         try:
-            values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
+            values = self.recv(self.timeout).split()
             return map(float, values)
         except:
             return []
@@ -228,17 +219,11 @@ class Arduino:
         self.mutex.release()
         return values
 
+    #通过串口向下位机发送控制命令,并判断反馈是否为'ok'
     def execute_ack(self, cmd):
-        ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
-        '''
         self.mutex.acquire()
 
-        try:
-            self.serial_port.flushInput()
-        except:
-            pass
-
-        ntries   = 1
+        ntries   = 2
         attempts = 0
         try:
             self.serial_port.write(cmd + '\r')
@@ -250,7 +235,7 @@ class Arduino:
                     ack = self.recv(self.timeout)
                 except:
                     rospy.logerr("Exception executing command: " + cmd)
-                attempts += 1
+                    attempts += 1
         except:
             self.mutex.release()
             rospy.logerr("execute_ack exception when executing" + cmd)

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -55,7 +55,6 @@ class Sensor(object):
                     #rospy.loginfo("read value: "+str(self.value))
                 except:
                     try:
-                        # try again
                         rospy.logwarn("Failed to read sensor values, try again. ")
                         self.ack = self.read_value()
                     except:
@@ -209,7 +208,6 @@ class MyIRSensor(MyRangeSensor):
 class GP2Y0A41(MyIRSensor):
     def __init__(self, *args, **kwargs):
         super(GP2Y0A41, self).__init__(*args, **kwargs)
-
         self.msg.field_of_view = 0.01
         self.msg.min_range = 0.04
         self.msg.max_range = 0.30
@@ -226,8 +224,3 @@ class BatPercent(DigitalSensor):
     def read_value(self):
         percent = self.controller.getBatPercent()
         return int(percent)
-
-
-if __name__ == '__main__':
-    rospy.loginf("arduino sensor main function ...")
-