ROS小車４

上位機程序：

主要介紹ros_arduino_python包

 ros_arduino_python                      #ROS相關的Python包，用於上位機，樹莓派等開發板或電腦等。
    ├── CMakeLists.txt
    ├── config                              #配置目錄
    │   └── arduino_params.yaml             #定義相關參數，端口，rate，PID，sensors等默認參數。由arduino.launch調用
    ├── launch
    │   └── arduino.launch                  #啓動文件
    ├── nodes
    │   └── arduino_node.py                 #python文件,實際處理節點，由arduino.launch調用，即可單獨調用。
    ├── package.xml
    ├── setup.py
    └── src                                 #Python類包目錄
        └── ros_arduino_python
            ├── arduino_driver.py           #Arduino驅動類
            ├── arduino_sensors.py          #Arduino傳感器類
            ├── base_controller.py          #基本控制類，訂閱cmd_vel話題，發佈odom話題
            └── __init__.py                 #類包默認空文件  

• arduino_params.yaml  ：

• 這裏建議換個名字，比如我是　my_arduino_params.yaml 

//如果要直接複製使用，請刪除註釋
# For a direct USB cable connection, the port name is typically
# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
# For a wireless connection like XBee, the port is typically
# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.

port: /dev/ttyACM0 //arduino端口
baud: 57600 //波特率
timeout: 0.1　//超時時間

rate: 50
#sensorstate_rate: 10

use_base_controller: True //是否使用base_controller
base_controller_rate: 10 //base_controller控制頻率

# For a robot that uses base_footprint, change base_frame to base_footprint
//base_link:機器人本體座標系，與機器人中心重合，當然有些機器人(PR 2)是base_footprint,其實是一個意思。
base_frame: base_link

# === Robot drivetrain parameters
wheel_diameter: 0.0973 //車輪直徑
wheel_track: 0.292 //兩輪間距
encoder_resolution: 1322 # from Pololu for 30:1 motors
//編碼器碼盤一圈的脈衝數，可以用arduino的串口輸入Ｅ獲得 這裏我的電機是有減速的
//但是我圖方便，沒有計算減速比，直接記錄車輪轉一圈的脈衝數
gear_reduction: 1　//減速比
motors_reversed: True //電機是否允許反向

# === PID parameters//PID參數
lKp: 25
lKd: 12
lKi: 0
lKo: 55

rKp: 25
rKd: 12
rKi: 5
rKo: 55
accel_limit: 0.5

# === Sensor definitions.  Examples only - edit for your robot.
#     Sensor type can be one of the follow (case sensitive!):
#	  * Ping
#	  * GP2D12
#	  * Analog
#	  * Digital
#	  * PololuMotorCurrent
#	  * PhidgetsVoltage
#	  * PhidgetsCurrent (20 Amp, DC)



sensors: {
  #motor_current_left:   {pin: 0, type: PololuMotorCurrent, rate: 5},
  #motor_current_right:  {pin: 1, type: PololuMotorCurrent, rate: 5},
  #ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
  #sonar_front_center:   {pin: 5, type: Ping, rate: 10},
  #arduino_led:          {pin: 13, type: Digital, rate: 5, direction: output}
}

• arduino.launch

在這裏把配置加載

<launch>
   <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">

      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
   </node>
</launch>

•  arduino_node.py      實際處理節點：

將這行更改成這樣：發佈的topic名字改爲 cmd_vel

# A cmd_vel publisher so we can stop the robot when shutting down
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

將base_frame改爲base_link

self.base_frame = rospy.get_param("~base_frame", 'base_link')

 只要注意和前後對應即可

• arduino_driver.py  Arduino驅動類：

在Arduino類中添加：

def get_pidin(self):
    values = self.execute_array('i')
    if len(values) != 2:
        print "get_pidin count was not 2"
        raise SerialException
        return None
    else:
        return values

def get_pidout(self):
    values = self.execute_array('f')
    if len(values) != 2:
        print "get_pidout count was not 2"
        raise SerialException
        return None
    else:
        return values

將update_pid更改爲下面

def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo):
        ''' Set the PID parameters on the Arduino
        '''
        print "Updating PID parameters"
        cmd = 'u ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo)+':'+ str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo)
        self.execute_ack(cmd)

•  base_controller.py      基本控制類

它訂閱cmd_vel話題，發佈odom話題

由於左右兩輪使用不同的pid參數，所=所以需要做出如下改動：

在from tf.broadcaster import TransformBroadcaster下面增加

from std_msgs.msg import Int32

將pid_params = dict()後的字典進行更改

pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        pid_params['lkp'] = rospy.get_param("~lkp", 20)
        pid_params['lkd'] = rospy.get_param("~lkd", 12)
        pid_params['lki'] = rospy.get_param("~lki", 0)
        pid_params['lko'] = rospy.get_param("~lko", 50)

        pid_params['rkp'] = rospy.get_param("~rkp", 20)
        pid_params['rkd'] = rospy.get_param("~rkd", 12)
        pid_params['rki'] = rospy.get_param("~rki", 0)
        pid_params['rko'] = rospy.get_param("~rko", 50)

 將setup_pid進行如下修改：

def setup_pid(self, pid_params):
        # Check to see if any PID parameters are missing
        missing_params = False
        for param in pid_params:
            if pid_params[param] == "":
                print("*** PID Parameter " + param + " is missing. ***")
                missing_params = True
        
        if missing_params:
            os._exit(1)
                
        self.wheel_diameter = pid_params['wheel_diameter']
        self.wheel_track = pid_params['wheel_track']
        self.encoder_resolution = pid_params['encoder_resolution']
        self.gear_reduction = pid_params['gear_reduction']
        
        self.lkp = pid_params['lkp']
        self.lkd = pid_params['lkd']
        self.lki = pid_params['lki']
        self.lko = pid_params['lko']

        self.rkp = pid_params['rkp']
        self.rkd = pid_params['rkd']
        self.rki = pid_params['rki']
        self.rko = pid_params['rko']
        
        self.arduino.update_pid(self.lkp, self.lkd, self.lki, self.lko,self.rkp, self.rkd, self.rki, self.rko)

self.odomPub = rospy.Publisher(‘odom’, Odometry) 上面添加如下內容：

self.lEncoderPub = rospy.Publisher('Lencoder', Int32)
self.rEncoderPub = rospy.Publisher('Rencoder', Int32)
self.lPidoutPub = rospy.Publisher('Lpidout', Int32)
self.rPidoutPub = rospy.Publisher('Rpidout', Int32)
self.lVelPub = rospy.Publisher('Lvel', Int32)
self.rVelPub = rospy.Publisher('Rvel', Int32)

在poll(self)函數的，if now > self.t_next: 下添加

try:
    left_pidin, right_pidin = self.arduino.get_pidin()
except:
    rospy.logerr("getpidout exception count: ")
    return

self.lEncoderPub.publish(left_pidin)
self.rEncoderPub.publish(right_pidin)
try:
    left_pidout, right_pidout = self.arduino.get_pidout()
except:
    rospy.logerr("getpidout exception count: ")
    return
self.lPidoutPub.publish(left_pidout)
self.rPidoutPub.publish(right_pidout)

在poll(self)函數的，if not self.stopped: 下添加：

self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)

 運行

將小車的電源等接好

每打開一個新終端就要刷新一下環境：（進入到工作目錄運行）

source devel/setup.bash 

打開新終端，進入到ros_arduino_bridge所在工作空間，刷新工作環境後運行如下命令：

roslaunch ros_arduino_python arduino.launch

再打開一個終端，運行鍵盤控制程序，這個程序可以使用turtlebot包中的robot_keyboard_teleop.py。將其發送的話題重命名即可

這時如果一切正常，小車就可以正常移動，如果轉動方向什麼的有問題，根據個人經驗調整arduino電機驅動代碼

如果一切正常，進行下一步

再打開一個新終運行如下：

rqt_plot /Lencoder /Lpidout /Lvel 

可以看到實時的速度曲線，方便進行pid參數的調整。

如果小車速度不穩定，通過校準PID來改進

問題：

• 如果發現小車速度忽快忽慢，或者走不了直線，那是因爲PID參數給的不合理造成的。
• PID的目的是通過改變電機PWM值，使電機實際的轉速基本等於期望的轉速。
• 如果參數不合理，就會出現實際的轉速和期望的轉速相差很遠。
• 也就是說，我們沒辦法精準控制小車。

校準方法：

• 把電機PWM值、期望的轉速和實際的轉速這三者的值用圖表實時地描繪出來。
• 根據PWM值和實際的轉速的運動軌跡，不停地修改PID的參數，讓期望的轉速和實際的轉速能在很短時間內的達到一致。
• 調節順序，先調P,再調I，最後調D，通常只需要P和I兩個參數就可以了。

參考鏈接：https://www.ncnynl.com/archives/201612/1208.html