參見博主 diegodiego 的 “ 

ROS機器人Diego 1#製作（六）base controller---對ros_arduino_bridge的修改，實現兩個馬達獨立PID調速

”

PID呼叫流程：

Arduino 端：
1, ROSArduinoBridge.ino  loop() 方法中的343行 ->updatePID();
需要檢查 ROSArduinoBridge.ino 第233行，參照 base_controller.py 的 def setup_pid(self, pid_params) 方法


ROS端： 
1,base_controller.py  _init_(self,arduino,base_frame) 第91行 rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
監聽"cmd_vel" topic, 有資訊更新之後啟動回撥函式 cmdVelCallback（）；
2, cmdVelCallback（）去設定目標速度，poll（）回重複使用目標速度與arduino端互動，SEE  self.arduino.drive(self.v_left, self.v_right)。
poll是在arduino_node.py 的 _init_() 152行被迴圈呼叫的 ，SEE： 
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()

mutex.release()

為了方便理解update_PID()函式，改寫了個小方法： 

#include <iostream>
#include <string>
#include <stdlib.h>
#include<stdio.h>
using namespace std;

int main() {
string source = "u 20:12:0:50:20:12:0:50\r";
int len = source.length();
int i =0;
int arg = 0;
int index =0;
char argv1[32];
char argv2[32];
char cmd;

while (len > 0) {
len--;
char chr = source[i++];
if (chr == 13) {
if (arg == 1)
argv1[index] = NULL;
else if (arg == 2)
argv2[index] = NULL;
}
else if (chr == ' ') {
if (arg == 0)
arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
} else {
if (arg == 0) {
cmd = chr;
} else if (arg == 1) {
argv1[index] = chr;
index++;
} else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
cout << " cmd = "<< cmd << "\n";
cout << " argv1 = "<< argv1 << "\n";
cout << " argv2 = "<< argv2 << "\n";
}

執行結果如下： 

 cmd = u
 argv1 = 20:12:0:50:20:12:0:50
 argv2 = ?@

很明顯能夠看出argv1 的值是用來個兩個輪子分別做PID用的。

# 為了直觀的看到 encoder 和 out 之間的關係，修該如下兩處： 

arduino_driver.py 

def recv_array(self):
        ''' This command should not be used on its own: it is called by the execute commands
            below in a thread safe manner.
        '''
        try:
            temp_str = self.recv(self.timeout * self.N_ANALOG_PORTS)
             values = temp_str.split()
            if temp_str.find('|'):
                i = 0
                out = ['','']
                list = ['','']
                for temp in values:
                    list = temp.split('|')
                    values[i] = list[0]
                    out[i] = list[1]
                    i +=1
                print 'out : ' + str(out)
                print 'encoder : '+str(values)
            return map(int, values)
        except:
            return []

ROSArduinoBridge.ino

runCommand(){

.......

 case READ_ENCODERS:
    Serial.print(readEncoder(LEFT));
    Serial.print("|");
    Serial.print(leftPID.output);
    Serial.print(" ");
    Serial.print(readEncoder(RIGHT));
    Serial.print("|");
    Serial.println(rightPID.output);
    break;

........

修改之後就可以直觀的看到 encoder 和out 之間的關係，方便pid的調整。