ROSArduinoBridge中PID 在 ros端 与arduino端的工作机制

参见博主 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的调整。