大家好，在下又回来了。今天开一个新坑，算是小小地总结一下之前的工作。

在这个系列教程中，我会尝试教大家一步一步从底层开始，构建属于自己的移动机器人。为了开发的简单方便，上层使用了装有ROS（robot operating system）的linux板卡和台式电脑（台式机），而下层使用了STM32F407作为MCU的嵌入式系统，并且使用了开源项目HANDS-FREE[2]的大部分代码并在此基础上做了一些移植工作。

直入主题。轮式机器人可以简单定义为以轮子作为运动机构的移动机器人。可以将轮式机器人分为两轮（平衡车），三轮和四轮，当然还有六轮的形式。其实都大同小异。

下表引自[1]。

除了两轮车外，其他轮式的移动机器人都是天然鲁棒系统，所以这对高性能控制的要求几乎为零。

明显地，这是一个项目教程，因此我将以三轮全向轮式移动机器人为例，讲一讲如何从零完成一个机器人项目。

第一部分：机械

淘宝搜索“全向移动机器人”，本例中用的是这款

注意，电机为普通的直流电机，因为是室内移动机器人，机器人的速度小于1m/s，速度不需要很高。要求每个电机上都要有里程计（编码器），目的是获得小车上每个电机的里程与比较粗糙的速度信息。

第二部分：嵌入式系统-硬件

为了解除广告之嫌，我不会放链接。如果有兴趣请留言联系我。

首先，为了要控制电机，需要两到三个电机驱动。我选择的是具有L298N逻辑的双路电机驱动，满足三个电机的控制要求。淘宝上随便找种L298N模块即可。

电源系统：需要给单片机最小系统板提供5v供电，仅需要一个能提供5v的LM25XX开关电源模块即可。非常轻松愉快。

单片机最小系统板：一个STM32F407VET6最小系统板即可。需要板上自带能提供3.3v的LDO，还有三到四个开关和两盏LED，可以不需要RTC电池。

在Embedded/doc/下会此版的原理图。

传感器：最少需要一个MPU6050模块，一个HMC5883L模块。这是为了融合出机器人的yaw，这是三个机器人的状态变量之一。单独一个MPU6050测出的yaw角度会随着时间漂移，需要HMC5883磁力计矫正。最好可以有一个到两个普通的超声波测距模块（如HC-SR04）,主要作为壁障用。下图是集成了MPU6050，HMC5883，BMP180的GY-87模块

电池：12V动力锂电池即可。

其他：①舵机：普通数字舵机或者高级的带串口的数字舵机均可，可以组成简单机械臂或者机器人头；

第三部分：嵌入式系统-软件

本例中的代码结构和大部分的源代码来自开源工程HANDS-FREE。它是一个由西北工大的团队维护的开源项目。在本文中，我会比较详尽的对代码进行讲解。

首先整体介绍以下嵌入式软件部分。这里没有使用实时操作系统。开发环境为keil-ARM V5.17+ windows 10 enterprise。使用C++编程。

[这里为嵌入式软件整体功能以及算法流程图]

工程文件结构如下

USER文件夹内主要是用户文件。Start_Code文件夹内主要是底层库函数代码。而BSP文件夹内放有用户库函数。剩下的就是外设功能包。

首先介绍一下主函数。

//main.cppint 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;
}

可以大致看到整个机器人底层的执行流程：先进行系统架构的初始化和系统外设初始化。然后进入主循环：每一毫秒都会检查接收串口1数据的队列是否有数，如果有数据就对数据帧作分析，否则就更新imu的数据并处理（或者只将数据更新之后交由上位机处理）；每10ms更新一次系统数据（CPU温度，系统时间和电池电压）；每20ms控制一次电机（对电机底层的控制）；每50ms控制一次机器人运动（对机器人姿态的控制）；每500ms翻转一下led电平（指示系统正在运行）。

下面简单讲解一下系统架构初始化和系统外设初始化函数。

下面是系统外设初始化。

//main.cppvoid 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;
}

这里将描述系统外设的所有抽象类进行初始化，我将以board类为例大致讲一讲C++在嵌入式编程中的具体运用，因为确实好用。

//board.hclassBoard
{public:floatcpu_temperature;floatcpu_usage;floatbattery_voltage;float system_time;   //system working time (unit:us), start after power-onuint8_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: KBpublic:Board();void boardBasicInit(void);void boardBasicCall(void);/**********************************************************************************************************************/void debugPutChar(uint8_t tx_byte_);  //system support functionsvoidsetLedState(uint8_t led_id, uint8_t operation);voidsetBeepState(uint8_t operation);uint8_t getKeyState(uint8_t key_id){returnkey_state[key_id] ;}/**********************************************************************************************************************/void motorInterfaceInit(uint8_t motor_id , float motor_pwm_T); //package "PAKG_MOTOR" support functionsvoidmotorEnable(uint8_t motor_id);voidmotorDisable(uint8_t motor_id);voidmotorEnableForward(uint8_t motor_id);voidmotorEnableBackward(uint8_t motor_id);floatgetMotorEncoderCNT(uint8_t motor_id);floatgetMotorCurrent(uint8_t motor_id);void motorSetPWM(uint8_t motor_id , intpwm_value);/**********************************************************************************************************************/void axServoInterfaceInit(void); //package " PAKG_SERVO" support  functionsvoid axServoTxModel(void);void axServoRxModel(void);voidaxServoSendTxByte(uint8_t tx_byte);uint8_t axServoGetRxByte(uint8_t*error);/**********************************************************************************************************************/void sbusInterfaceInit(void); //package " SBUS_PPM" support  functionsvoid ppmInterfaceInit(void);/**********************************************************************************************************************///package " ANALOG_SERVO" support  functionsvoid analogServoInterfaceInit(void);void analogServoSetPWM(uint8_t servo_id , intpwm_value);/**********************************************************************************************************************/void imuI2CInit(void);       //package "PAKG_IMU" support  functionsvoidimuI2CWriteByte(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);voidgpsSendTxByte(uint8_t tx_byte);/**********************************************************************************************************************/void eepromI2CInit(void); //package "AT24Cxx" support functionsvoideepromI2CWriteByte(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 functionsvoid extendI2CInit(void);   //extend iic interface is using for extend other sensorsvoidextendI2CWriteByte(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 modulationvoid pwmInterfaceInit(uint8_t mode , floatpwm_t);void setPWMValue(uint8_t channel_x , float pwm_value);  //channel_x 1~6/**********************************************************************************************************************/\private:floatbattery_voltage_;floatbattery_voltage_alarm_ ;floatbattery_proportion_ ;floattemperature_;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++是一种面向对象的编程语言，C++ 在 C 语言的基础上增加了面向对象编程，C++ 支持面向对象程序设计。类是 C++ 的核心特性，通常被称为用户定义的类型。[3]

类本身是抽象的。定义一个类本质上是定义了一个数据类型的蓝图。程序员将一种事物定义为一种类，然后把这个类的属性与能进行的操作包含在类下。如此例，定义board为描述主控板的类。public标识符下的数据成员为共有的，也就是说其他的类可以调用这些数据成员。而private下则是私有的，也就是说这些数据成员只能在这个类里面使用。主要的原因是C++数据封装与信息隐藏的功能。在大型项目的开发中，代码应该简明并且合理，应该在类里完成的工作就让他在类里面完成就行了。

在这个类下，定义了板子的初始化函数（这里初始化了单片机的时钟和开发板的外设，如定时器、key、led、蜂鸣器、ADC模块等）

//control_unit_v2.cppvoid Board::boardBasicInit(void)
{inti;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 1sHF_ADC_Moder_Init(0X3E00 , 5 , 2.5f);  //ADC initHF_Timer_Init(TIM6 , 1000);     //timer6 init , 1000us
setBeepState(1);delay_ms(500);setBeepState(0);analogServoInterfaceInit();
}

后面还提供了可供用户调用的其他外设包的函数，比如设定led状态的

    void setLedState(uint8_t led_id, uint8_t operation);

和设定使能和失能电机的

    voidmotorEnable(uint8_t motor_id);void motorDisable(uint8_t motor_id);

其他类的具体实现可以参考其源码。

回到主函数，我们这时其实还没有进行任何初始化，只是对类进行了个“抽象”的初始化（具体的初始化还没有实现），接下来就开始进行真正的硬件外设初始化。

//main.cppvoid 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 interruptionprintf("app start \r\n");
}

这里的所有函数除了printf，都是各种外设的初始化。上面已经提到了boardBasicInit()，那就以Motor_top类下的motorTopInit()为例吧。

//motor_top.cppvoid MotorTop::motorTopInit(float motor_enable_num_ , floatmotor_encoder_num_ ,float motor_pid_t_  , unsigned charmotor_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);}
}

这个函数有两大功能，一是初始化STM32的TIM1，二是初始化里程计（编码器）用到的TIM2、TIM3、TIM4定时器。具体的实现函数为motorInterfaceInit()

//control_unit_v2.cppvoid Board::motorInterfaceInit(uint8_t motor_id , floatmotor_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.8KHF_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);
}

因为使用了L298N逻辑的电机驱动，所以一路电机需要两个GPIO和一路PWM控制，因此上面这个函数主要是初始化使用的gpio和pwm功能的定时器TIM1，因为TIM1能发出四路不同的PWM信号，因此，三个电机仅仅需要一个TIM1。编码器也用到了TIM，具体的代码解析就不做了，[4]为介绍STM32编码器接口库函数的解析文档，讲得不错。

回到systemInit函数。剩下分别初始化了motorTop的一些pid参数。这个函数属于RobotWheel类，这个类主要包含了一些控制机器人姿态和运动的接口函数。它主要将上层发出的期望机器人速度或位置信号转换成三个电机的控制信号。imu.topInit()主要完成了IMU的初始化。Ultra_Init完成了超声波模块的初始化。

接下来开始程序主循环的介绍。

首先每次都检查队列（queue）中的数据，调用queue.emptyCheck()。因为每次串口收到数据都会进入串口中断接收数据并保存在队列中，具体的代码实现如下：

//stm32f4xx_it.cppvoid USART1_IRQHandler(void)
{unsignedchardata;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
}

如果检测到队列中有数，就要开始对数据帧做解析了，实现函数如下。这个函数先将协议去除，获得上位机发出的命令所代表的字，然后在通过调用packageAnalysis()对命令一一对应地做出相应。

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

下面就是去除协议帧的函数。它是通过一个状态机来解析协议的。

//hf_link.cppunsigned char HFLink::receiveFiniteStates(const unsigned charrx_data)
{switch(receive_state_)    //协议解析状态机{caseWAITING_FF1:if (rx_data == 0xff)   //接收到帧头第一个数据{receive_state_ =WAITING_FF2;receive_check_sum_=0;receive_message_length_= 0;byte_count_=0;receive_check_sum_+=rx_data;    }break;　　　　　　　　　　//状态机复位caseWAITING_FF2:if (rx_data == 0xff)　　　　//接收到帧头第二个数据，下面以此类推{receive_state_=SENDER_ID;receive_check_sum_+=rx_data;}elsereceive_state_=WAITING_FF1;break;caseSENDER_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;caseRECEIVER_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;caseRECEIVE_LEN_H:receive_check_sum_+=rx_data;receive_message_length_|= rx_data<<8;receive_state_=RECEIVE_LEN_L;break;caseRECEIVE_LEN_L:receive_check_sum_+=rx_data;receive_message_length_|=rx_data;rx_message_.length=receive_message_length_;receive_state_=RECEIVE_PACKAGE;break;caseRECEIVE_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;caseRECEIVE_CHECK:if(rx_data == (unsigned char)receive_check_sum_)  //对比发送和接收的校验和，如果不一致直接放弃此数据帧{receive_check_sum_=0;receive_state_=WAITING_FF1;return 1;　　　　　　　　　　　　　　　　　　　　　//传回1，表示一个数据包收到}else{receive_state_=WAITING_FF1;}break;default:receive_state_=WAITING_FF1;}return 0;
}

这里的协议为：0XFF 0XFF sender_id receiver_id length_H length_L  ****(data)  check_sum。

解析完协议就要开始分析命令了。由packageAnalysis()来实现这一步工作。

//hf_link.cpp

unsigned char HFLink::packageAnalysis(void)
{unsignedchar 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  , (unsignedchar *)single_command, 0);return 1;}}switch(command_state_){caseSHAKING_HANDS :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_global_coordinate , sizeof(my_robot->measure_global_coordinate));break;caseREAD_ROBOT_SYSTEM_INFO :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->robot_system_info , sizeof(my_robot->robot_system_info));break;caseSET_GLOBAL_SPEED :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_global_speed , sizeof(my_robot->expect_global_speed));break;caseREAD_GLOBAL_SPEED :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_global_speed , sizeof(my_robot->measure_global_speed));break;caseSET_ROBOT_SPEED :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_robot_speed , sizeof(my_robot->expect_robot_speed));break;caseREAD_ROBOT_SPEED :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_robot_speed , sizeof(my_robot->measure_robot_speed));break;caseSET_MOTOR_SPEED :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_motor_speed, sizeof(my_robot->expect_motor_speed));break;caseREAD_MOTOR_SPEED :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_motor_speed , sizeof(my_robot->measure_motor_speed));break;caseREAD_MOTOR_MILEAGE :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_motor_mileage , sizeof(my_robot->measure_motor_mileage));break;caseREAD_GLOBAL_COORDINATE :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_global_coordinate , sizeof(my_robot->measure_global_coordinate));break;caseREAD_ROBOT_COORDINATE :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_robot_coordinate , sizeof(my_robot->measure_robot_coordinate));break;caseCLEAR_COORDINATE_DATA :if (hf_link_node_model==0){sendStruct(command_state_ , (unsignedchar *)single_command , 0);analysis_state=1;receive_package_renew[(unsignedchar)command_state_] = 1;}break;caseSET_ARM_1 :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_arm1_state, sizeof(my_robot->expect_arm1_state));break;caseREAD_ARM_1 :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_arm1_state , sizeof(my_robot->measure_arm1_state));break;caseSET_ARM_2 :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_arm2_state, sizeof(my_robot->expect_arm2_state));break;caseREAD_ARM_2 :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_arm2_state , sizeof(my_robot->measure_arm2_state));break;caseSET_HEAD_1 :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_head1_state , sizeof(my_robot->expect_head1_state ));break;caseREAD_HEAD_1 :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_head1_state , sizeof(my_robot->measure_head1_state));break;caseSET_HEAD_2 :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->expect_head2_state, sizeof(my_robot->expect_head2_state));break;caseREAD_HEAD_2 :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_head2_state , sizeof(my_robot->measure_head2_state));break;caseREAD_IMU_DATA :analysis_state=readCommandAnalysis(command_state_ , (unsigned char *)&my_robot->measure_imu_euler_angle , sizeof(my_robot->measure_imu_euler_angle));break;caseSET_ROBOY_PARAMETERS :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->robot_parameters, sizeof(my_robot->robot_parameters));break;caseSAVE_ROBOY_PARAMETERS :if (hf_link_node_model==0){sendStruct(command_state_ , (unsignedchar *)single_command , 0);analysis_state=1;receive_package_renew[(unsignedchar)command_state_] = 1;}break;caseSET_MOTOR_PARAMETERS :analysis_state=setCommandAnalysis(command_state_ , (unsigned char *)&my_robot->motor_parameters, sizeof(my_robot->motor_parameters));break;caseSAVE_MOTOR_PARAMETERS :if (hf_link_node_model==0){sendStruct(command_state_ , (unsignedchar *)single_command , 0);analysis_state=1;receive_package_renew[(unsignedchar)command_state_] = 1;}break;default:analysis_state=0;break;}rx_message_.sender_id=0;    //clear flagrx_message_.receiver_id=0;rx_message_.length=0;rx_message_.data[0]=0;returnanalysis_state;
}

上位机在一开始时通常会发一帧握手信号，然后开始发送数据。然后在switch结构里，对应每一个命令都有相应的代码做处理。大致可以将处理分为设定数据（setCommandAnalysis）和上传数据（readCommandAnalysis）两个函数。

结束后返回分析状态，标志命令更新。在上述两个函数里，还有对应的发送数据的函数，详细请参考源码。

将收到的数据获得后，主循环进入了实际的控制模式。

首先最需要处理是IMU数据。下面的代码将陀螺仪数据的传输频率设定为500hz，bmp085为100hz。只是将数据传输给上位机，并没有做融合处理。

//imu_top.cppvoid 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的系统数据更新函数，还有50hz的底层电机控制函数。

//motor_top.cppvoid 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) ) );}}

此函数获得此刻编码器的速度数据之后先进行速度控制，实现如下。

//robot_wheel_top.cppfloat MotorControl::speedControl(float expect_speed , floatunit_count)
{//0.3float 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 pidexpect_unit_encoder = ( expect_angle_speed / 360 ) * encoder_num *pid_t;expect_total_encoder+= expect_unit_encoder ;   //recording total encodermeasure_total_encoder +=measure_unit_encoder ;//recording total angle for robot coordinate calculationd_past_angle += (measure_unit_encoder/encoder_num)*360;past_total_angle+=(measure_unit_encoder/encoder_num)*360;//calc motor speed  degree/smeasure_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);returnpid_parameters_.i_pidout;
}

一开始，先对车轮速度进行滑动平局滤波，之后计算期望速度，实际速度，期望位置，实际位置。最后将数据送入PID函数，进行位置速度双闭环控制。

之后是20hz的机器人底盘控制robotWheelTopCall()函数。

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

{robotDataUpdate();chassisControl();//control your robotic chassisrobotCoordCalc();    //for robot localizationheadControl();       //control your robotic headarmControl();        //control your robotic arm
}

首先机器人会将所有数据更新，包括全局速度（global_speed）、机器人速度（robot_speed）、之前测量的电机转速，位置和期望电机转速，位置、数字舵机的位置（如果有），四个机器人系统数据和IMU数据，最后还有pid数据（如果底层收到上位机的设定参数的请求，便会更新参数）。

然后调用ChassisControl()控制机器人底盘，它先分别判断是否收到新的命令，如果收到便执行相应操作。

最后调用robotCoordCalc()通过电机测量的三个电机的速度与位置计算机器人坐标系的速度位置。

我已经将代码同步到git上，里面有我改好的三轮底盘机器人地层程序。

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

在/0_Project/STM32F4/Application_HF_Wheel_Robot_Beta/目录下有已经建好的keil工程，应该没问题。如果有问题请留言。

参考：

[1]R.西格沃特等.自主移动机器人导论[M].第二版.李人厚等译.西安：西安交通大学出版社,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