大家好，在下又回來了。今天開一個新坑，算是小小地總結一下之前的工作。
在這個系列教程中，我會嘗試教大家一步一步從底層開始，構建屬於自己的移動機器人。為了開發的簡單方便，上層使用了裝有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.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) ) ); } }
此函數獲得此刻編碼器的速度數據之后先進行速度控制，實現如下。
//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; }
一開始，先對車輪速度進行滑動平局濾波，之后計算期望速度，實際速度，期望位置，實際位置。最后將數據送入PID函數，進行位置速度雙閉環控制。
之后是20hz的機器人底盤控制robotWheelTopCall()函數。
//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 }
首先機器人會將所有數據更新，包括全局速度（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