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