ROS機器人底盤(7)-PIBOT的Firmware的代碼分析(1)

包列表

包 說明 備注
AFMotor Adafruit shield電機驅動板驅動
Board 板子資源接口及Mega2560中的實現 0
DataHolder 關鍵數據存儲類 0
Encoder 編碼器接口及Mega2560中編碼器實現 0
KinematicModels 機器人模型類 針對不同型號機型的解算
Motor 電機驅動接口類及AF電機驅動與一般驅動板的實現 0
PID PID運算類 0
Transport 通訊接口類與實現 0
robot robot調度類 0

main分析

直接貼源碼，無需贅述

#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"

//根據機器人模型選擇相應的解算類頭文件
#if ROBOT_MODEL == ROBOT_MODEL_DIFF//差分輪
#include "differential.h"
#endif

#if ROBOT_MODEL == ROBOT_OMNI_3//全向三輪
#include "omni3.h"
#endif

class MotorController;//電機控制器接口類
class Encoder;          //編碼器接口類
class PID;              //PID運算接口類
class Transport;        //通訊接口類
class Dataframe;        //通訊協(xié)議數據包接口類
class Model;            //機器人模型接口類

class Robot:public Notify{
    public:
        //單例模式
        static Robot* get(){
            static Robot robot;
            return &robot;
        }
        
        //初始化
        void init();

        //檢測上位機命令
        void check_command();
        
        //運動學控制，下發(fā)的速度轉換為各個輪子速度平且進行PID控制
        void do_kinmatics();
        
        //根據個輪子編碼器反饋給出機器人位置姿態(tài)及實時速度信息
        void calc_odom();

        //Notify接口實現， 針對某些消息關注的回調函數
        void update(const MESSAGE_ID id, void* data);
    private:
        Robot(){}
        void init_motor();//初始化電機相關
        void init_trans();//初始化通訊相關
    private:
        void clear_odom();//清除累計的位置姿態(tài)信息
        void update_velocity();//更新下發(fā)的實時控制速度
    private:
        MotorController* motor[MOTOR_COUNT];//電機控制器接口
        Encoder*        encoder[MOTOR_COUNT];//編碼器接口
        PID*            pid[MOTOR_COUNT];
        float           input[MOTOR_COUNT];//PID間隔時間內期望的encoder增加或減少的個數
        float           feedback[MOTOR_COUNT];//PID間隔時間內反饋的encoder增加或減少的個數

        Transport*      trans;//通訊接口
        Dataframe*      frame;//通訊數據包接口
        Model*          model;//機器人模型接口
        bool            do_kinmatics_flag; //進行運動控制的標記

        Odom            odom;//機器人位置姿態(tài)實時速度信息

        unsigned long   last_velocity_command_time;//上次下發(fā)速度的時間點
};

#endif

robot.cpp關鍵代碼

初始化

void Robot::init(){
    Data_holder::get()->load_parameter();//加載EPPROM中的配置機器人信息

#if DEBUG_ENABLE
    Board::get()->usart_debug_init();//調試串口初始化 printf通過串口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");
}

電機及編碼器相關初始化

void Robot::init_motor(){

#if MOTOR_COUNT>0//根據配置的個數定義編碼器、電機控制器及PID的具體實現，MOTOR_COUNT在具體的機器人模型中定義
    #if MOTOR_CONTROLLER == COMMON_CONTROLLER//根據使用的電機控制器選擇電機控制器的實現 這里CommonMotorController與AFSMotorController都MotorController接口類的實現
    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接口類的實現
    static EncoderImp encoder1(MOTOR_1_ENCODER_A_PIN, MOTOR_1_ENCODER_B_PIN);
    //PID計算接口，給定參數為期望的數據地址、反饋的數據地址以及各個PID參數值
    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;//接口指向具體實現
    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//根據配置的機器人模型選擇解算類的實現，這個Differential與Omni3是運動解算接口Model的實現
    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

    //初始化電機驅動器
    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;
}

通訊相關初始化

void Robot::init_trans(){
    static USART_transport _trans(MASTER_USART, 115200);//使用串口作為通訊接口
    static Simple_dataframe    _frame(&_trans);//使用Simple_dataframe協(xié)議數據包實現數據打包解包
    trans = &_trans;
    frame = &_frame;

    trans->init();
    frame->init();

    //注冊相關消息的通知， 收到該消息this->update回調會被調用
    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)){//從通訊口中讀取數據
        //printf("%02x ", ch);
        if (frame->data_recv(ch)){//使用數據包接收和解析數據
            //printf("\r\n");
            frame->data_parse();
        }
    }
}

運動處理


void Robot::do_kinmatics(){
    if (!do_kinmatics_flag){//該標記收到上位的命令會置true， 超時停止會置false
        for(int i=0;i<MOTOR_COUNT;i++){
            pid[i]->clear();//停止后清除pid變量值
            encoder[i]->get_increment_count_for_dopid();//讀取掉停止時編碼器的變化至，放置手動轉動電機導致下次啟動pid時的異常
        }
        return;
    }
    
    static unsigned long last_millis=0;
    //根據配置PID間隔時間進行pid運算
    if (Board::get()->get_tick_count()-last_millis>=Data_holder::get()->parameter.do_pid_interval){
        last_millis = Board::get()->get_tick_count();
        //得到PID間隔時間反饋編碼器的值
        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
        //判斷超時，則無需繼續(xù)PID運算
        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{
            //計算得到輸出PWM  input[i]在update回調通知中給定
            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
        //控制各個電機
        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;
            }
        }
    }
}

計算里程位置姿態(tài)

void Robot::calc_odom(){

    static unsigned long last_millis=0;
    //每間CALC_ODOM_INTERVAL隔時間計算
    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};
        //得到間隔時間內個輪子行徑的距離(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
        }
        
        //通過使用的模型接口到當前里程信息
        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
        //更新至數據存儲中
        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;
    }
}
最后編輯于 ：
?著作權歸作者所有,轉載或內容合作請聯系作者
【社區(qū)內容提示】社區(qū)部分內容疑似由AI輔助生成，瀏覽時請結合常識與多方信息審慎甄別。
平臺聲明：文章內容（如有圖片或視頻亦包括在內）由作者上傳并發(fā)布，文章內容僅代表作者本人觀點，簡書系信息發(fā)布平臺，僅提供信息存儲服務。

相關閱讀更多精彩內容

  • tess4.0中主要的數據結構 Page analysis result: PAGE_RES (ccstruct/...
    RobertY閱讀 2,651評論 1贊 6
  • 2017年6月7日 星期三 晴(農歷五月十四) 放學接了譽仔之后，讓他和他的好朋友仲浩宇多玩了一會，想著...
    譽仔媽媽閱讀 311評論 8贊 3
  • 九月金秋，歲在戊子。慕學后生，齊聚燕京。故都涵若，納八方之俊秀，容四海之賢才。吾輩小子，忝居于此，求學書院，習為師...
    柴子恒閱讀 377評論 0贊 0
  • 你走過 倩影 似夢中走來 迷醉煙火 夜色 揮灑著銀灰 掩埋寂寞 那一刻 心跳 不受控制著 步伐 不自覺跟著 江山如...
    夕辰沭閱讀 557評論 2贊 3

友情鏈接更多精彩內容