【請認準：OpenSir開源達人】

開源STM32自平衡小車

平衡小車開源資料網盤鏈接: 平衡小車百度網盤資料鏈接，點擊進入
【嗶站視頻一鍵三連后，評論區留言郵箱獲取提取碼(3天內發至郵箱)】

嗶站播放視頻鏈接如下：
鏈接: 【開源STM32自平衡小車】嗶站播放視頻效果，點擊進行播放

ps：如有不愿意進行自主diy的小伙伴們，我們也為其提供有推薦的套件
鏈接: STM32自平衡小車開源整車套件，點擊即可進行查看

---

文章目錄

• 【請認準：OpenSir開源達人】
• 一、硬件篇（附淘寶器件鏈接，也可自行選購）
• • 1.STM32F103C8T6最小系統板（核心板）
  • 2.MPU6050姿態傳感器（六軸）
  • 3.TB6612電機驅動（雙通道）
  • 4.0.96寸OLED顯示屏
  • 5.藍牙無線通信模塊
  • 6.電源降壓模塊（雙電壓輸出）
  • 7.超聲波測距模塊
  • 8.動力鋰電池（雙節18650）
  • 9.小車橡膠車輪（65mm）
  • 10.STLINK代碼仿真下載器
• 二、軟件篇
• • 1.主函數代碼
  • 2.systick滴答定時器中斷任務
  • 3.滴答定時器5ms中斷配置
  • 4.調試串口發送數據至上位機
  • 5.ADC采用DMA方式進行數據采集
  • 6.陀螺儀數據校準
  • 7.超聲波測距控制
  • 8.歐拉角姿態解析
  • 9.PID控制器
  • 10.藍牙數據包接收并解析
• 三、原理圖

---

提示：以下是本篇文章正文內容

一、硬件篇（附淘寶器件鏈接，也可自行選購）

1.STM32F103C8T6最小系統板（核心板）

推薦鏈接: STM32F103C8T6最小系統核心板，點擊進入!

2.MPU6050姿態傳感器（六軸）

推薦鏈接: MPU6050姿態傳感器模塊，點擊進入!
傳感器資料鏈接:

https://pan.baidu.com/s/1dNDqcp76L9QdM7iSZYfz_A 提取碼: 4eum

https://pan.baidu.com/s/13ZoMZ-lpAtDRhey4lKEcFQ 提取碼: p7fr

3.TB6612電機驅動（雙通道）

推薦鏈接: TB6612電機驅動（雙通道），點擊進入!

4.0.96寸OLED顯示屏

推薦鏈接: 0.96寸OLED顯示屏模塊（128x64），點擊進入!
模塊資料鏈接：

https://pan.baidu.com/s/108smkVOLg-23cAnr37ZeGQ 密碼：91t7

https://pan.baidu.com/s/1Lr8lw_6vt_VdM1UWe9CGsA 密碼：p0un

5.藍牙無線通信模塊

推薦鏈接: JDY-31藍牙無線通信模塊，點擊進入!
DY-31帶底板資料：

https://pan.baidu.com/s/16qyiOO05whOXqYtBVBGs2Q

6.電源降壓模塊（雙電壓輸出）

推薦鏈接: 5V和3.3V雙通道輸出降壓模塊，點擊進入!（咨詢客服）

7.超聲波測距模塊

推薦鏈接: HC-SR04超聲波測距模塊，點擊進入!
HC-SR04資料下載：

http://pan.baidu.com/s/1sjHKDI1

HC-SR04單芯片版本資料下載接：

https://pan.baidu.com/s/1sSah9PvLBrmbA7So-6YcSw 提取碼: qq35

8.動力鋰電池（雙節18650）

推薦鏈接: 3.7V 18650鋰電池，點擊進入!

9.小車橡膠車輪（65mm）

10.STLINK代碼仿真下載器

推薦鏈接: STLINK V2代碼燒錄器，點擊進入!

STLINK資料網盤鏈接:

https://pan.baidu.com/s/1gxzJeDe7CJaCCl4pGnwdNQ 提取碼: an2m

WIN10系統驅動解決方案

https://pan.baidu.com/s/1OQosxeUMnCe5CZkwKRKhOA 提取碼: h9ii

二、軟件篇

點擊文章頂部 B站視頻鏈接 或者 點擊此處 一鍵三連+關注，留言郵箱即可獲取本小車Keil源代碼文件

1.主函數代碼

代碼如下（main.c）：

/********************************************************************** 版權所有：源動力科技 淘 寶：https://1024tech.taobao.com/ 文 件 名：mian.c 版 本：V21.01.30 摘 要： ***********************************************************************/ #include "stm32f10x.h" #include "systick.h" #include "led.h" #include "iic.h" #include "mpu6050.h" #include "timer.h" #include "nvic.h" #include "imu.h" #include "pwm.h" #include "flash.h" #include "pid.h" #include "usart1.h" #include "bluetooth.h" #include "key.h" #include "ps2.h" #include "oled.h" #include "adc.h" #include "ultrasonic.h" #include "imath.h" #include "Infrared.h"/* * 使能SWD， 失能JTAG * PB3,PB4,PA15作為普通IO使用 * MCU復位后，PA13/14/15 & PB3/4默認配置為JTAG功能 */ static void _SWJ_Config(void) {RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); }int main(void) { NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //優先級組別2_SWJ_Config(); //使能SWD，失能JTAG usart1_init(115200); //串口1配置初始化 printf("usart1 is ok\r\n");BluetoothUsart_init(9600); //藍牙串口初始化pwm_init(); //pwm初始driver_pin_init(); //電機驅動配置初始化Encoder_A_init(); //編碼器A初始化 Encoder_B_init(); //編碼器B初始化 tdelay_ms(100); IIC_Init(); //IIC端口配置mpu6050_init(); //mpu6050初始化 while(1) //mpu6050在位檢測 {uint8_t mpuId;mpuId = get_mpu_id(); //讀取mpu6050的idif(mpuId==0x68||mpuId==0x98) //判斷mpu6050的ID是否正確break; tdelay_ms(50); }get_iir_factor(&Mpu.att_acc_factor,0.005f,25); //姿態解算時加速度低通系數 OLED_Init(); //OLED顯示屏端口初始化adc_init(); //ADC配置初始化ReadCalData(); //讀取校準后的陀螺儀零偏數據tdelay_ms(100); timer_init(); //定0時器初始化 HCSR04_Init(); //超聲波端口初始化systick_init(); //滴答定時器初始化 NVIC_config(); //中斷配置初始化while(1){if(softTimes[0] == 0) //100ms{ softTimes[0] = 20; }if(softTimes[1] == 0) //20ms{softTimes[1] = 4; UltrasonicWave_StartMeasure(); //超聲波發出起始高電平 ParseBluetoothMessage(USARTxBlueTooth_RX_DATA, &BluetoothParseMsg); //藍牙數據包解析 }if(softTimes[2] == 0) //100ms{softTimes[2] = 20; voltage_detection(); //低電壓檢測OledShowDatas(); } } }

2.systick滴答定時器中斷任務

代碼如下（SysTick_Handler() ）：

/*** @brief This function handles SysTick Handler.* @param None* @retval None*/ void SysTick_Handler(void) {softTimesCountdown(); //軟件定時倒計時UltrasonicCheck(); //超聲波在位檢測get_gyro_raw(); //陀螺儀數據get_deg_s(&gyro_raw_f,&Mpu.deg_s); //陀螺儀原始數據轉為度為單位的速率 get_rad_s(&gyro_raw_f,&Mpu.rad_s); //陀螺儀原始數據轉為弧度為單位的速率get_acc_raw(); //加速度數據acc_iir_lpf(&acc_raw_f,&acc_att_lpf,Mpu.att_acc_factor); //姿態解算時加速度低通濾波 get_acc_g(&acc_att_lpf,&Mpu.acc_g); //姿態解算mahony_update(Mpu.rad_s.x,Mpu.rad_s.y,Mpu.rad_s.z,Mpu.acc_g.x,Mpu.acc_g.y,Mpu.acc_g.z); Matrix_ready(); //姿態解算相關矩陣更新encoder_val_a = read_encoder(ENCODER_A); //A編碼器值讀取encoder_manage(&encoder_val_a); //編碼器值處理encoder_val_b = read_encoder(ENCODER_B); //B編碼器值讀取 encoder_manage(&encoder_val_b); //編碼器值處理ctr.pwm[0] = ctr_bal(att.rol,Mpu.deg_s.y); //角度直立平衡控制器ctr.pwm[1] = ctr_vel(encoder_val_a,-encoder_val_b); //速度控制器ctr.pwm[2] = ctr_turn(encoder_val_a,-encoder_val_b,Mpu.deg_s.z); //轉向控制器ctr.out[0] = ctr.pwm[0] + ctr.pwm[1] + ctr.pwm[2]; //電機1匹配輸出 ctr.out[1] = ctr.pwm[0] + ctr.pwm[1] - ctr.pwm[2]; //電機2匹配輸出i_limit(&(ctr.out[0]),AMPLITUDE); //輸出限幅i_limit(&(ctr.out[1]),AMPLITUDE); //輸出限幅dir_config(&(ctr.out[0]),&(ctr.out[1])); //根據正負設置方向_ctr_out(); //控制器輸出ANO_DMA_SEND_DATA(); //地面站波形顯示gyro_cal(&gyro_raw_cal); //陀螺儀零偏校準 }

3.滴答定時器5ms中斷配置

代碼如下（ systick_init ）：

/** 函數名：systick_init* 描述 ：系統滴答定時器配置初始化* 輸入 ： * 返回 ： */ void systick_init(void) {SystemCoreClockUpdate();//時鐘頻率：72Mhz ， 每秒可以計數72000000次，一次的時間則為：1/72000（ms），10ms需要的次數：10/(1/72000) = 720000 -> 5ms需要的次數則為：360000if (SysTick_Config(SystemCoreClock / 200)) //1000 -> 1ms{ /* Capture error */ while (1);}SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; //使能 }

4.調試串口發送數據至上位機

代碼如下（ ANO_DMA_SEND_DATA ）：

vvoid ANO_DMA_SEND_DATA(void) {static uint8_t ANO_debug_cnt = 0;ANO_debug_cnt++;if(ANO_debug_cnt==1){ANO_DT_Send_Status();}else if(ANO_debug_cnt==2){ ANO_DT_Send_Senser((int16_t)acc_raw.x,(int16_t)acc_raw.y,(int16_t)acc_raw.z, (int16_t)gyro_raw.x,(int16_t)gyro_raw.y,(int16_t)gyro_raw.z,(int16_t)0,(int16_t)0,(int16_t)0);}else if(ANO_debug_cnt==3){ANO_DT_Send_RCData(1100,1200,1300,1400,1500,1600,1700,1800,1900,1100);}else if(ANO_debug_cnt==4){ANO_DT_Send_Power(13.52,57.63);}else if(ANO_debug_cnt==5){ANO_DT_Send_User(0,0,0,0,0,encoder_val_a, encoder_val_b,ctr.pwm[0],ctr.pwm[1],acc_raw_f.z,0,0,0,0,0);}else if(ANO_debug_cnt==6){if(f.send_pid1){f.send_pid1 = 0;//向上位機發送內環速度環PID參數值ANO_DT_Send_PID(1, vel.kp*0.1f,vel.ki,vel.kd,1.5f,2.5f,1.5f,2.5f,1.5f,2.5f); }if(f.send_pid2){f.send_pid2 = 0;//向上位機發送外環角度環PID參數值ANO_DT_Send_PID(2, bal.kp*0.1f,bal.ki,bal.kd,2.6f,1.5f,2.4f,1.1f,2.2f,1.1f); }if(CalGyro.success==1) //返回校準成功信息給上位機{CalGyro.success = 0;ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);}else if(CalGyro.success==2) //反饋校準失敗信息給上位機{CalGyro.success = 0;ANO_DT_Send_MSG(SENSOR_GYRO,CAL_SUCCESS);}}else if(ANO_debug_cnt==7){ANO_debug_cnt = 0;} }

5.ADC采用DMA方式進行數據采集

代碼如下：

/********************************************************************** 版權所有：源動力科技 淘 寶：https://1024tech.taobao.com/ 文 件 名：adc.c 版 本：V21.01.30 摘 要： ***********************************************************************/ #include "adc.h"_ADC_VALUE bat = {0 ,0 };uint16_t adc_value[1];/* 端口配置初始化 */ void adc_gpio_init(void) {GPIO_InitTypeDef GPIO_initStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);GPIO_initStructure.GPIO_Pin = GPIO_Pin_2; GPIO_initStructure.GPIO_Mode = GPIO_Mode_AIN; GPIO_Init(GPIOA,&GPIO_initStructure); }/* adc配置 */ void adc_config(void) {ADC_InitTypeDef ADC_initStructure;RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1,ENABLE);ADC_initStructure.ADC_ContinuousConvMode = ENABLE; //連續轉換 ADC_initStructure.ADC_DataAlign = ADC_DataAlign_Right; //數據右對齊 ADC_initStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //軟件觸發 ADC_initStructure.ADC_Mode = ADC_Mode_Independent; //獨立工作模式ADC_initStructure.ADC_NbrOfChannel = 1; //順序進行規則轉換的ADC通道的數目 1-16 ADC_initStructure.ADC_ScanConvMode = DISABLE; //掃描模式：單次模式 ADC_Init(ADC1,&ADC_initStructure);ADC_Cmd(ADC1,ENABLE);ADC_DMACmd(ADC1,ENABLE);RCC_ADCCLKConfig(RCC_PCLK2_Div8); //ADC時鐘分頻 //通道配置，采樣時間設置ADC_RegularChannelConfig(ADC1,ADC_Channel_2,1,ADC_SampleTime_239Cycles5); ADC_ResetCalibration(ADC1); //復位校準 while(ADC_GetCalibrationStatus(ADC1)); //等待ADC_StartCalibration(ADC1); //啟動校準while(ADC_GetCalibrationStatus(ADC1)); //等待校準完成ADC_SoftwareStartConvCmd(ADC1,ENABLE); //開啟轉換 }void dma_config(void) {DMA_InitTypeDef DMA_initStructure;RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1,ENABLE);DMA_initStructure.DMA_BufferSize = 1; //緩存的數據大小 DMA_initStructure.DMA_DIR = DMA_DIR_PeripheralSRC; //傳輸方向：外設->內存 DMA_initStructure.DMA_M2M = DMA_M2M_Disable; DMA_initStructure.DMA_MemoryBaseAddr = (u32)adc_value; //內存基地址DMA_initStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; //DMA傳輸的內存數據大小：半字為單位 DMA_initStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //內存地址自增DMA_initStructure.DMA_Mode = DMA_Mode_Circular; //循環模式DMA_initStructure.DMA_PeripheralBaseAddr = ((u32)&ADC1->DR); //外設地址 DMA_initStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; //DMA傳輸的外設數據大小：半字為單位DMA_initStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //外設地址不變DMA_initStructure.DMA_Priority = DMA_Priority_Medium; //中等優先級DMA_Init(DMA1_Channel1,&DMA_initStructure);DMA_Cmd(DMA1_Channel1,ENABLE); }void adc_init(void) {adc_gpio_init();adc_config();dma_config(); } /** 函數名：voltage_detection* 描述 ：電源電壓采集* 輸入 ： * 返回 ： */ void voltage_detection(void) {bat.adc = adc_value[0];bat.voltage = (float)(bat.adc)/4096.0f*3.3f*11.0f; //實際電路采用電阻10:1比例進行分壓if(bat.voltage<=7.0f){bat.danger_flag = 1;}else{bat.danger_flag = 0;} }

6.陀螺儀數據校準

代碼如下：

/** 函數名：gyro_cal* 描述 ：陀螺儀零偏數據校準* 輸入 ：gyro_in原始的靜止時陀螺儀數據首地址 * 返回 ： */ void gyro_cal(SI_F_XYZ *gyro_in) {if(CalGyro.flag==1 && 1) {if(CalGyro.i < GyroCalSumValue) //原始靜止數據500次累加求取平均 { CalGyro.offset.x += gyro_in->x; CalGyro.offset.y += gyro_in->y;CalGyro.offset.z += gyro_in->z;CalGyro.i++;}else{CalGyro.i = 0;CalGyro.OffsetFlashWrite.x = CalGyro.offset.x / GyroCalSumValue; //得到三軸的零偏 CalGyro.OffsetFlashWrite.y = CalGyro.offset.y / GyroCalSumValue; //得到三軸的零偏CalGyro.OffsetFlashWrite.z = CalGyro.offset.z / GyroCalSumValue; //得到三軸的零偏/* 將陀螺儀零偏寫入flash */FlashWriteNineFloat(SENSOR_CAL_ADDRESS, 0,0,0,0,0,0,CalGyro.OffsetFlashWrite.x,CalGyro.OffsetFlashWrite.y,CalGyro.OffsetFlashWrite.z); /* 校準完數據之后立馬讀取出來進行使用 */flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS, &CalGyro.None.x,&CalGyro.None.y,&CalGyro.None.z, &CalGyro.None.x, &CalGyro.None.y,&CalGyro.None.z,&CalGyro.OffsetFlashRead.x,&CalGyro.OffsetFlashRead.y,&CalGyro.OffsetFlashRead.z);/* 判斷是否正確讀出 */if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0){_set_val(&gyro_offset,&CalGyro.OffsetFlashRead); //陀螺儀零偏設置CalGyro.success = 1; //校準成功_led.sta = 0;}else {CalGyro.success = 2; //校準失敗}CalGyro.offset.x = 0;CalGyro.offset.y = 0;CalGyro.offset.z = 0;CalGyro.flag = 0; //校準標志位清除}} }/** 函數名：ReadCalData* 描述 ：陀螺儀零偏校準數據進行讀取* 輸入 ： * 返回 ：err */ int ReadCalData(void) {ErrorStatus err;flash_flag.sensor = FlashReadNineFloat(SENSOR_CAL_ADDRESS, &CalGyro.None.x,&CalGyro.None.y,&CalGyro.None.z, &CalGyro.None.x, &CalGyro.None.y,&CalGyro.None.z,&CalGyro.OffsetFlashRead.x,&CalGyro.OffsetFlashRead.y,&CalGyro.OffsetFlashRead.z);/* 判斷是否正確讀出 */if(flash_flag.sensor!=0x01ff && flash_flag.sensor!=0x01C0){_set_val(&gyro_offset, &CalGyro.OffsetFlashRead); //陀螺儀零偏設置err = SUCCESS ;}else{err = ERROR;}return err; }

7.超聲波測距控制

代碼如下：

float getDistance; uint16_t EchoFlag = 0; uint16_t disCounter; void EXTI15_10_IRQHandler(void) {disCounter = 0;tdelay_us(3); //延時10usif(EXTI_GetITStatus(EXTI_Line13) != RESET){EchoFlag = 1;TIM_SetCounter(TIM1,0);TIM_Cmd(TIM1, ENABLE); while(GPIO_ReadInputDataBit(HCSR04_ECHO_PORT,HCSR04_ECHO_PIN)) //等待低電平{disCounter = TIM_GetCounter(TIM1);if(disCounter>5000) //最遠測量控制在5000計數值，換為距離為85cmbreak;}TIM_Cmd(TIM1, DISABLE);getDistance = TIM_GetCounter(TIM1)/1000000.0f * 340.0f / 2.0f * 100.0f; //us->s 聲速340m/s 最終轉為厘米cm //計算公式為：距離=（高電平時間*340m/s）/2// printf("%.3fcm %d\r\n",getDistance,TIM_GetCounter(TIM1));EXTI_ClearITPendingBit(EXTI_Line13); //清除EXTI7線路掛起位} }

8.歐拉角姿態解析

代碼如下（ mahony_update(float gx, float gy, float gz, float ax, float ay, float az) ）：

/********************************************************************** 版權所有：源動力科技 淘 寶：https://1024tech.taobao.com/ 文 件 名：imu.c 版 本：V21.01.30 摘 要： ***********************************************************************/ #include "imu.h" #include "imath.h" #include "math.h" #include "mpu6050.h" #include "timer.h"_Matrix Mat = {0}; _Attitude att = {0}; #define mahonyPERIOD 5.0f //姿態解算周期（ms） #define kp 0.5f //proportional gain governs rate of convergence to accelerometer/magnetometer #define ki 0.0001f //integral gain governs rate of convergenceof gyroscope biasesfloat q0 = 1, q1 = 0, q2 = 0, q3 = 0; //quaternion elements representing theestimated orientation float exInt = 0, eyInt = 0, ezInt = 0; //scaled integral error /** 函數名：mahony_update* 描述 ：姿態解算* 輸入 ：陀螺儀三軸數據（單位：弧度/秒），加速度三軸數據（單位：g） * 返回 ： */ //Gyroscope units are radians/second, accelerometer units are irrelevant as the vector is normalised. void mahony_update(float gx, float gy, float gz, float ax, float ay, float az) {float norm;float vx, vy, vz;float ex, ey, ez;if(ax*ay*az==0)return;//[ax,ay,az]是機體坐標系下加速度計測得的重力向量(豎直向下)norm = invSqrt(ax*ax + ay*ay + az*az);ax = ax * norm;ay = ay * norm;az = az * norm;//VectorA = MatrixC * VectorB//VectorA ：參考重力向量轉到在機體下的值//MatrixC ：地理坐標系轉機體坐標系的旋轉矩陣 //VectorB ：參考重力向量（0,0,1） //[vx,vy,vz]是地理坐標系重力分向量[0,0,1]經過DCM旋轉矩陣(C(n->b))計算得到的機體坐標系中的重力向量(豎直向下) vx = Mat.DCM_T[0][2];vy = Mat.DCM_T[1][2];vz = Mat.DCM_T[2][2];//機體坐標系下向量叉乘得到誤差向量，誤差e就是測量得到的vˉ和預測得到的 v^之間的相對旋轉。這里的vˉ就是[ax,ay,az]’,v^就是[vx,vy,vz]’//利用這個誤差來修正DCM方向余弦矩陣(修正DCM矩陣中的四元素)，這個矩陣的作用就是將b系和n正確的轉化直到重合。//實際上這種修正方法只把b系和n系的XOY平面重合起來，對于z軸旋轉的偏航，加速度計無可奈何，//但是，由于加速度計無法感知z軸上的旋轉運動，所以還需要用地磁計來進一步補償。//兩個向量的叉積得到的結果是兩個向量的模與他們之間夾角正弦的乘積a×v=|a||v|sinθ,//加速度計測量得到的重力向量和預測得到的機體重力向量已經經過單位化，因而他們的模是1，//也就是說它們向量的叉積結果僅與sinθ有關，當角度很小時，叉積結果可以近似于角度成正比。ex = ay*vz - az*vy;ey = az*vx - ax*vz;ez = ax*vy - ay*vx;//對誤差向量進行積分exInt = exInt + ex*ki;eyInt = eyInt + ey*ki;ezInt = ezInt + ez*ki;//姿態誤差補償到角速度上，修正角速度積分漂移，通過調節Kp、Ki兩個參數，可以控制加速度計修正陀螺儀積分姿態的速度。gx = gx + kp*ex + exInt;gy = gy + kp*ey + eyInt;gz = gz + kp*ez + ezInt;//一階龍格庫塔法更新四元數 q0 = q0 + (-q1*gx - q2*gy - q3*gz)* mahonyPERIOD * 0.0005f;q1 = q1 + ( q0*gx + q2*gz - q3*gy)* mahonyPERIOD * 0.0005f;q2 = q2 + ( q0*gy - q1*gz + q3*gx)* mahonyPERIOD * 0.0005f;q3 = q3 + ( q0*gz + q1*gy - q2*gx)* mahonyPERIOD * 0.0005f; //把上述運算后的四元數進行歸一化處理。得到了物體經過旋轉后的新的四元數。norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 * norm;q1 = q1 * norm;q2 = q2 * norm;q3 = q3 * norm;//四元素轉歐拉角att.pit = atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * rad_to_angle;att.rol = asin(2.0f*(q0*q2 - q1*q3)) * rad_to_angle; //z軸角速度積分的偏航角att.yaw += Mpu.deg_s.z * mahonyPERIOD * 0.001f; } /** 函數名：rotation_matrix* 描述 ：旋轉矩陣：機體坐標系 -> 地理坐標系* 輸入 ： * 返回 ： */ void rotation_matrix(void) {Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2; } /** 函數名：rotation_matrix_T* 描述 ：旋轉矩陣的轉置矩陣：地理坐標系 -> 機體坐標系* 輸入 ： * 返回 ： */ void rotation_matrix_T(void) {Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3); Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2); Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3; Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1); Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2; } /** 函數名：Matrix_ready* 描述 ：矩陣更新準備，為姿態解算使用* 輸入 ： * 返回 ： */ void Matrix_ready(void) {rotation_matrix(); //旋轉矩陣更新rotation_matrix_T(); //旋轉矩陣的逆矩陣更新 }

9.PID控制器

代碼如下：

/* ctr.bais[0]：角度控制器偏差存儲 ctr.bais[1]：速度控制器偏差累計存儲 ctr.bais[2]：轉向控制器偏差存儲 ctr.exp[0]：角度控制器期望角度 ctr.exp[1]：速度控制器期望速度 ctr.exp[2]：轉向控制器期望角度 *//** 函數名：ctr_bal* 描述 ：角度PD控制器* 輸入 ：angle當前角度，gyro當前角速度* 返回 ：PID控制器輸出*/ int ctr_bal(float angle,float gyro) { ctr.exp[0] = 0; //期望角度 ctr.bais[0] = (float)(angle - ctr.exp[0]); //角度偏差ctr.balance = bal.kp * ctr.bais[0] + gyro * bal.kd; //角度平衡控制return ctr.balance; } int CarStepForwardOrBackward; /** 函數名：ctr_vel* 描述 ：速度PI控制器* 輸入 ：encoder_left編碼器值A，encoder_right編碼器值B* 返回 ：PID控制器輸出*/ int ctr_vel(int encoder_left,int encoder_right) { static float encoder_cur,encoder_last;CarStepForwardOrBackward = CarSpeedCtrlForwardOrBackward(1); //遙控器控制的前后方向速度 remote.ForwardOrBackward = remoteCarForwardOrBackward( (float)CarStepForwardOrBackward , MODE_BLUETEETH , UltraSuccess(BluetoothParseMsg.ModeRocker) ); //遙控器控制前后方向運動encoder_last = ((encoder_left) + (encoder_right)) - 0; //速度誤差encoder_cur = encoder_cur * 0.8 + encoder_last * 0.2; //對偏差進行低通濾波ctr.bais[1] += encoder_cur; //偏差累加和 ctr.bais[1] = ctr.bais[1] - remote.ForwardOrBackward; //遙控控制前后方向if(ctr.bais[1] > 10000) ctr.bais[1] = 10000; //限幅if(ctr.bais[1] <-10000) ctr.bais[1] =-10000; ctr.velocity = encoder_cur * vel.kp + ctr.bais[1] * vel.ki; //速度控制 return ctr.velocity; }

10.藍牙數據包接收并解析

代碼如下（ ParseBluetoothMessage(char *pInput ,uint16_t rcLens , BluetoothParse *blueParseMsg ) ）：

** 函數名：USART3_IRQHandler* 描述 ：藍牙串口接收中斷處理函數* 輸入 ：* 返回 ：*/ void USART3_IRQHandler(void) { if(USART_GetITStatus(BLUETOOTH_USARTx, USART_IT_RXNE) != RESET) { if(BlueToothBufIndex > USARTxBlueTooth_RX_LEN)BlueToothBufIndex = 0;USARTxBlueTooth_RX_DATA[BlueToothBufIndex++] = USART_ReceiveData(BLUETOOTH_USARTx);BlueToothNumbers ++;USART_ClearITPendingBit(BLUETOOTH_USARTx, USART_IT_RXNE); //清除空閑中斷標志} } /** 函數名：ParseBluetoothMessage* 描述 ：解析藍牙app發送過來的數據包* 輸入 ：pInput收到的藍牙遙控數據首地址 , blueParseMsg解析后保存的藍牙數據有效信息* 返回 ：err*/ /** 協議形式：字符串* 幀頭：# 幀尾：** 例如：#9,1,1,2,2,1* * 每位代表含義：9：數據長度(固定) 1：模式 1：關閉/開始狀態 2：藍牙搖桿X軸數據 2：藍牙搖桿Y軸數據 1：校驗值(累加和減去7) */ uint8_t ParseBluetoothMessage(char *pInput , BluetoothParse *blueParseMsg) {unsigned char plen,sum,check;ErrorStatus err;char *pdata = pInput;while(( pdata-pInput ) < BlueToothBufIndex ){if(*pdata == '#')// #9,1,1,2,2,1* //接收到數據的包頭#{plen = (unsigned char)atof(strtok(pdata+1, ",")); //解析長度lengthif(plen == 9){/* 將讀出的數據進行累加校驗 */ sum = (unsigned char)int_abs( ((int)atof(strtok(pdata+3, ",")) + (int)atof(strtok(NULL, ",")) +(int)atof(strtok(NULL, ",")) + (int)atof(strtok(NULL, ","))) - 7 );/* 讀出累加校驗數據 */check = (unsigned char)atof(strtok(NULL, ",")) ; //累加校驗數據if(sum == check) //校驗匹配成功才進行數據解析 {blueParseMsg->ModeRocker = (unsigned char)atof(strtok(pdata+3, ",")); //模式數據blueParseMsg->OpenCloseStatus = (unsigned char)atof(strtok(pdata+5, ",")); //關閉/開始狀態數據blueParseMsg->Xrocker = (unsigned char)atof(strtok(pdata+7, ",")); //搖桿X數據blueParseMsg->Yrocker = (unsigned char)atof(strtok(pdata+9, ",")); //搖桿Y數據err = SUCCESS;}else{err = ERROR; }}else {err = ERROR;}}else {err = ERROR;}pdata++;}BlueToothBufIndex = 0;return err ; }

---

三、原理圖

【 PS：點擊文章頂部 B站視頻鏈接 或者 點擊此處 一鍵三連+關注，留言郵箱即可獲取本小車原理圖紙文件。】

總結

以上是生活随笔為你收集整理的【开源STM32自平衡小车】 教你如何自己DIY一辆双轮自平衡小车的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。