1.概述
使用PIBOT提供的小車已經(jīng)完成了程序和硬件的調(diào)試屿愚，如果需要移植到自己的小車兼蜈，可能會(huì)遇到PID調(diào)速不正常，解算得到運(yùn)動(dòng)結(jié)果不一致，反解得到里程有問題等，原因就是電機(jī)上的電機(jī)接線次序、編碼器AB相次序，以及控制板輸出口跟電機(jī)次序寥假、，這里需要注意下面幾點(diǎn)即可
- a. 給定電機(jī)接口函數(shù)的輸入?yún)?shù)正負(fù)與電機(jī)方向的關(guān)系
- b.電機(jī)索引關(guān)系
- c.電機(jī)轉(zhuǎn)向得到編碼器值正負(fù)關(guān)系
2.PIBOT運(yùn)動(dòng)解算和PID
void Robot::do_kinmatics(){
if (!do_kinmatics_flag){
for(int i=0;i<MOTOR_COUNT;i++){
pid[i]->clear();
encoder[i]->get_increment_count_for_dopid();
}
return;
}
static unsigned long last_millis=0;
if (Board::get()->get_tick_count()-last_millis>=Data_holder::get()->parameter.do_pid_interval){
last_millis = Board::get()->get_tick_count();
for(int i=0;i<MOTOR_COUNT;i++){
feedback[i] = encoder[i]->get_increment_count_for_dopid();
}
#if PID_DEBUG_OUTPUT
#if MOTOR_COUNT==2
printf("input=%ld %ld feedback=%ld %ld\r\n", long(input[0]*1000), long(input[1]*1000),
long(feedback[0]), long(feedback[1]));
#endif
#if MOTOR_COUNT==3
printf("input=%ld %ld %ld feedback=%ld %ld %ld\r\n", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000),
long(feedback[0]), long(feedback[1]), long(feedback[2]));
#endif
#if MOTOR_COUNT==4
printf("input=%ld %ld %ld %ld feedback=%ld %ld %ld %ld\r\n", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000), long(input[3]*1000),
long(feedback[0]), long(feedback[1]), long(feedback[2]), long(feedback[3]));
#endif
#endif
bool stoped=true;
for(int i=0;i<MOTOR_COUNT;i++){
if (input[i] != 0 || feedback[i] != 0){
stoped = false;
break;
}
}
short output[MOTOR_COUNT]={0};
if (stoped){
for(int i=0;i<MOTOR_COUNT;i++){
output[i] = 0;
}
do_kinmatics_flag = false;
}else{
for(int i=0;i<MOTOR_COUNT;i++){
output[i] = pid[i]->compute(Data_holder::get()->parameter.do_pid_interval*0.001);
}
}
for(int i=0;i<MOTOR_COUNT;i++){
Data_holder::get()->pid_data.input[i] = int(input[i]);
Data_holder::get()->pid_data.output[i] = int(feedback[i]);
}
#if PID_DEBUG_OUTPUT
#if MOTOR_COUNT==2
printf("output=%ld %ld\r\n\r\n", output[0], output[1]);
#endif
#if MOTOR_COUNT==3
printf("output=%ld %ld %ld\r\n\r\n", output[0], output[1], output[2]);
#endif
#if MOTOR_COUNT==4
printf("output=%ld %ld %ld %ld\r\n\r\n", output[0], output[1], output[2], output[3]);
#endif
#endif
for(int i=0;i<MOTOR_COUNT;i++){
motor[i]->control(output[i]);
}
if (Board::get()->get_tick_count()-last_velocity_command_time>Data_holder::get()->parameter.cmd_last_time){
for(int i=0;i<MOTOR_COUNT;i++){
input[i] = 0;
}
}
}
}
運(yùn)動(dòng)控制是在Robot類中的do_kinmatics函數(shù)實(shí)現(xiàn)的莽鸿，大概流程
- 根據(jù)解算到各個(gè)輪子的速度轉(zhuǎn)換到對(duì)應(yīng)編碼器的值和編碼器的輸出計(jì)算PID
- 根據(jù)PID結(jié)果控制電機(jī)
- 超時(shí)判斷
上面說到的解算就是在Robot類中的update_velocity函數(shù)實(shí)現(xiàn)的
void Robot::update_velocity(){
short vx = min(max(Data_holder::get()->velocity.v_liner_x, -(short(Data_holder::get()->parameter.max_v_liner_x))), short(Data_holder::get()->parameter.max_v_liner_x));
short vy = min(max(Data_holder::get()->velocity.v_liner_y, -(short(Data_holder::get()->parameter.max_v_liner_y))), short(Data_holder::get()->parameter.max_v_liner_y));
short vz = min(max(Data_holder::get()->velocity.v_angular_z, -(short(Data_holder::get()->parameter.max_v_angular_z))), short(Data_holder::get()->parameter.max_v_angular_z));
float vel[3]={vx/100.0, vy/100.0, vz/100.0};
float motor_speed[MOTOR_COUNT]={0};
model->motion_solver(vel, motor_speed);
for(int i=0;i<MOTOR_COUNT;i++){
input[i] = motor_speed[i]*short(Data_holder::get()->parameter.encoder_resolution)/(2*__PI)*short(Data_holder::get()->parameter.do_pid_interval)*0.001;
}
#if DEBUG_ENABLE
printf("vx=%d %d motor_speed=%ld %ld\r\n", vx, vz, long(motor_speed[0]*1000), long(motor_speed[1]*1000));
#endif
last_velocity_command_time = Board::get()->get_tick_count();
do_kinmatics_flag = true;
}
通過調(diào)用運(yùn)動(dòng)模型接口調(diào)用運(yùn)動(dòng)解算(model->motion_solver(vel, motor_speed))昧旨，完成從控制的全局速度（下發(fā)的角速度和線速度）到各個(gè)輪子速度的轉(zhuǎn)換，最終轉(zhuǎn)換為在do_pid_interval時(shí)間內(nèi)編碼器的變化值
3.PIBOT電機(jī)方向和順序
3.1電機(jī)方向與pwm_value值關(guān)系
PIBOT定義所有電機(jī)控制給正值時(shí)(motor[i]->control(pwm_value))祥得，從電機(jī)輸出軸方向看電機(jī)順時(shí)針轉(zhuǎn)兔沃。
對(duì)應(yīng)到差分小車apollo,
如
motor[0]->control(2000);//控制左電機(jī)向后
motor[1]->control(2000);//控制右電機(jī)向前
對(duì)與小車就是在逆時(shí)針運(yùn)動(dòng)
再如
motor[0]->control(-2000);//控制左電機(jī)向前
motor[1]->control(2000);//控制右電機(jī)向前
對(duì)與小車就是在向前運(yùn)動(dòng)
總結(jié)就是control給定正值，輸出軸看電機(jī)順時(shí)針轉(zhuǎn)動(dòng)级及，反之給定負(fù)值逆時(shí)針轉(zhuǎn)動(dòng)
3.2電機(jī)順序
上面可以看出來motor[i]中的索引i
apollo中左電機(jī)為0乒疏，右電機(jī)為1
zeus中前電機(jī)為0，左電機(jī)為1饮焦，右電機(jī)為2
hades和hera中右后電機(jī)為0怕吴，右前電機(jī)為1，左前電機(jī)為2县踢，左后電機(jī)為3
具體編號(hào)源碼kinematic_models文件夾中相應(yīng)文件前右說明
3.3 編碼器
這里編碼器值是一個(gè)程序計(jì)數(shù)值转绷，一個(gè)方向轉(zhuǎn)動(dòng)編碼器值會(huì)累加，反方向會(huì)減少
PIBOT定義上面2.1中給定正值時(shí)硼啤，編碼器值累加议经，給定負(fù)是編碼器值減少
4.驗(yàn)證測(cè)試
如果上面有點(diǎn)晦澀，那直接按照如下方式測(cè)試即可
apollo為例
4.1 測(cè)試方向
do_kinmatics直接motor[0]->control(2000); return;
** 這里2000相對(duì)于PWM最大值來的Arduino最大1024 STM32最大設(shè)置為5000**
觀察左電機(jī)電機(jī)是否向后
分別查看下面各個(gè)控制與實(shí)際電機(jī)轉(zhuǎn)動(dòng)情況
motor[0]->control(2000) 左電機(jī)向后
motor[0]->control(-2000) 左電機(jī)向前
motor[1]->control(2000) 右電機(jī)向前
motor[1]->control(-2000) 右電機(jī)向后
如果得不到相應(yīng)的結(jié)果，根據(jù)情況調(diào)整:
a. motor[0]->control時(shí)右電機(jī)轉(zhuǎn)動(dòng)煞肾，可能考慮左右電機(jī)接線反了
b. motor[1]->control(xx) ,哪個(gè)電機(jī)是對(duì)的咧织，但發(fā)現(xiàn)不對(duì)〖龋可以調(diào)整電機(jī)接線习绢，也可以調(diào)整程序的方向控制
PIBOT提供相關(guān)的宏MOTORx_REVERSE可以不需要對(duì)換電機(jī)接線(PIN1和PIN6),達(dá)到調(diào)整方向的目的
4.2測(cè)試編碼器
恢復(fù)會(huì)正常程序并新增調(diào)試的輸出，連接調(diào)試串口蝙昙，打開串口調(diào)試工具
對(duì)應(yīng)到程序的輸出
觀察串口調(diào)試助手中的
total_count=xx yy輸出闪萄，xx隨著左輪的向前轉(zhuǎn)動(dòng)越來越小，反之越來越大耸黑；yy隨著右輪的向前越來越大桃煎，反之越來越小
如果得不到相應(yīng)的結(jié)果，根據(jù)情況調(diào)整:
a. 左電機(jī)轉(zhuǎn)動(dòng)yy變化或者右電機(jī)轉(zhuǎn)動(dòng)xx變化大刊，那應(yīng)該是2個(gè)電機(jī)編碼器反了，需要調(diào)換下
b. 左電機(jī)轉(zhuǎn)動(dòng)xx變化三椿，但相反缺菌。應(yīng)該是編碼器AB相接線反了；右電機(jī)同理
PIBOT提供相關(guān)的宏ENCODERx_REVERSE可以不需要對(duì)換電機(jī)編碼器接線（PIN3和PIN4）,達(dá)到調(diào)整方向的目的
5.程序修改
最新程序已經(jīng)支持直接修改配置方式搜锰， 具體請(qǐng)參考6.配置修改
固件程序提供了直接的宏伴郁，針對(duì)電機(jī)反向或者編碼器反向的問題
例如STM32F1電機(jī)方向反轉(zhuǎn)宏,修改宏定義
STM32F4編碼器方向反轉(zhuǎn)宏(在param.mk文件)
6. 配置修改
目前最新程序已經(jīng)支持動(dòng)態(tài)修改無需修改代碼重新編譯,
# 分別開啟2個(gè)終端執(zhí)行下面命令
pibot_bringup
pibot_configure
