新增在平衡小车模式和默认模式下,主循环频率设置为不一样的

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -5,12 +5,13 @@
 #   serial_baud:串口通信时波特率
 #   i2c_smbus_num:系统管理总线号,树莓派为1
 #   isc_slave_addr:arduino的IIC设备地址
-#   main_rate:主循环的频率
+#   default_main_rate:默认主循环的频率,不在平衡小车模式下
 #   cmd_topic:底盘订阅的控制移动的话题名称
 # Author: corvin
 # History:
 #   20200706:init this file.
 #   20211016:增加两轮小车平衡模式下的参数.
+#   20211116:增加默认主循环和平衡小车模式下两个不同配置参数.
 
 is_use_serial: True
 
@@ -21,7 +22,7 @@ serial_baud: 57600
 i2c_smbus_num: 1   #raspberryPi:1, huawei altals 200DK: 2
 i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
 
-main_rate: 100
+default_main_rate: 20
 cmd_topic: cmd_vel
 
 base_frame: base_footprint
@@ -37,6 +38,7 @@ angular_scale_correction: 1.00
 
 #是否开启平衡车模式,直立pd控制
 balance_mode: False    #开启时设置为True
+balance_main_rate: 100 #在平衡小车模式下主循环的频率
 middle_val: 4.24       #小车物理平衡时的俯仰角度
 motor_stop_delay: 11   #单位ms
 check_down_pitch: 18   #当倾斜角超过该度数,认为小车已经倒下,停止电机转动
@@ -64,6 +66,6 @@ BWheel_Ko: 10
 
 # Sensor definitions (case sensitive!):
 sensors: {
-  GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5, direction: input},
-  batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1, direction: input},
+  GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5},
+  batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1},
 }

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -31,8 +31,12 @@ class ArduinoROS():
         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", 100))
+        #根据参数中配置的频率来设置主循环频率,平衡小车模式为100hz,默认支撑轮模式20hz
+        if self.balance_mode:
+            self.rate = int(rospy.get_param("~balance_main_rate", 100))
+        else:
+            self.rate = int(rospy.get_param("~default_main_rate", 20))
+
         loop_rate = rospy.Rate(self.rate)
 
         #初始化控制移动的Twist发布者,用于断开连接时停止小车移动
@@ -76,18 +80,12 @@ class ArduinoROS():
         sensor_params = rospy.get_param("~sensors", dict({}))
 
         for name, params in sensor_params.iteritems():
-            # Set the direction to input if not specified
-            try:
-                params['direction']
-            except:
-                params['direction'] = 'input'
-
             if params['type'] == "GP2Y0A41":
                 sensor = GP2Y0A41(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == 'Digital':
-                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == 'Analog':
-                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == 'BAT_PERCENT':
                 sensor = BatPercent(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == "US025":

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

@@ -283,11 +283,11 @@ class Arduino:
             rospy.loginfo("get baud failed !")
             return None
 
+    #串口方式获取到两个电机的编码器计数值
     def get_encoder_counts(self):
         values = self.execute_array('e')
         if len(values) != 2:
-            rospy.logerr("Encoder count was not 2")
-            raise SerialException
+            rospy.logwarn("Encoder count error")
             return None
         else:
             return values
@@ -403,6 +403,7 @@ class Arduino:
             except:
                 return None
 
+    #往指定的数字引脚写入高低电平,在使用该函数前需要先调用set_digital_pin_mode设置数字引脚模式
     def digital_write(self, pin, value):
         if self.is_use_serial:
             return self.execute_ack('w %d %d' %(pin, value))

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

@@ -32,13 +32,11 @@ class MessageType:
     ULTRASONIC = 7
 
 class Sensor(object):
-    def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
+    def __init__(self, controller, name, pin, rate, frame_id, **kwargs):
         self.controller = controller
         self.name = name
-        self.pin = pin
+        self.pin  = pin
         self.rate = rate
-        self.direction = direction
-
         self.frame_id = frame_id
         self.value = None
 
@@ -49,23 +47,8 @@ class Sensor(object):
     def poll(self):
         now = rospy.Time.now()
         if now > self.t_next:
-            if self.direction == "input":
-                try:
-                    self.value = self.read_value()
-                    #rospy.loginfo("read value: "+str(self.value))
-                except:
-                    try:
-                        rospy.logwarn("Failed to read sensor values, try again. ")
-                        self.ack = self.read_value()
-                    except:
-                        rospy.logerr("Sensor read value error !")
-                        return
-            else:
-                try:
-                    self.ack = self.write_value()
-                except:
-                    rospy.logerr("Sensor write value error !")
-                    return
+            self.value = self.read_value()
+            #rospy.loginfo("read value: "+str(self.value))
 
             # For range sensors, assign the value to the range message field
             if self.message_type == MessageType.RANGE:
@@ -104,30 +87,6 @@ class AnalogSensor(Sensor):
     def write_value(self, value):
         return self.controller.analog_write(self.pin, value)
 
-class AnalogFloatSensor(Sensor):
-    def __init__(self, *args, **kwargs):
-        super(AnalogFloatSensor, self).__init__(*args, **kwargs)
-
-        self.message_type = MessageType.ANALOG
-
-        self.msg = AnalogFloat()
-        self.msg.header.frame_id = self.frame_id
-
-        self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)
-
-        if self.direction == "output":
-            self.controller.pin_mode(self.pin, OUTPUT)
-        else:
-            self.controller.pin_mode(self.pin, INPUT)
-
-        self.value = LOW
-
-    def read_value(self):
-        return self.controller.analog_read(self.pin)
-
-    def write_value(self, value):
-        return self.controller.analog_write(self.pin, value)
-
 class DigitalSensor(Sensor):
     def __init__(self, *args, **kwargs):
         super(DigitalSensor, self).__init__(*args, **kwargs)
@@ -143,7 +102,6 @@ class DigitalSensor(Sensor):
         return self.controller.digital_read(self.pin)
 
     def write_value(self):
-        # Alternate HIGH/LOW when writing at a fixed rate
         self.value = not self.value
         return self.controller.digital_write(self.pin, self.value)