包列表
| 包 | 說(shuō)明 | 備注 |
|---|---|---|
| AFMotor | Adafruit shield電機(jī)驅(qū)動(dòng)板驅(qū)動(dòng) | |
| Board | 板子資源接口及Mega2560中的實(shí)現(xiàn) | 0 |
| DataHolder | 關(guān)鍵數(shù)據(jù)存儲(chǔ)類 | 0 |
| Encoder | 編碼器接口及Mega2560中編碼器實(shí)現(xiàn) | 0 |
| KinematicModels | 機(jī)器人模型類 | 針對(duì)不同型號(hào)機(jī)型的解算 |
| Motor | 電機(jī)驅(qū)動(dòng)接口類及AF電機(jī)驅(qū)動(dòng)與一般驅(qū)動(dòng)板的實(shí)現(xiàn) | 0 |
| PID | PID運(yùn)算類 | 0 |
| Transport | 通訊接口類與實(shí)現(xiàn) | 0 |
| robot | robot調(diào)度類 | 0 |
main分析
直接貼源碼蛹找，無(wú)需贅述
#include "Arduino.h"
#include "robot.h"
void setup()
{
Robot::get()->init();
}
void loop()
{
Robot::get()->check_command();
Robot::get()->do_kinmatics();
Robot::get()->calc_odom();
}
robot文件分析
robot.h
#ifndef PIBOT_ROBOT_H_
#define PIBOT_ROBOT_H_
#include "dataframe.h"
//根據(jù)機(jī)器人模型選擇相應(yīng)的解算類頭文件
#if ROBOT_MODEL == ROBOT_MODEL_DIFF//差分輪
#include "differential.h"
#endif
#if ROBOT_MODEL == ROBOT_OMNI_3//全向三輪
#include "omni3.h"
#endif
class MotorController;//電機(jī)控制器接口類
class Encoder; //編碼器接口類
class PID; //PID運(yùn)算接口類
class Transport; //通訊接口類
class Dataframe; //通訊協(xié)議數(shù)據(jù)包接口類
class Model; //機(jī)器人模型接口類
class Robot:public Notify{
public:
//單例模式
static Robot* get(){
static Robot robot;
return &robot;
}
//初始化
void init();
//檢測(cè)上位機(jī)命令
void check_command();
//運(yùn)動(dòng)學(xué)控制疚察，下發(fā)的速度轉(zhuǎn)換為各個(gè)輪子速度平且進(jìn)行PID控制
void do_kinmatics();
//根據(jù)個(gè)輪子編碼器反饋給出機(jī)器人位置姿態(tài)及實(shí)時(shí)速度信息
void calc_odom();
//Notify接口實(shí)現(xiàn)膝擂， 針對(duì)某些消息關(guān)注的回調(diào)函數(shù)
void update(const MESSAGE_ID id, void* data);
private:
Robot(){}
void init_motor();//初始化電機(jī)相關(guān)
void init_trans();//初始化通訊相關(guān)
private:
void clear_odom();//清除累計(jì)的位置姿態(tài)信息
void update_velocity();//更新下發(fā)的實(shí)時(shí)控制速度
private:
MotorController* motor[MOTOR_COUNT];//電機(jī)控制器接口
Encoder* encoder[MOTOR_COUNT];//編碼器接口
PID* pid[MOTOR_COUNT];
float input[MOTOR_COUNT];//PID間隔時(shí)間內(nèi)期望的encoder增加或減少的個(gè)數(shù)
float feedback[MOTOR_COUNT];//PID間隔時(shí)間內(nèi)反饋的encoder增加或減少的個(gè)數(shù)
Transport* trans;//通訊接口
Dataframe* frame;//通訊數(shù)據(jù)包接口
Model* model;//機(jī)器人模型接口
bool do_kinmatics_flag; //進(jìn)行運(yùn)動(dòng)控制的標(biāo)記
Odom odom;//機(jī)器人位置姿態(tài)實(shí)時(shí)速度信息
unsigned long last_velocity_command_time;//上次下發(fā)速度的時(shí)間點(diǎn)
};
#endif
robot.cpp關(guān)鍵代碼
初始化
void Robot::init(){
Data_holder::get()->load_parameter();//加載EPPROM中的配置機(jī)器人信息
#if DEBUG_ENABLE
Board::get()->usart_debug_init();//調(diào)試串口初始化 printf通過(guò)串口3輸出
#endif
printf("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d\r\n",
Data_holder::get()->parameter.wheel_diameter, Data_holder::get()->parameter.wheel_track, Data_holder::get()->parameter.encoder_resolution,
Data_holder::get()->parameter.do_pid_interval, Data_holder::get()->parameter.kp, Data_holder::get()->parameter.ki, Data_holder::get()->parameter.kd, Data_holder::get()->parameter.ko,
Data_holder::get()->parameter.cmd_last_time, Data_holder::get()->parameter.max_v_liner_x, Data_holder::get()->parameter.max_v_liner_y, Data_holder::get()->parameter.max_v_angular_z);
printf("init_motor\r\n");
init_motor();
printf("init_trans\r\n");
init_trans();
printf("pibot startup\r\n");
}
電機(jī)及編碼器相關(guān)初始化
void Robot::init_motor(){
#if MOTOR_COUNT>0//根據(jù)配置的個(gè)數(shù)定義編碼器钠龙、電機(jī)控制器及PID的具體實(shí)現(xiàn)斩跌，MOTOR_COUNT在具體的機(jī)器人模型中定義
#if MOTOR_CONTROLLER == COMMON_CONTROLLER//根據(jù)使用的電機(jī)控制器選擇電機(jī)控制器的實(shí)現(xiàn) 這里CommonMotorController與AFSMotorController都MotorController接口類的實(shí)現(xiàn)
static CommonMotorController motor1(MOTOR_1_PWM_PIN, MOTOR_1_DIR_A_PIN, MOTOR_1_DIR_B_PIN, MAX_PWM_VALUE);
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
static AFSMotorController motor1(MOTOR_1_PORT_NUM, MAX_PWM_VALUE);
#endif
//EncoderImp是Encoder接口類的實(shí)現(xiàn)
static EncoderImp encoder1(MOTOR_1_ENCODER_A_PIN, MOTOR_1_ENCODER_B_PIN);
//PID計(jì)算接口脖苏，給定參數(shù)為期望的數(shù)據(jù)地址渊抽、反饋的數(shù)據(jù)地址以及各個(gè)PID參數(shù)值
static PID pid1(&input[0], &feedback[0], float(Data_holder::get()->parameter.kp)/Data_holder::get()->parameter.ko,
float(Data_holder::get()->parameter.ki)/Data_holder::get()->parameter.ko,
float(Data_holder::get()->parameter.kd)/Data_holder::get()->parameter.ko , MAX_PWM_VALUE);
#endif
#if MOTOR_COUNT>1
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
static CommonMotorController motor2(MOTOR_2_PWM_PIN, MOTOR_2_DIR_A_PIN, MOTOR_2_DIR_B_PIN, MAX_PWM_VALUE);
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
static AFSMotorController motor2(MOTOR_2_PORT_NUM, MAX_PWM_VALUE);
#endif
static EncoderImp encoder2(MOTOR_2_ENCODER_A_PIN, MOTOR_2_ENCODER_B_PIN);
static PID pid2(&input[1], &feedback[1], float(Data_holder::get()->parameter.kp)/Data_holder::get()->parameter.ko,
float(Data_holder::get()->parameter.ki)/Data_holder::get()->parameter.ko,
float(Data_holder::get()->parameter.kd)/Data_holder::get()->parameter.ko , MAX_PWM_VALUE);
#endif
#if MOTOR_COUNT>2
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
static CommonMotorController motor3(MOTOR_3_PWM_PIN, MOTOR_3_DIR_A_PIN, MOTOR_3_DIR_B_PIN, MAX_PWM_VALUE);
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
static AFSMotorController motor3(MOTOR_3_PORT_NUM, MAX_PWM_VALUE);
#endif
static EncoderImp encoder3(MOTOR_3_ENCODER_A_PIN, MOTOR_3_ENCODER_B_PIN);
static PID pid3(&input[2], &feedback[2], float(Data_holder::get()->parameter.kp)/Data_holder::get()->parameter.ko,
float(Data_holder::get()->parameter.ki)/Data_holder::get()->parameter.ko,
float(Data_holder::get()->parameter.kd)/Data_holder::get()->parameter.ko , MAX_PWM_VALUE);
#endif
#if MOTOR_COUNT>0
motor[0] = &motor1;//接口指向具體實(shí)現(xiàn)
encoder[0] = &encoder1;
pid[0] = &pid1;
#endif
#if MOTOR_COUNT>1
motor[1] = &motor2;
encoder[1] = &encoder2;
pid[1] = &pid2;
#endif
#if MOTOR_COUNT>2
motor[2] = &motor3;
encoder[2] = &encoder3;
pid[2] = &pid3;
#endif
#if ROBOT_MODEL == ROBOT_MODEL_DIFF//根據(jù)配置的機(jī)器人模型選擇解算類的實(shí)現(xiàn)，這個(gè)Differential與Omni3是運(yùn)動(dòng)解算接口Model的實(shí)現(xiàn)
static Differential diff(Data_holder::get()->parameter.wheel_diameter*0.0005, Data_holder::get()->parameter.wheel_track*0.0005);
model = &diff;
#endif
#if ROBOT_MODEL == ROBOT_OMNI_3
static Omni3 omni3(Data_holder::get()->parameter.wheel_diameter*0.0005, Data_holder::get()->parameter.wheel_track*0.0005);
model = &omni3;
#endif
//初始化電機(jī)驅(qū)動(dòng)器
for (int i=0;i<MOTOR_COUNT;i++){
motor[i]->init();
}
do_kinmatics_flag = false;
memset(&odom, 0 , sizeof(odom));
memset(&input, 0 , sizeof(input));
memset(&feedback, 0 , sizeof(feedback));
last_velocity_command_time = 0;
}
通訊相關(guān)初始化
void Robot::init_trans(){
static USART_transport _trans(MASTER_USART, 115200);//使用串口作為通訊接口
static Simple_dataframe _frame(&_trans);//使用Simple_dataframe協(xié)議數(shù)據(jù)包實(shí)現(xiàn)數(shù)據(jù)打包解包
trans = &_trans;
frame = &_frame;
trans->init();
frame->init();
//注冊(cè)相關(guān)消息的通知篙耗， 收到該消息this->update回調(diào)會(huì)被調(diào)用
frame->register_notify(ID_SET_ROBOT_PARAMTER, this);
frame->register_notify(ID_CLEAR_ODOM, this);
frame->register_notify(ID_SET_VELOCITY, this);
}
上位命令處理
void Robot::check_command(){
unsigned char ch=0;
if (trans->read(ch)){//從通訊口中讀取數(shù)據(jù)
//printf("%02x ", ch);
if (frame->data_recv(ch)){//使用數(shù)據(jù)包接收和解析數(shù)據(jù)
//printf("\r\n");
frame->data_parse();
}
}
}
運(yùn)動(dòng)處理
void Robot::do_kinmatics(){
if (!do_kinmatics_flag){//該標(biāo)記收到上位的命令會(huì)置true迫筑， 超時(shí)停止會(huì)置false
for(int i=0;i<MOTOR_COUNT;i++){
pid[i]->clear();//停止后清除pid變量值
encoder[i]->get_increment_count_for_dopid();//讀取掉停止時(shí)編碼器的變化至，放置手動(dòng)轉(zhuǎn)動(dòng)電機(jī)導(dǎo)致下次啟動(dòng)pid時(shí)的異常
}
return;
}
static unsigned long last_millis=0;
//根據(jù)配置PID間隔時(shí)間進(jìn)行pid運(yùn)算
if (Board::get()->get_tick_count()-last_millis>=Data_holder::get()->parameter.do_pid_interval){
last_millis = Board::get()->get_tick_count();
//得到PID間隔時(shí)間反饋編碼器的值
for(int i=0;i<MOTOR_COUNT;i++){
feedback[i] = encoder[i]->get_increment_count_for_dopid();
}
#ifdef PID_DEBUG_OUTPUT
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
//判斷超時(shí)宗弯，則無(wú)需繼續(xù)PID運(yùn)算
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{
//計(jì)算得到輸出PWM input[i]在update回調(diào)通知中給定
for(int i=0;i<MOTOR_COUNT;i++){
output[i] = pid[i]->compute(Data_holder::get()->parameter.do_pid_interval*0.001);
}
}
#ifdef PID_DEBUG_OUTPUT
printf("output=%ld %ld %ld\r\n\r\n", output[0], output[1], output[2]);
#endif
//控制各個(gè)電機(jī)
for(int i=0;i<MOTOR_COUNT;i++){
motor[i]->control(output[i]);
}
//超時(shí)判斷
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;
}
}
}
}
計(jì)算里程位置姿態(tài)
void Robot::calc_odom(){
static unsigned long last_millis=0;
//每間CALC_ODOM_INTERVAL隔時(shí)間計(jì)算
if (Board::get()->get_tick_count()-last_millis>=CALC_ODOM_INTERVAL){
last_millis = Board::get()->get_tick_count();
#ifdef ODOM_DEBUG_OUTPUT
long total_count[MOTOR_COUNT]={0};
for(int i=0;i<MOTOR_COUNT;i++){
total_count[i] = encoder[i]->get_total_count();
}
printf("total_count=%ld %ld\r\n", total_count[0], total_count[1]);
#endif
float dis[MOTOR_COUNT] = {0};
//得到間隔時(shí)間內(nèi)個(gè)輪子行徑的距離(m)
for(int i=0;i<MOTOR_COUNT;i++){
dis[i] = encoder[i]->get_increment_count_for_odom()*__PI*Data_holder::get()->parameter.wheel_diameter*0.001/Data_holder::get()->parameter.encoder_resolution;
#ifdef ODOM_DEBUG_OUTPUT
printf(" %ld ", long(dis[i]*1000000));
#endif
}
//通過(guò)使用的模型接口到當(dāng)前里程信息
model->get_odom(&odom, dis, CALC_ODOM_INTERVAL);
#ifdef ODOM_DEBUG_OUTPUT
printf(" x=%ld y=%ld yaw=%ld", long(odom.x*1000), long(odom.y*1000), long(odom.z*1000));
printf("\r\n");
#endif
//更新至數(shù)據(jù)存儲(chǔ)中
Data_holder::get()->odom.v_liner_x = odom.vel_x*100;
Data_holder::get()->odom.v_liner_y = odom.vel_y*100;
Data_holder::get()->odom.v_angular_z = odom.vel_z*100;
Data_holder::get()->odom.x = odom.x*100;
Data_holder::get()->odom.y = odom.y*100;
Data_holder::get()->odom.yaw = odom.z*100;
}
}
