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

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

下表引自[1]。

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

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

第一部分：机械

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

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

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

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

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

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

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

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

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

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

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

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

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

工程文件结构如下

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

首先介绍一下主函数。

//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; }

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

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

下面是系统外设初始化。

//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; }

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

//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++是一种面向对象的编程语言，C++ 在 C 语言的基础上增加了面向对象编程，C++ 支持面向对象程序设计。类是 C++ 的核心特性，通常被称为用户定义的类型。[3]

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

在这个类下，定义了板子的初始化函数（这里初始化了单片机的时钟和开发板的外设，如定时器、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(); }

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

void setLedState(uint8_t led_id, uint8_t operation);

和设定使能和失能电机的

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

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

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

//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"); }

这里的所有函数除了printf，都是各种外设的初始化。上面已经提到了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); } }

这个函数有两大功能，一是初始化STM32的TIM1，二是初始化里程计（编码器）用到的TIM2、TIM3、TIM4定时器。具体的实现函数为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); }

因为使用了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.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 }

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

//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; }

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

//hf_link.cpp unsigned char HFLink::receiveFiniteStates(const unsigned char rx_data) { switch (receive_state_) //协议解析状态机 { case WAITING_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;　　　　　　　　　　//状态机复位 case WAITING_FF2: if (rx_data == 0xff)　　　　//接收到帧头第二个数据，下面以此类推 { 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_) //对比发送和接收的校验和，如果不一致直接放弃此数据帧 { 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) { 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; }

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

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

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

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

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

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

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

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; }

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

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

void RobotWheel::robotWheelTopCall(void) { robotDataUpdate(); chassisControl(); // control your robotic chassis robotCoordCalc(); // for robot localization headControl(); // control your robotic head armControl(); // control your robotic arm }