大家好，在下又回來了。今天開一個(gè)新坑，算是小小地總結(jié)一下之前的工作。

在這個(gè)系列教程中，我會(huì)嘗試教大家一步一步從底層開始，構(gòu)建屬于自己的移動(dòng)機(jī)器人。為了開發(fā)的簡單方便，上層使用了裝有ROS（robot operatingsystem）的linux板卡和臺(tái)式電腦（臺(tái)式機(jī)），而下層使用了STM32F407作為MCU的嵌入式系統(tǒng)，并且使用了開源項(xiàng)目HANDS-FREE[2]的大部分代碼并在此基礎(chǔ)上做了一些移植工作。

直入主題。輪式機(jī)器人可以簡單定義為以輪子作為運(yùn)動(dòng)機(jī)構(gòu)的移動(dòng)機(jī)器人。可以將輪式機(jī)器人分為兩輪（平衡車），三輪和四輪，當(dāng)然還有六輪的形式。其實(shí)都大同小異。

下表引自[1]。

除了兩輪車外，其他輪式的移動(dòng)機(jī)器人都是天然魯棒系統(tǒng)，所以這對(duì)高性能控制的要求幾乎為零。

明顯地，這是一個(gè)項(xiàng)目教程，因此我將以三輪全向輪式移動(dòng)機(jī)器人為例，講一講如何從零完成一個(gè)機(jī)器人項(xiàng)目。

第一部分：機(jī)械

淘寶搜索“全向移動(dòng)機(jī)器人”，本例中用的是這款

注意，電機(jī)為普通的直流電機(jī)，因?yàn)槭鞘覂?nèi)移動(dòng)機(jī)器人，機(jī)器人的速度小于1m/s，速度不需要很高。要求每個(gè)電機(jī)上都要有里程計(jì)（編碼器），目的是獲得小車上每個(gè)電機(jī)的里程與比較粗糙的速度信息。

第二部分：嵌入式系統(tǒng)-硬件

為了解除廣告之嫌，我不會(huì)放鏈接。如果有興趣請(qǐng)留言聯(lián)系我。

首先，為了要控制電機(jī)，需要兩到三個(gè)電機(jī)驅(qū)動(dòng)。我選擇的是具有L298N邏輯的雙路電機(jī)驅(qū)動(dòng)，滿足三個(gè)電機(jī)的控制要求。淘寶上隨便找種L298N模塊即可。

電源系統(tǒng)：需要給單片機(jī)最小系統(tǒng)板提供5v供電，僅需要一個(gè)能提供5v的LM25XX開關(guān)電源模塊即可。非常輕松愉快。

單片機(jī)最小系統(tǒng)板：一個(gè)STM32F407VET6最小系統(tǒng)板即可。需要板上自帶能提供3.3v的LDO，還有三到四個(gè)開關(guān)和兩盞LED，可以不需要RTC電池。

在Embedded/doc/下會(huì)此版的原理圖。

傳感器：最少需要一個(gè)MPU6050模塊，一個(gè)HMC5883L模塊。這是為了融合出機(jī)器人的yaw，這是三個(gè)機(jī)器人的狀態(tài)變量之一。單獨(dú)一個(gè)MPU6050測(cè)出的yaw角度會(huì)隨著時(shí)間漂移，需要HMC5883磁力計(jì)矯正。最好可以有一個(gè)到兩個(gè)普通的超聲波測(cè)距模塊（如HC-SR04）,主要作為壁障用。下圖是集成了MPU6050，HMC5883，BMP180的GY-87模塊

電池：12V動(dòng)力鋰電池即可。

其他：①舵機(jī)：普通數(shù)字舵機(jī)或者高級(jí)的帶串口的數(shù)字舵機(jī)均可，可以組成簡單機(jī)械臂或者機(jī)器人頭；

第三部分：嵌入式系統(tǒng)-軟件

本例中的代碼結(jié)構(gòu)和大部分的源代碼來自開源工程HANDS-FREE。它是一個(gè)由西北工大的團(tuán)隊(duì)維護(hù)的開源項(xiàng)目。在本文中，我會(huì)比較詳盡的對(duì)代碼進(jìn)行講解。

首先整體介紹以下嵌入式軟件部分。這里沒有使用實(shí)時(shí)操作系統(tǒng)。開發(fā)環(huán)境為keil-ARM V5.17+ windows 10 enterprise。使用C++編程。

[這里為嵌入式軟件整體功能以及算法流程圖]

工程文件結(jié)構(gòu)如下

USER文件夾內(nèi)主要是用戶文件。Start_Code文件夾內(nèi)主要是底層庫函數(shù)代碼。而BSP文件夾內(nèi)放有用戶庫函數(shù)。剩下的就是外設(shè)功能包。

首先介紹一下主函數(shù)。

//main.cpp
int main(void)
{         
    constructorInit();
    systemInit();
    while(1)
    {

        if(usart1_queue.emptyCheck()==0){
            hf_link_pc_node.byteAnalysisCall(usart1_queue.getData());
        }
        if ( board.cnt_1ms >= 1 )      // 1000hz
        {
            board.cnt_1ms=0;
            imu.topCall();             // stm32f4 -- 631us(fpu)
        }
        if ( board.cnt_2ms >= 2 )      // 500hz
        {
            board.cnt_2ms=0;
        }
        if ( board.cnt_5ms >= 5 )      // 200hz
        {
            board.cnt_5ms=0;
        }
        if ( board.cnt_10ms >= 10 )    // 100hz
        {
            board.cnt_10ms=0;
            board.boardBasicCall();    // need time stm32f1  35us
        }
        if ( board.cnt_20ms >= 20 )    // 50hz
        {
            board.cnt_20ms = 0 ;
            motor_top.motorTopCall();  //motor speed control

        }
        if ( board.cnt_50ms >= 50 )    // 20hz
        {
            board.cnt_50ms = 0 ;
            //robot_head.headTopCall();
            hands_free_robot.robotWheelTopCall();  //robot control interface

        }
        if ( board.cnt_500ms >= 500 )    // 2hz
        {        
            board.cnt_500ms = 0 ;
            //robot_head.headTopCall();
            board.setLedState(0,2);
            

                }

    }

    return 0;
}

可以大致看到整個(gè)機(jī)器人底層的執(zhí)行流程：先進(jìn)行系統(tǒng)架構(gòu)的初始化和系統(tǒng)外設(shè)初始化。然后進(jìn)入主循環(huán)：每一毫秒都會(huì)檢查接收串口1數(shù)據(jù)的隊(duì)列是否有數(shù)，如果有數(shù)據(jù)就對(duì)數(shù)據(jù)幀作分析，否則就更新imu的數(shù)據(jù)并處理（或者只將數(shù)據(jù)更新之后交由上位機(jī)處理）；每10ms更新一次系統(tǒng)數(shù)據(jù)（CPU溫度，系統(tǒng)時(shí)間和電池電壓）；每20ms控制一次電機(jī)（對(duì)電機(jī)底層的控制）；每50ms控制一次機(jī)器人運(yùn)動(dòng)（對(duì)機(jī)器人姿態(tài)的控制）；每500ms翻轉(zhuǎn)一下led電平（指示系統(tǒng)正在運(yùn)行）。

下面簡單講解一下系統(tǒng)架構(gòu)初始化和系統(tǒng)外設(shè)初始化函數(shù)。

下面是系統(tǒng)外設(shè)初始化。

//main.cpp
void constructorInit(void)
{
    board = Board();
    Ultrasonic = ULTRASONIC();
    my_robot = RobotAbstract();
    motor_top = MotorTop();
    //robot_head = HeadAX();
    hands_free_robot = RobotWheel();
    //sbus = SBUS();
    imu = IMU();
    usart1_queue = Queue();
    hf_link_pc_node =  HFLink(0x11,0x01,&my_robot);
    hf_link_node_pointer=&hf_link_pc_node;
}

這里將描述系統(tǒng)外設(shè)的所有抽象類進(jìn)行初始化，我將以board類為例大致講一講C++在嵌入式編程中的具體運(yùn)用，因?yàn)榇_實(shí)好用。

//board.h
class Board
{
public:
    float cpu_temperature;
    float cpu_usage;
    float battery_voltage;
    float system_time;   //system working time (unit:us), start after power-on
    uint8_t system_init; //state of system: 0-->not initialize  1-->initialized
    uint16_t cnt_1ms;
    uint16_t cnt_2ms;
    uint16_t cnt_5ms;
    uint16_t cnt_10ms;
    uint16_t cnt_20ms;
    uint16_t cnt_50ms;
    uint16_t cnt_500ms;
    uint32_t chipUniqueID[3];
    uint16_t flashSize;    //Unit: KB

public:
    Board();
    void boardBasicInit(void);
    void boardBasicCall(void);
    /**********************************************************************************************************************/
    void debugPutChar(uint8_t tx_byte_);  //system support functions
    void setLedState(uint8_t led_id, uint8_t operation);
    void setBeepState(uint8_t operation);
    uint8_t getKeyState(uint8_t key_id){return key_state[key_id] ;}
    /**********************************************************************************************************************/
    void motorInterfaceInit(uint8_t motor_id , float motor_pwm_T); //package "PAKG_MOTOR" support functions
    void motorEnable(uint8_t motor_id);
    void motorDisable(uint8_t motor_id);
    void motorEnableForward(uint8_t motor_id);
    void motorEnableBackward(uint8_t motor_id);
    float getMotorEncoderCNT(uint8_t motor_id);
    float getMotorCurrent(uint8_t motor_id);
    void motorSetPWM(uint8_t motor_id , int pwm_value);
    /**********************************************************************************************************************/
    void axServoInterfaceInit(void); //package " PAKG_SERVO" support  functions
    void axServoTxModel(void);
    void axServoRxModel(void);
    void axServoSendTxByte(uint8_t tx_byte);
    uint8_t axServoGetRxByte(uint8_t *error);
    /**********************************************************************************************************************/
    void sbusInterfaceInit(void); //package " SBUS_PPM" support  functions
    void ppmInterfaceInit(void);
    /**********************************************************************************************************************/
                                  //package " ANALOG_SERVO" support  functions
    void analogServoInterfaceInit(void); 
    void analogServoSetPWM(uint8_t servo_id , int pwm_value);
    /**********************************************************************************************************************/
    void imuI2CInit(void);       //package "PAKG_IMU" support  functions
    void imuI2CWriteByte(uint8_t equipment_address , uint8_t reg_address ,
                         uint8_t reg_data , uint8_t fastmode);
    uint8_t imuI2CReadByte(uint8_t equipment_address , uint8_t reg_address , uint8_t fastmode);
    uint8_t imuI2CWriteBuf(uint8_t equipment_address,uint8_t reg_address,
                        uint8_t* pt_char , uint8_t size , uint8_t fastmode);
    uint8_t imuI2CReadBuf(uint8_t equipment_address,uint8_t reg_address,
                       uint8_t * pt_char , uint8_t size , uint8_t fastmode);

    void gpsInterfaceInit(void);
    void gpsSendTxByte(uint8_t tx_byte);
    /**********************************************************************************************************************/
    void eepromI2CInit(void); //package "AT24Cxx" support functions
    void eepromI2CWriteByte(uint8_t equipment_address , uint8_t reg_address ,
                            uint8_t reg_data , uint8_t fastmode);
    uint8_t eepromI2CReadByte(uint8_t equipment_address , uint8_t reg_address , uint8_t fastmode);
    uint8_t eepromI2CWriteBuf(uint8_t equipment_address,uint8_t reg_address,
                           uint8_t* pt_char , uint8_t size , uint8_t fastmode);
    uint8_t eepromI2CReadBuf(uint8_t equipment_address,uint8_t reg_address,
                          uint8_t * pt_char , uint8_t size , uint8_t fastmode);
    /**********************************************************************************************************************/
    //extend interface support functions
    void extendI2CInit(void);   // extend iic interface is using for extend other sensors
    void extendI2CWriteByte(uint8_t equipment_address , uint8_t reg_address ,
                            uint8_t reg_data , uint8_t fastmode);
    uint8_t extendI2CReadByte(uint8_t equipment_address , uint8_t reg_address , uint8_t fastmode);
    uint8_t extendI2CWriteBuf(uint8_t equipment_address,uint8_t reg_address,
                           uint8_t* pt_char , uint8_t size , uint8_t fastmode);
    uint8_t extendI2CReadBuf(uint8_t equipment_address,uint8_t reg_address,
                          uint8_t* pt_char , uint8_t size , uint8_t fastmode);

    //extend spi interface
    //extend can interface
    //extend PWM interface
    //mode=1 high frequency control   ; mode = 2 for  electric modulation
    void pwmInterfaceInit(uint8_t mode , float pwm_t);
    void setPWMValue(uint8_t channel_x , float pwm_value);  //channel_x 1~6
    /**********************************************************************************************************************/\

private:
    float battery_voltage_;
    float battery_voltage_alarm_ ;
    float battery_proportion_ ;
    float temperature_;
    uint16_t beep_alarm_cnt_ ;
    uint16_t board_call_i , board_call_j , board_call_k;
    uint8_t key_state[5];

    void systemMonitoring(void);
    void ledInit(void);
    void keyInit(void);
    void keyStateRenew(void);
    void beepInit(void);

    void debugInterfaceInit(void);
    float getBatteryVoltage(void);
    float getCPUUsage(void);
    float getCPUTemperature(void);
    void getCPUInfo(void);
};

extern Board board;

C++是一種面向?qū)ο蟮木幊陶Z言，C++ 在 C 語言的基礎(chǔ)上增加了面向?qū)ο缶幊蹋珻++ 支持面向?qū)ο蟪绦蛟O(shè)計(jì)。類是 C++ 的核心特性，通常被稱為用戶定義的類型。[3]

類本身是抽象的。定義一個(gè)類本質(zhì)上是定義了一個(gè)數(shù)據(jù)類型的藍(lán)圖。程序員將一種事物定義為一種類，然后把這個(gè)類的屬性與能進(jìn)行的操作包含在類下。如此例，定義board為描述主控板的類。public標(biāo)識(shí)符下的數(shù)據(jù)成員為共有的，也就是說其他的類可以調(diào)用這些數(shù)據(jù)成員。而private下則是私有的，也就是說這些數(shù)據(jù)成員只能在這個(gè)類里面使用。主要的原因是C++數(shù)據(jù)封裝與信息隱藏的功能。在大型項(xiàng)目的開發(fā)中，代碼應(yīng)該簡明并且合理，應(yīng)該在類里完成的工作就讓他在類里面完成就行了。

在這個(gè)類下，定義了板子的初始化函數(shù)（這里初始化了單片機(jī)的時(shí)鐘和開發(fā)板的外設(shè)，如定時(shí)器、key、led、蜂鳴器、ADC模塊等）

//control_unit_v2.cpp
void Board::boardBasicInit(void)
{
    int i;
    for(i=0;i<0x8fff;i++);

    HF_System_Timer_Init();         //Initialize the measurement systemm
    debugInterfaceInit();

    ledInit();
    keyInit();
    beepInit();

    //HF_RTC_Init();                //Initialize the RTC, if return 0:failed,else if return 1:succeeded
    //HF_IWDG_Init();               //Initialize the independed watch dog, system will reset if not feeding dog over 1s
    HF_ADC_Moder_Init(0X3E00 , 5 , 2.5f);  //ADC init
    HF_Timer_Init(TIM6 , 1000);     //timer6 init , 1000us

    setBeepState(1);
    delay_ms(500);
    setBeepState(0);
    analogServoInterfaceInit();
}

后面還提供了可供用戶調(diào)用的其他外設(shè)包的函數(shù)，比如設(shè)定led狀態(tài)的

    void setLedState(uint8_t led_id, uint8_t operation);

和設(shè)定使能和失能電機(jī)的

    void motorEnable(uint8_t motor_id);
    void motorDisable(uint8_t motor_id);

其他類的具體實(shí)現(xiàn)可以參考其源碼。

回到主函數(shù)，我們這時(shí)其實(shí)還沒有進(jìn)行任何初始化，只是對(duì)類進(jìn)行了個(gè)“抽象”的初始化（具體的初始化還沒有實(shí)現(xiàn)），接下來就開始進(jìn)行真正的硬件外設(shè)初始化。

//main.cpp
void systemInit(void)
{
    //INTX_DISABLE();  //close all interruption
    board.boardBasicInit();
    motor_top.motorTopInit(3 , 1560 , 0.02 , 0);
    //robot_head.axServoInit();
    hands_free_robot.robotWheelTopInit();
    //sbus.sbusInit();
    imu.topInit(1,1,1,0,0,1);
    Ultrasonic.Ultra_Init(1);
    //INTX_ENABLE();     //enable all interruption
    printf("app start \r\n");
}

這里的所有函數(shù)除了printf，都是各種外設(shè)的初始化。上面已經(jīng)提到了boardBasicInit()，那就以Motor_top類下的motorTopInit()為例吧。

//motor_top.cpp
void MotorTop::motorTopInit(float motor_enable_num_ , float motor_encoder_num_ ,
                            float motor_pid_t_  , unsigned char motor_simulation_model_)
{
    motor_enable_num = motor_enable_num_;
    motor_encoder_num = motor_encoder_num_;
    motor_pid_t = motor_pid_t_;

    board.motorInterfaceInit(1, motor_pwm_max); //motor_x init
    motor1.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
    motorPWMRenew(1,0);

    board.motorInterfaceInit(2, motor_pwm_max);
    motor2.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
    motorPWMRenew(2,0);

    if(motor_enable_num >= 3)
    {
        board.motorInterfaceInit(3, motor_pwm_max);
        motor3.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
        motorPWMRenew(3,0);
    }

    if(motor_enable_num >= 4)
    {
        board.motorInterfaceInit(4, motor_pwm_max);
        motor4.motorControlInit(motor_pid_t_ , motor_encoder_num_ , motor_pwm_max , motor_simulation_model_);
        motorPWMRenew(4,0);
    }
}

這個(gè)函數(shù)有兩大功能，一是初始化STM32的TIM1，二是初始化里程計(jì)（編碼器）用到的TIM2、TIM3、TIM4定時(shí)器。具體的實(shí)現(xiàn)函數(shù)為motorInterfaceInit()

//control_unit_v2.cpp
void Board::motorInterfaceInit(uint8_t motor_id , float motor_pwm_T)
{
    GPIO_InitTypeDef  GPIO_InitStructure;
    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;

    if(motor_id == 1 ){
        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE , ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_10;
        GPIO_Init(GPIOE , &GPIO_InitStructure);
        GPIO_ResetBits(GPIOE , GPIO_Pin_8);
        GPIO_ResetBits(GPIOE , GPIO_Pin_10);
        HF_Encoder_Init(TIM2,0);   //encoder init for PID speed control
    }
    else if(motor_id == 2){
        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC , ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
        GPIO_Init(GPIOC , &GPIO_InitStructure);
        GPIO_ResetBits(GPIOC , GPIO_Pin_0);
        GPIO_ResetBits(GPIOC , GPIO_Pin_1);
        HF_Encoder_Init(TIM3,2);
    }
    else if(motor_id == 3){
        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC , ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;
        GPIO_Init(GPIOC , &GPIO_InitStructure);
        GPIO_ResetBits(GPIOC , GPIO_Pin_2);
        GPIO_ResetBits(GPIOC , GPIO_Pin_3);
        HF_Encoder_Init(TIM4,1);
    }
    else if(motor_id == 4){
        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE , ENABLE);
        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
        GPIO_Init(GPIOE , &GPIO_InitStructure);
        GPIO_ResetBits(GPIOE , GPIO_Pin_15);
        HF_Encoder_Init(TIM5,0);
    }

    //motor_pwm_T = 5000 , TIM1 motor pwm frequency  = (168M/4) / motor_pwm_T  = 16.8K
    HF_PWMOut_Init(TIM1 , 2-1 , motor_pwm_T , 1);

//    //motor_pwm_T = 5000 , TIM9 motor pwm frequency  = (168M/4) / motor_pwm_T = 16.8K
//    HF_PWMOut_Init(TIM9 , 2-1 , motor_pwm_T , 0);

//    //motor_pwm_T = 5000 , TIM12 motor pwm frequency = (84M/2) / motor_pwm_T  = 16.8K
//    HF_PWMOut_Init(TIM12 , 0 , motor_pwm_T , 0);
}

因?yàn)槭褂昧薒298N邏輯的電機(jī)驅(qū)動(dòng)，所以一路電機(jī)需要兩個(gè)GPIO和一路PWM控制，因此上面這個(gè)函數(shù)主要是初始化使用的gpio和pwm功能的定時(shí)器TIM1，因?yàn)門IM1能發(fā)出四路不同的PWM信號(hào)，因此，三個(gè)電機(jī)僅僅需要一個(gè)TIM1。編碼器也用到了TIM，具體的代碼解析就不做了，[4]為介紹STM32編碼器接口庫函數(shù)的解析文檔，講得不錯(cuò)。

回到systemInit函數(shù)。剩下分別初始化了motorTop的一些pid參數(shù)。這個(gè)函數(shù)屬于RobotWheel類，這個(gè)類主要包含了一些控制機(jī)器人姿態(tài)和運(yùn)動(dòng)的接口函數(shù)。它主要將上層發(fā)出的期望機(jī)器人速度或位置信號(hào)轉(zhuǎn)換成三個(gè)電機(jī)的控制信號(hào)。imu.topInit()主要完成了IMU的初始化。Ultra_Init完成了超聲波模塊的初始化。

接下來開始程序主循環(huán)的介紹。

首先每次都檢查隊(duì)列（queue）中的數(shù)據(jù)，調(diào)用queue.emptyCheck()。因?yàn)槊看未谑盏綌?shù)據(jù)都會(huì)進(jìn)入串口中斷接收數(shù)據(jù)并保存在隊(duì)列中，具體的代碼實(shí)現(xiàn)如下：

//stm32f4xx_it.cpp
void USART1_IRQHandler(void)
{

    unsigned char data;
    if(USART1->SR&(1<<5))
    {
        data=USART1->DR;
        if(usart1_queue.fullCheck()==0){
            usart1_queue.putData(data);
        }
        USART_ClearITPendingBit(USART1, USART_IT_RXNE);   // clear interrupt flag
    }

如果檢測(cè)到隊(duì)列中有數(shù)，就要開始對(duì)數(shù)據(jù)幀做解析了，實(shí)現(xiàn)函數(shù)如下。這個(gè)函數(shù)先將協(xié)議去除，獲得上位機(jī)發(fā)出的命令所代表的字，然后在通過調(diào)用packageAnalysis()對(duì)命令一一對(duì)應(yīng)地做出相應(yīng)。

//hf_link.cpp
unsigned char HFLink::byteAnalysisCall(const unsigned char rx_byte)
{
    unsigned char package_update=0;
    unsigned char receive_message_update=0;
    receive_message_update=receiveFiniteStates(rx_byte) ;  //Jump communication status
    if( receive_message_update ==1 )
    {
        receive_message_update = 0;
        receive_message_count++;
        package_update=packageAnalysis();
        if(package_update==1) receive_package_count++;
        return package_update;
    }
    return 0;
}

下面就是去除協(xié)議幀的函數(shù)。它是通過一個(gè)狀態(tài)機(jī)來解析協(xié)議的。

//hf_link.cpp
unsigned char HFLink::receiveFiniteStates(const unsigned char rx_data)
{

    switch (receive_state_)    //協(xié)議解析狀態(tài)機(jī)
    {
    case WAITING_FF1:
        if (rx_data == 0xff)   //接收到幀頭第一個(gè)數(shù)據(jù)
        {
            receive_state_ = WAITING_FF2;
            receive_check_sum_ =0;
            receive_message_length_ = 0;
            byte_count_=0;
            receive_check_sum_ += rx_data;    
        }
        break;　　　　　　　　　　//狀態(tài)機(jī)復(fù)位

    case WAITING_FF2:
        if (rx_data == 0xff)　　　　//接收到幀頭第二個(gè)數(shù)據(jù)，下面以此類推
        {
            receive_state_ = SENDER_ID;
            receive_check_sum_ += rx_data;
        }
        else
            receive_state_ = WAITING_FF1;
        break;

    case SENDER_ID:
        rx_message_.sender_id = rx_data ;
        if (rx_message_.sender_id == friend_id)  //id check
        {
            receive_check_sum_ += rx_data;
            receive_state_ = RECEIVER_ID;
        }
        else
        {

            receive_state_ = WAITING_FF1;
        }
        break;

    case RECEIVER_ID:
        rx_message_.receiver_id = rx_data ;
        if (rx_message_.receiver_id == my_id)  //id check
        {
            receive_check_sum_ += rx_data;
            receive_state_ = RECEIVE_LEN_H;
        }
        else
        {

            receive_state_ = WAITING_FF1;
        }
        break;

    case RECEIVE_LEN_H:
        receive_check_sum_ += rx_data;
        receive_message_length_ |= rx_data<<8;
        receive_state_ = RECEIVE_LEN_L;
        break;

    case RECEIVE_LEN_L:
        receive_check_sum_ += rx_data;
        receive_message_length_ |= rx_data;
        rx_message_.length = receive_message_length_;
        receive_state_ = RECEIVE_PACKAGE;
        break;

    case RECEIVE_PACKAGE:
        receive_check_sum_ += rx_data;
        rx_message_.data[byte_count_++] = rx_data;
        if(byte_count_ >= receive_message_length_)
        {
            receive_state_ = RECEIVE_CHECK;
            receive_check_sum_=receive_check_sum_%255;
        }
        break;

    case RECEIVE_CHECK:
        if(rx_data == (unsigned char)receive_check_sum_)   //對(duì)比發(fā)送和接收的校驗(yàn)和，如果不一致直接放棄此數(shù)據(jù)幀
        {
            receive_check_sum_=0;
            receive_state_ = WAITING_FF1;
            return 1 ;　　　　　　　　　　　　　　　　　　　　　//傳回1，表示一個(gè)數(shù)據(jù)包收到
        }
        else
        {
            receive_state_ = WAITING_FF1;
        }
        break;
    default:
        receive_state_ = WAITING_FF1;
    }

    return 0;
}

這里的協(xié)議為：0XFF 0XFF sender_id receiver_id length_H length_L ****(data) check_sum。

解析完協(xié)議就要開始分析命令了。由packageAnalysis()來實(shí)現(xiàn)這一步工作。

//hf_link.cpp

unsigned char HFLink::packageAnalysis(void)
{

    unsigned char analysis_state=0;
    void* single_command;

    command_state_ = (Command)rx_message_.data[0];

    if (hf_link_node_model== 0)  //the slave need to check the SHAKING_HANDS"s state
    {
        if(shaking_hands_state==0 && command_state_ != SHAKING_HANDS) //if not  shaking hands
        {
            sendStruct(SHAKING_HANDS  , (unsigned char *)single_command, 0);
            return 1;
        }
    }

    switch (command_state_)
    {
    case SHAKING_HANDS :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_global_coordinate , sizeof(my_robot->measure_global_coordinate));
        break;

    case READ_ROBOT_SYSTEM_INFO :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->robot_system_info , sizeof(my_robot->robot_system_info));
        break;

    case SET_GLOBAL_SPEED :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_global_speed , sizeof(my_robot->expect_global_speed));
        break;

    case READ_GLOBAL_SPEED :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_global_speed , sizeof(my_robot->measure_global_speed));
        break;

    case SET_ROBOT_SPEED :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_robot_speed , sizeof(my_robot->expect_robot_speed));
        break;

    case READ_ROBOT_SPEED :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_robot_speed , sizeof(my_robot->measure_robot_speed));
        break;

    case SET_MOTOR_SPEED :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_motor_speed, sizeof(my_robot->expect_motor_speed));
        break;

    case READ_MOTOR_SPEED :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_motor_speed , sizeof(my_robot->measure_motor_speed));
        break;

    case READ_MOTOR_MILEAGE :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_motor_mileage , sizeof(my_robot->measure_motor_mileage));
        break;

    case READ_GLOBAL_COORDINATE :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_global_coordinate , sizeof(my_robot->measure_global_coordinate));
        break;

    case READ_ROBOT_COORDINATE :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_robot_coordinate , sizeof(my_robot->measure_robot_coordinate));
        break;

    case CLEAR_COORDINATE_DATA :
        if (hf_link_node_model==0)
        {
            sendStruct(command_state_ , (unsigned char *)single_command , 0);
            analysis_state=1;
            receive_package_renew[(unsigned char)command_state_] = 1 ;
        }
        break;

    case SET_ARM_1 :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_arm1_state, sizeof(my_robot->expect_arm1_state));
        break;

    case READ_ARM_1 :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_arm1_state , sizeof(my_robot->measure_arm1_state));
        break;

    case SET_ARM_2 :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_arm2_state, sizeof(my_robot->expect_arm2_state));
        break;

    case READ_ARM_2 :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_arm2_state , sizeof(my_robot->measure_arm2_state));
        break;

    case SET_HEAD_1 :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_head1_state , sizeof(my_robot->expect_head1_state ));
        break;

    case READ_HEAD_1 :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_head1_state , sizeof(my_robot->measure_head1_state));
        break;

    case SET_HEAD_2 :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_head2_state, sizeof(my_robot->expect_head2_state));
        break;

    case READ_HEAD_2 :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_head2_state , sizeof(my_robot->measure_head2_state));
        break;

    case READ_IMU_DATA :
        analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_imu_euler_angle , sizeof(my_robot->measure_imu_euler_angle));
        break;

    case SET_ROBOY_PARAMETERS :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->robot_parameters, sizeof(my_robot->robot_parameters));
        break;

    case SAVE_ROBOY_PARAMETERS :
        if (hf_link_node_model==0)
        {
            sendStruct(command_state_ , (unsigned char *)single_command , 0);
            analysis_state=1;
            receive_package_renew[(unsigned char)command_state_] = 1 ;
        }
        break;

    case SET_MOTOR_PARAMETERS :
        analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->motor_parameters, sizeof(my_robot->motor_parameters));
        break;

    case SAVE_MOTOR_PARAMETERS :
        if (hf_link_node_model==0)
        {
            sendStruct(command_state_ , (unsigned char *)single_command , 0);
            analysis_state=1;
            receive_package_renew[(unsigned char)command_state_] = 1 ;
        }
        break;

    default :
        analysis_state=0;
        break;

    }

    rx_message_.sender_id=0;    //clear flag
    rx_message_.receiver_id=0;
    rx_message_.length=0;
    rx_message_.data[0]=0;

    return analysis_state;
}

上位機(jī)在一開始時(shí)通常會(huì)發(fā)一幀握手信號(hào)，然后開始發(fā)送數(shù)據(jù)。然后在switch結(jié)構(gòu)里，對(duì)應(yīng)每一個(gè)命令都有相應(yīng)的代碼做處理。大致可以將處理分為設(shè)定數(shù)據(jù)（setCommandAnalysis）和上傳數(shù)據(jù)（readCommandAnalysis）兩個(gè)函數(shù)。

結(jié)束后返回分析狀態(tài)，標(biāo)志命令更新。在上述兩個(gè)函數(shù)里，還有對(duì)應(yīng)的發(fā)送數(shù)據(jù)的函數(shù)，詳細(xì)請(qǐng)參考源碼。

將收到的數(shù)據(jù)獲得后，主循環(huán)進(jìn)入了實(shí)際的控制模式。

首先最需要處理是IMU數(shù)據(jù)。下面的代碼將陀螺儀數(shù)據(jù)的傳輸頻率設(shè)定為500hz，bmp085為100hz。只是將數(shù)據(jù)傳輸給上位機(jī)，并沒有做融合處理。

//imu_top.cpp
void IMU::topCall(void)
{

    imu_call_1++;
    imu_call_2++;
    imu_call_3++;
    imu_call_4++;
    imu_call_5++;

    if( imu_call_1 >= 2 ) //500HZ
    {
        imu_call_1=0;
        mpu6050.dataUpdate();
    }

    if( imu_call_2 >= 5 ) //200HZ
    {
        imu_call_2=0;
    }

    if( imu_call_3 >= 10 ) //100HZ
    {
        imu_call_3 = 0;
        if(bmp085_en == 1) bmp085.dataUpdate();
        if(ms611_en == 1) ms611.dataUpdate();
    }

    if( imu_call_4 >= 20 ) //50HZ
    {
        imu_call_4=0;
        if(hmc085_en == 1) hmc5883l.dataUpdate();
    }

    if( imu_call_5 >= 50 ) //20HZ
    {
        imu_call_5=0;
        if( debug_en == 1)
       {
            printf("mpuaccx = %.4f  mpuaccy = %.4f mpuaccz = %.4f\r\n" , mpu6050.acc_normal.x , mpu6050.acc_normal.y,mpu6050.acc_normal.z);
            printf("hmc_normalx = %.4f  hmc_normaly = %.4f hmc_normalz = %.4f\r\n" , hmc5883l.hmc_normal.x , hmc5883l.hmc_normal.y , hmc5883l.hmc_normal.z);
            printf("temperature = %.4f pressure = %.4f altitude = %.4f altitude_offset = %.4f\r\n" , ms611.temperature , ms611.pressure , ms611.altitude , ms611.altitude_offset);
        }
    }

}

接下來就是頻率為100hz的系統(tǒng)數(shù)據(jù)更新函數(shù)，還有50hz的底層電機(jī)控制函數(shù)。

//motor_top.cpp
void MotorTop::motorTopCall(void)
{        
    
    motorPWMRenew( 1 , motor1.speedControl(expect_angle_speed_m[0] , board.getMotorEncoderCNT(1) ) );
    motorPWMRenew( 2 , motor2.speedControl(expect_angle_speed_m[1] , board.getMotorEncoderCNT(2) ) );
    if(motor_enable_num >= 3)
    {
        motorPWMRenew( 3 , motor3.speedControl(expect_angle_speed_m[2] , board.getMotorEncoderCNT(3) ) );
    }
    if(motor_enable_num >= 4)
    {
        motorPWMRenew( 4 , motor4.speedControl(expect_angle_speed_m[3] , board.getMotorEncoderCNT(4) ) );
    }

}

此函數(shù)獲得此刻編碼器的速度數(shù)據(jù)之后先進(jìn)行速度控制，實(shí)現(xiàn)如下。

//robot_wheel_top.cpp
float MotorControl::speedControl(float expect_speed , float unit_count)
{
    //0.3
    float filter =  0.3 * (50 * pid_t);

    expect_angle_speed = (1-filter) * measure_angle_speed + filter * expect_speed;

    if(motor_simulation_model == 1)
    {
        measure_unit_encoder = (simulation_max_angel_speed/360.0f)
                *( pid_parameters_.i_pidout/pwm_max ) * encoder_num * pid_t;
    }
    else
    {
        measure_unit_encoder =  unit_count;  //if you using real motor
    }

    //expect unit encoder num in one cycle to pid
    expect_unit_encoder = ( expect_angle_speed / 360 ) * encoder_num * pid_t;
    expect_total_encoder += expect_unit_encoder ;   //recording total encoder
    measure_total_encoder += measure_unit_encoder ;
    //recording total angle for robot coordinate calculation
    d_past_angle += (measure_unit_encoder/encoder_num)*360;
    past_total_angle+=(measure_unit_encoder/encoder_num)*360;

    //calc motor speed  degree/s
    measure_angle_speed  = measure_unit_encoder * 360 / ( encoder_num*pid_t);

    //motor speed pid control function
    pidOrdinaryCall(expect_total_encoder , measure_total_encoder
                    , expect_unit_encoder , measure_unit_encoder , pwm_max);

    return  pid_parameters_.i_pidout;
}

一開始，先對(duì)車輪速度進(jìn)行滑動(dòng)平局濾波，之后計(jì)算期望速度，實(shí)際速度，期望位置，實(shí)際位置。最后將數(shù)據(jù)送入PID函數(shù)，進(jìn)行位置速度雙閉環(huán)控制。

之后是20hz的機(jī)器人底盤控制robotWheelTopCall()函數(shù)。

//robot_wheel_top.cpp
void RobotWheel::robotWheelTopCall(void)

{

    robotDataUpdate();
    chassisControl();    // control your robotic chassis
    robotCoordCalc();    // for robot localization
    headControl();       // control your robotic head
    armControl();        // control your robotic arm
}

首先機(jī)器人會(huì)將所有數(shù)據(jù)更新，包括全局速度（global_speed）、機(jī)器人速度（robot_speed）、之前測(cè)量的電機(jī)轉(zhuǎn)速，位置和期望電機(jī)轉(zhuǎn)速，位置、數(shù)字舵機(jī)的位置（如果有），四個(gè)機(jī)器人系統(tǒng)數(shù)據(jù)和IMU數(shù)據(jù)，最后還有pid數(shù)據(jù)（如果底層收到上位機(jī)的設(shè)定參數(shù)的請(qǐng)求，便會(huì)更新參數(shù)）。

然后調(diào)用ChassisControl()控制機(jī)器人底盤，它先分別判斷是否收到新的命令，如果收到便執(zhí)行相應(yīng)操作。

最后調(diào)用robotCoordCalc()通過電機(jī)測(cè)量的三個(gè)電機(jī)的速度與位置計(jì)算機(jī)器人坐標(biāo)系的速度位置。

我已經(jīng)將代碼同步到git上，里面有我改好的三輪底盤機(jī)器人地層程序。

git：https://github.com/63445538/Embedded/tree/my_pro

在/0_Project/STM32F4/Application_HF_Wheel_Robot_Beta/目錄下有已經(jīng)建好的keil工程，應(yīng)該沒問題。如果有問題請(qǐng)留言。

參考：

[1]R.西格沃特等.自主移動(dòng)機(jī)器人導(dǎo)論[M].第二版.李人厚等譯.西安：西安交通大學(xué)出版社,2013.5

[2]https://github.com/HANDS-FREE

[3]http://www.runoob.com/cplusplus/cpp-classes-objects.html

[4]http://yfrobot.com/forum.php?mod=viewthread&tid=2411

總結(jié)

以上是生活随笔為你收集整理的[如何构建自己的轮式移动机器人系统·从入门到放弃]机器人底层篇的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。