删除一些无用的自定义srv文件,优化代码结构

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -13,19 +13,11 @@ add_message_files(FILES
                  )
 
 add_service_files(FILES
-                  AnalogSensorWrite.srv
-                  AnalogFloatSensorWrite.srv
                   AnalogRead.srv
-                  AnalogSensorRead.srv
-                  AnalogFloatSensorRead.srv
-                  DigitalPinMode.srv
                   DigitalRead.srv
                   DigitalSensorRead.srv
                   DigitalSetDirection.srv
-                  DigitalSensorPinMode.srv
                   DigitalWrite.srv
-                  DigitalSensorWrite.srv
-                  AnalogSensorRead.srv
                   ServoRead.srv
                   ServoWrite.srv
                   SetSpeed.srv

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogFloatSensorRead.srv

@@ -1,2 +0,0 @@
----
-float32 value

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv

@@ -1,2 +0,0 @@
-float32 value
----

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogSensorRead.srv

@@ -1,2 +0,0 @@
----
-uint16 value

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogSensorWrite.srv

@@ -1,2 +0,0 @@
-uint16 value
----

src/ros_arduino_bridge/ros_arduino_msgs/srv/DigitalPinMode.srv

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

src/ros_arduino_bridge/ros_arduino_msgs/srv/DigitalSensorPinMode.srv

@@ -1,2 +0,0 @@
-bool direction
----

src/ros_arduino_bridge/ros_arduino_msgs/srv/DigitalSensorWrite.srv

@@ -1,2 +0,0 @@
-bool value
----

src/ros_arduino_bridge/ros_arduino_msgs/srv/DigitalSetDirection.srv

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

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -56,7 +56,7 @@ class ArduinoROS():
         # A service to get new pid params
         rospy.Service('~dynamic_pid', DynamicPID, self.DynamicPIDHandler)
 
-        # A service to return infrared sensor distance
+        #用来获取红外测据传感器距离信息的服务
         rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
         
         #当节点退出的时候执行的回调函数
@@ -122,7 +122,7 @@ class ArduinoROS():
             loop_rate.sleep()
 
     def DigitalSetDirectionHandler(self, req):
-        self.controller.pin_mode(req.pin, req.direction)
+        self.controller.set_digital_pin_mode(req.direction)
         return DigitalSetDirectionResponse()
 
     def DigitalWriteHandler(self, req):

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

@@ -365,16 +365,16 @@ class Arduino:
         except:
             return None
 
-    #使用串口发送停止电机转动命令
+    #使用串口或IIC向下位机发送停止电机转动命令
     def stop(self):
-        self.drive(0, 0)
-
-    def i2c_stop(self):
-        cmd = (' 0 0\r')
-        try:
-            self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('m'), [ord(c) for c in cmd])
-        except:
-            return None
+        if self.is_use_serial:
+            self.drive(0, 0)
+        else:
+            cmd = (' 0 0\r')
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('m'), [ord(c) for c in cmd])
+            except:
+                return None
 
     def analog_read(self, pin):
         if self.is_use_serial:
@@ -391,16 +391,6 @@ class Arduino:
             except:
                 return None
 
-    def analog_write(self, pin, value):
-        if self.is_use_serial:
-            return self.execute_ack('x %d %d' %(pin, value))
-        else:
-            cmd = (' %d %d\r' %(pin, value))
-            try:
-                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('x'), [ord(c) for c in cmd])
-            except:
-                return None
-
     def digital_read(self, pin):
         if self.is_use_serial:
             return self.execute('d %d' %pin)
@@ -427,11 +417,12 @@ class Arduino:
             except:
                 return None
 
-    def pin_mode(self, pin, mode):
+    #设置数字引脚的输入输出模式,D49,D50,D51,D52四个引脚同时设置为相同模式-> 0:输入模式  1:输出模式
+    def set_digital_pin_mode(self, mode):
         if self.is_use_serial:
-            return self.execute_ack('c %d %d' %(pin, mode))
+            return self.execute_ack('c %d' %(mode))
         else:
-            cmd = (' %d %d\r' %(pin, mode))
+            cmd = (' %d\r' %(mode))
             try:
                 self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('c'), [ord(c) for c in cmd])
             except:

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

@@ -20,17 +20,17 @@ class BalanceMode:
         self.angularVal = 57.296     #1弧度对应的角度
         self.encoderFilterPer = 0.7  #低通滤波系数
 
-        self.pitchData = 0
-        self.angularPitch = 0
+        self.pitchData    = 0    #记录小车当前的俯仰角度
+        self.angularPitch = 0    #小车y轴的角速度
         self.encoder_integral = 0
         self.encoder_lowout = 0  #低通滤波上次的输出值
-        self.downPitchData = 0   #pitch角度超过该值,认为小车倒下,停止电机转动
+        self.downPitchData  = 0  #pitch角度超过该值,认为小车倒下,停止电机转动
 
-        self.middle_val  = rospy.get_param("~middle_val", 4.3)
+        self.middle_val      = rospy.get_param("~middle_val", 4.3)
         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)
+        self.balance_kp  = rospy.get_param("~balance_kp", 10.2)
+        self.balance_kd  = rospy.get_param("~balance_kd", 0.65)
         self.speed_kp    = rospy.get_param("~speed_kp", 1)
         self.speed_ki    = rospy.get_param("~speed_ki", 1)
 

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

@@ -23,7 +23,6 @@ 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, cmd_topic, balance_mode):
         self.use_serial = is_use_serial
@@ -56,14 +55,14 @@ 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)
 
-        # Set up PID parameters and check for missing values
+        #配置电机pid参数
         self.setup_pid(pid_params)
 
-        # How many encoder ticks are there per meter?
+        #配置电机每米有多少个脉冲数
         self.ticks_per_meter = self.encoder_resolution*self.gear_reduction*2/(self.wheel_diameter*pi)
         self.ticks_per_meter = self.ticks_per_meter/self.linear_scale_correction
 
-        # What is the maximum acceleration we will tolerate when changing wheel speeds?
+        #设置电机的加速度最大值
         self.max_accel = self.accel_limit*self.ticks_per_meter
 
         now = rospy.Time.now()