風力擺控制系統(tǒng)-設(shè)計報告(共27頁)

1、精選優(yōu)質(zhì)文檔-傾情為你奉上摘要：本系統(tǒng)主要是以STM32單片機為控制芯片控制4只直流軸流風機，從而調(diào)節(jié)風機轉(zhuǎn)速來控制使風力擺呈現(xiàn)不同狀態(tài)的控制系統(tǒng)。該系統(tǒng)主要由主控板，無線遙控器，直流軸流風機，擺架框架等四大部分組成風力擺控制系統(tǒng)。 關(guān)鍵字：風力擺 無線 STM32單片機 直流軸流風機專心-專注-專業(yè)一系統(tǒng)方案1.系統(tǒng)方案論證 本系統(tǒng)主要由遙控模塊、控制模塊、陀螺儀模塊、直流軸流風機組成，添加一些輔助電路作為擴展功能。系統(tǒng)工作有六種工作模式，使用無線遙控切換模式并顯示。下面分別論證這幾個模塊的選擇。1.1直流風機的論證與選擇方案一：使用直流鼓風機。直流鼓風機的機械摩擦非常小，具有較大的精度，并

2、能提供足夠的風力進行運動。但在實驗過程中，風機啟動速度較慢，且由于其自身重量過大，風擺在運動過程中受慣性影響極大，不能有效的完成任務要求。方案二：采用直流軸流風機。直流軸流風機是在固定位置使空氣流動，自身重量和體積都比較小，且出風口大，能夠很好的提供動力與控制。在實驗過程中能夠較快的啟動，并能較好的實現(xiàn)任務要求，符合實驗需要。綜合以上兩種方案，風力擺在運動過程中需要進行實時控制擺桿的姿態(tài)，且需要風機啟停反應快，故選擇方案二。1.2控制器模塊的論證與選擇根據(jù)設(shè)計要求，控制器主要用于計算擺桿姿態(tài)、控制直流軸流風機PWM、使擺桿能完成相應等功能。方案一：采用STC89C51作為系統(tǒng)控制器。它的技術(shù)成

3、熟，成本低。STC89C51是8位的單片機，數(shù)據(jù)傳輸速度慢，在用于精密的操作時，不能滿足實時控制的要求，且復雜的控制算法難以實現(xiàn)，不利于控制。方案二：采用意法半導體公司的STM32F103單片機作為控制器。STM32系列單片機是32位、RISC、低功耗的處理器。在進行高精密的操作時，處理能力非常強，運算速度快，具有很好的控制能力，且成本低，更符合實驗要求。綜合考慮以上兩種方案，采用方案二。2.系統(tǒng)結(jié)構(gòu)根據(jù)上述方案的論證，我們確定以STC32F103作為控制核心，采用型號為PFB0812XHE的直流軸風機控制擺桿運動，用陀螺儀MPU6050檢測狀態(tài)數(shù)據(jù)，并將采集到的數(shù)據(jù)傳輸給控制板，然后通過單片

4、機計算處理得出擺桿的姿態(tài)并調(diào)整直流軸風機的轉(zhuǎn)速，從而使擺桿快速獲得需要的狀態(tài)，通過對應的無線遙控，設(shè)置相應的功能并發(fā)送給控制板，使其實現(xiàn)對應的功能，完成任務要求。系統(tǒng)設(shè)計框圖如圖1所示直流軸風機STM32F103最小系統(tǒng)模塊無線接收顯示按鍵STM32F103最小系統(tǒng)模塊陀螺儀模塊無線發(fā)送圖1 設(shè)計系統(tǒng)框圖2 系統(tǒng)理論分析與計算1.直流軸風機選型本系統(tǒng)采用型號為PFB0812XHE的直流軸風機控制擺桿運動，電壓為12V，電流4.65A。此型號直流軸風機大小適中，重量較輕，產(chǎn)生的風力較大且能較快的啟動，符合本系統(tǒng)要求。2.擺桿狀態(tài)檢測本系統(tǒng)采用陀螺儀MPU6050采集擺桿的當前姿態(tài)數(shù)據(jù)，將采集到的

5、數(shù)據(jù)實時傳遞給單片機，通過單片機PID計算處理后控制PWM的輸出，使直流軸流風機轉(zhuǎn)速改變，進而使擺桿達到相應的任務要求。3.運動控制風力擺的運動是連續(xù)運動，擺桿的變化也是連續(xù)的變化過程，因此我們采用PID控制算法。對直流電機或多圈電位器為執(zhí)行器件的系統(tǒng)中，基本采用增量式PID算法進行控制。由于單個直流軸風機，只能產(chǎn)生一個方向的力，為了便于控制，我們將4個直流軸風機直立圍繞成一個正方形（出風口向外），使四周都能產(chǎn)生相同的風力。然后單片機將陀螺儀MPU6050采集到的數(shù)據(jù)，確定其當前擺桿的姿態(tài)，在進行PID算法以后，將得到的結(jié)果對應去控制直流軸風機的轉(zhuǎn)速，以達到控制擺桿完成相應動作的效果三電路與程

6、序設(shè)計1電路設(shè)計（1）陀螺儀模塊陀螺儀MPU6050模塊的實物硬件圖如圖3所示。它與單片機的連接圖如圖附錄總圖所示。在這里主要使用的是它的3軸定位功能和加速度功能。圖2 MPU6050硬件模塊圖（2）最小系統(tǒng)模塊STM32單片機是意法半導體公司生產(chǎn)的32位低功耗、具有精簡指令集的混合信號處理器。其主要特點是處理能力強、運算速度快、低功耗等。在這里主要使用開發(fā)板原理圖如圖4所示。圖3 最小系統(tǒng)板原理圖2程序設(shè)計程序功能描述本系統(tǒng)的軟件部分主要由數(shù)據(jù)采集模塊、無線接收和發(fā)送、直流軸流風機PWM、PID控制模塊函數(shù)組成。當單片機上電后，根據(jù)不同的按鍵，遙控模塊無線發(fā)送對應的工作模式，風力擺的控制板按

7、照接收到的不同工作模式，對應的完成不同的的要求工作。軟件部分可實現(xiàn)對讀取陀螺儀輸出的數(shù)據(jù)信號，對直流軸流風機進行控制，實現(xiàn)擺桿的運動。程序流程圖如圖5所示。部分程序如附錄一所示。模式一開始模式二模塊初始化模式三工作模式按鍵無線發(fā)送無線接收模式四數(shù)據(jù)采集模式五 PID處理模式六直流軸風機PWM控制圖4 系統(tǒng)軟件流程圖模式一：從靜止開始，15s內(nèi)控制風力擺做自由擺運動，使激光筆穩(wěn)定地在地面畫出一條長度不短于50cm的直線段，其線性度偏差不大于正負2.5cm，并且具有較好的重復性；模式二：從靜止開始，15s內(nèi)完成幅度可控的擺動，畫出長度在3060cm間可設(shè)置，長度偏差不大于正負2.5cm的直線段，并

8、且具有較好的重復性；模式三：可設(shè)定擺動方向，風力擺從靜止開始，15s內(nèi)按照設(shè)置的方向（角度）擺動，畫出不短于20cm的直線段；模式四：將風力擺拉起一定角度（3045度）放開，5s內(nèi)是風力擺制動達到靜止狀態(tài)；模式五：以風力擺靜止時激光筆的光點為圓心，驅(qū)動風力擺用激光筆在地面花園，30s內(nèi)需重復3次；圓半徑可在1535cm范圍內(nèi)設(shè)置，激光筆畫出的軌跡應落在制定半徑2.5cm的圓環(huán)內(nèi)；模式六：在發(fā)揮部分（1）后繼續(xù)作圓周運動，在距離風力擺12m距離內(nèi)用一臺5060w臺扇在水平方向吹風力擺，臺扇吹5s后停止，風力擺能夠在5s內(nèi)恢復發(fā)揮部分（1）規(guī)定的圓周運動，激光筆畫出符合要求的軌跡。四測試方案與測試

9、結(jié)果1測試方案按照調(diào)試的順序，按照以下步驟進行調(diào)試。1）調(diào)試直流軸流風機的（PWM）速度2）調(diào)試單片機與陀螺儀之間的數(shù)據(jù)傳輸3）調(diào)試系統(tǒng)PID控制的三個參數(shù)值：balance.kp、balance.ki、balance.kd4）調(diào)試各模式相應直流軸流風機的轉(zhuǎn)速5）調(diào)試兩個無線模塊的數(shù)據(jù)傳輸2測試儀器：數(shù)字示波器，秒表，米尺，量角尺，風速儀，12V5A直流電源。3測試結(jié)果硬件實物圖如附錄三所示。（1）直流軸流風機的PWM能夠正?？刂破溥\轉(zhuǎn)速度（2）陀螺儀能正常的將數(shù)據(jù)發(fā)送給控制板，其中陀螺儀MPU6050模塊的通信模式為IIC（3）PID控制的調(diào)試：確定balance.kp、balance.kd

10、、balance.kd三個參數(shù)的值（4）各模式直流軸流風機的轉(zhuǎn)速，需要反復測試，目前已達到比較理想的轉(zhuǎn)速，其測試結(jié)果如下表一表一 測試數(shù)據(jù)表1次2次3次4次模式125s/30cm20s/40cm16s/57cm14s/53cm模式217s/68(50)cm14s/52(50)cm13s/41(40)cm11s/33(30)cm模式320度/13cm20度23cm30度/25cm270度/21cm模式414s14s6s4s模式515cm/23cm15cm/17cm25cm/22cm35cm/36cm模式68s6s3s4s（5）兩個無線NRF模塊，能夠較精確相互收發(fā)數(shù)據(jù)，完成兩塊單片機之間的數(shù)據(jù)傳

11、遞4.總結(jié)本次設(shè)計的風力擺控制系統(tǒng)完全可以實現(xiàn)設(shè)計基本要求和發(fā)揮部分的要求，系統(tǒng)穩(wěn)定性較好，成功的完成了本次比賽的任務和要求。參考文獻【1】全國大學生電子設(shè)計競賽組委會 編全國大學生電子設(shè)計競賽獲獎作品匯編M.北京理工大學出版社.2006年【2】黃正瑾.電子設(shè)計競賽賽體解析M.東南大學出版社. 2003年.第五章【3】周航慈.單片機應用程序設(shè)計M. 北京航空航天大學出版社.2006年 【4】李朝青.單片機原理及接口技術(shù)M. 北京航空航天大學出版社. 2005年.第四章附錄一：include "sys.h"#include "usart.h"#includ

12、e "delay.h"#include "led.h"#include "key.h"#include "beep.h"#include "24l01.h"#include "spi.h"#include "imu.h"#include "motor.h"#include "mpu6050.h"u8 Rx_bufRX_PLOAD_WIDTH;int main(void)u8 key_board;u8 i=0; Stm

13、32_Clock_Init(9);/系統(tǒng)時鐘設(shè)置delay_init(72); LED_Init(); uart_init(72,);KEY_Init();TIM4_PWM_Init(999,0);mpu6050_init();delay_ms(200);mpu6050_update();Timer3_Init(199,719);NRF24L01_Init();while(NRF24L01_Check()LED1 = LED1;delay_ms(30);printf("NRF24L01 OK!");NRF24l01_poweroff();delay_ms(3);NRF24L

14、01_RX_Mode();delay_ms(3);while(1)key_board = KEY_Scan(0);if(key_board=1)/基本功能1while(1)test_1(40);if(key_board=4)while(1)test_2(45);if(key_board=5)while(1)for(i=0;i<4;i+) for(i=0;i<3;i+) test_5(20);delay_ms(1000); for(i=0;i<3;i+) TIM4->CCR1=1000; delay_ms(1000); TIM4->CCR1=400; while(1

15、)if(angle_rao_x<0)&&(angle_rao_y<0)TIM4->CCR1=400;TIM4->CCR4=400;TIM4->CCR2=800;TIM4->CCR3=400;delay_ms(1000);if(angle_rao_x<0)&&(angle_rao_y>0)TIM4->CCR1=800;TIM4->CCR2=800;TIM4->CCR3=400;TIM4->CCR4=400;delay_ms(1000);if(angle_rao_x>0)&&

16、;(angle_rao_y>0) TIM4->CCR1=800; TIM4->CCR4=400;TIM4->CCR2=400;TIM4->CCR3=400;delay_ms(1000);if(angle_rao_x>0)&&(angle_rao_y<0)TIM4->CCR3=600;TIM4->CCR4=700;TIM4->CCR1=400;TIM4->CCR2=400;delay_ms(1000);#include "24l01.h"#include "delay.h"#

17、include "spi.h"const u8 TX_ADDRESSTX_ADR_WIDTH=0x01,0x02,0x03,0x04,0x05; /發(fā)送地址const u8 RX_ADDRESSRX_ADR_WIDTH=0x01,0x02,0x03,0x04,0x05; /發(fā)送地址void NRF24l01_poweroff(void)NRF24L01_Write_Reg(WRITE_REG+CONFIG,0x00); void NRF24L01_Init(void) RCC->APB2ENR|=1<<3; /使能PORTB時鐘 RCC->APB2E

18、NR|=1<<2; /使能PORTA時鐘 GPIOA->CRL&=0XFFF0FFFF; GPIOA->CRL|=0X;/PG6 7 推挽 GPIOA->CRH&=0XFFFFFFF0; GPIOA->CRH|=0X;/PG6 7 推挽 GPIOB->CRL&=0XFFFFFFF0; GPIOB->CRL|=0X;/PG8 輸入 SPI1_Init(); /初始化SPI SPI1->CR1&=(1<<6); /SPI設(shè)備失能SPI1->CR1&=(1<<1); /空閑模式

19、下SCK為0 CPOL=0SPI1->CR1&=(1<<0); /數(shù)據(jù)采樣從第1個時間邊沿開始,CPHA=0 SPI1->CR1|=1<<6; /SPI設(shè)備使能NRF24L01_CE=0; /使能24L01NRF24L01_CSN=1;/SPI片選取消 u8 NRF24L01_Check(void)u8 buf5=0XA5,0XA5,0XA5,0XA5,0XA5;u8 i;SPI1_SetSpeed(SPI_SPEED_16); /spi速度為9Mhz（24L01的最大SPI時鐘為10Mhz） NRF24L01_Write_Buf(WRITE_REG

20、+TX_ADDR,buf,5);/寫入5個字節(jié)的地址.NRF24L01_Read_Buf(TX_ADDR,buf,5); /讀出寫入的地址 for(i=0;i<5;i+)if(bufi!=0XA5)break; if(i!=5)return 1;/檢測24L01錯誤return 0; /檢測到24L01 u8 NRF24L01_Write_Reg(u8 reg,u8 value)u8 status; NRF24L01_CSN=0; /使能SPI傳輸 status =SPI1_ReadWriteByte(reg);/發(fā)送寄存器號 SPI1_ReadWriteByte(value); /寫入

21、寄存器的值 NRF24L01_CSN=1; /禁止SPI傳輸 return(status); /返回狀態(tài)值u8 NRF24L01_Read_Reg(u8 reg)u8 reg_val; NRF24L01_CSN = 0; /使能SPI傳輸 SPI1_ReadWriteByte(reg); /發(fā)送寄存器號 reg_val=SPI1_ReadWriteByte(0XFF);/讀取寄存器內(nèi)容 NRF24L01_CSN = 1; /禁止SPI傳輸 return(reg_val); /返回狀態(tài)值u8 NRF24L01_Read_Buf(u8 reg,u8 *pBuf,u8 len)u8 status,u

22、8_ctr; NRF24L01_CSN = 0; /使能SPI傳輸 status=SPI1_ReadWriteByte(reg);/發(fā)送寄存器值(位置),并讀取狀態(tài)值 for(u8_ctr=0;u8_ctr<len;u8_ctr+)pBufu8_ctr=SPI1_ReadWriteByte(0XFF);/讀出數(shù)據(jù) NRF24L01_CSN=1; /關(guān)閉SPI傳輸 return status; /返回讀到的狀態(tài)值u8 NRF24L01_Write_Buf(u8 reg, u8 *pBuf, u8 len)u8 status,u8_ctr; NRF24L01_CSN = 0; /使能SPI傳

23、輸 status = SPI1_ReadWriteByte(reg);/發(fā)送寄存器值(位置),并讀取狀態(tài)值 for(u8_ctr=0; u8_ctr<len; u8_ctr+)SPI1_ReadWriteByte(*pBuf+); /寫入數(shù)據(jù) NRF24L01_CSN = 1; /關(guān)閉SPI傳輸 return status; /返回讀到的狀態(tài)值 u8 NRF24L01_TxPacket(u8 *txbuf)u8 sta; SPI1_SetSpeed(SPI_SPEED_16);/spi速度為9Mhz（24L01的最大SPI時鐘為10Mhz） NRF24L01_CE=0; NRF24L01

24、_Write_Buf(WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);/寫數(shù)據(jù)到TX BUF 32個字節(jié) NRF24L01_CE=1;/啟動發(fā)送 while(NRF24L01_IRQ!=0);/等待發(fā)送完成sta=NRF24L01_Read_Reg(STATUS); /讀取狀態(tài)寄存器的值 NRF24L01_Write_Reg(WRITE_REG+STATUS,sta); /清除TX_DS或MAX_RT中斷標志if(sta&MAX_TX)/達到最大重發(fā)次數(shù)NRF24L01_Write_Reg(FLUSH_TX,0xff);/清除TX FIFO寄存器 return MA

25、X_TX; if(sta&TX_OK)/發(fā)送完成return TX_OK;return 0xff;/其他原因發(fā)送失敗u8 NRF24L01_RxPacket(u8 *rxbuf)u8 sta; SPI1_SetSpeed(SPI_SPEED_16); /spi速度為9Mhz（24L01的最大SPI時鐘為10Mhz） sta=NRF24L01_Read_Reg(STATUS); /讀取狀態(tài)寄存器的值 NRF24L01_Write_Reg(WRITE_REG+STATUS,sta); /清除TX_DS或MAX_RT中斷標志if(sta&RX_OK)/接收到數(shù)據(jù)NRF24L01_Re

26、ad_Buf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);/讀取數(shù)據(jù)NRF24L01_Write_Reg(FLUSH_RX,0xff);/清除RX FIFO寄存器 return 0; return 1;/沒收到任何數(shù)據(jù) void NRF24L01_RX_Mode(void)NRF24L01_CE=0; NRF24L01_Write_Buf(WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH);/寫RX節(jié)點地址 NRF24L01_Write_Reg(WRITE_REG+EN_AA,0x01); /使能通道0的自動應答 NRF

27、24L01_Write_Reg(WRITE_REG+EN_RXADDR,0x01);/使能通道0的接收地址 NRF24L01_Write_Reg(WRITE_REG+RF_CH,40); /設(shè)置RF通信頻率 NRF24L01_Write_Reg(WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);/選擇通道0的有效數(shù)據(jù)寬度 NRF24L01_Write_Reg(WRITE_REG+RF_SETUP,0x0f);/設(shè)置TX發(fā)射參數(shù),0db增益,2Mbps,低噪聲增益開啟 NRF24L01_Write_Reg(WRITE_REG+CONFIG, 0x0f);/配置基本工作模式的參

28、數(shù);PWR_UP,EN_CRC,16BIT_CRC,接收模式 NRF24L01_CE = 1; /CE為高,進入接收模式 void NRF24L01_TX_Mode(void) NRF24L01_CE=0; NRF24L01_Write_Buf(WRITE_REG+TX_ADDR,(u8*)TX_ADDRESS,TX_ADR_WIDTH);/寫TX節(jié)點地址 NRF24L01_Write_Buf(WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); /設(shè)置TX節(jié)點地址,主要為了使能ACK NRF24L01_Write_Reg(WRITE_REG+

29、EN_AA,0x01); /使能通道0的自動應答 NRF24L01_Write_Reg(WRITE_REG+EN_RXADDR,0x01); /使能通道0的接收地址 NRF24L01_Write_Reg(WRITE_REG+SETUP_RETR,0x1a);/設(shè)置自動重發(fā)間隔時間:500us + 86us;最大自動重發(fā)次數(shù):10次 NRF24L01_Write_Reg(WRITE_REG+RF_CH,40); /設(shè)置RF通道為40 NRF24L01_Write_Reg(WRITE_REG+RF_SETUP,0x0f); /設(shè)置TX發(fā)射參數(shù),0db增益,2Mbps,低噪聲增益開啟 NRF24L0

30、1_Write_Reg(WRITE_REG+CONFIG,0x0e); /配置基本工作模式的參數(shù);PWR_UP,EN_CRC,16BIT_CRC,接收模式,開啟所有中斷NRF24L01_CE=1;/CE為高,10us后啟動發(fā)送void BEEP_Init(void)RCC->APB2ENR|=1<<5; /使能PORTD時鐘 GPIOD->CRL&=0XFFFFF0FF; GPIOD->CRL|=0X;/PD2 推挽輸出 BEEP=0;/關(guān)閉蜂鳴器輸出#include <stm32f10x_lib.h>#include <math.h&g

31、t;#include <stdio.h>#include "delay.h"#include "sys.h"#include "mpu6050.h"#include "imu.h"#include "shuxue.h"#include "sys.h"#define PI 3.#define FILTER_SIZE 30float P=1;static s16 acc_x_cail=-145,acc_y_cail=-35,acc_z_cail=-555; /起始條件

32、下，偏差，正常是4096，x和y軸都差不多，而z軸接近4900，因此減掉800/static s32 x_sum=0,y_sum=0,z_sum=0;static s16 x_hist_tabFILTER_SIZE ,y_hist_tabFILTER_SIZE ,z_hist_tabFILTER_SIZE ;s16 ave_acc_x ,ave_acc_y,ave_acc_z;s16 pmx,pmy,pmz;float v_angle_rao_x=0,v_angle_rao_y=0,piancha_x=0,piancha_y=0,wen_piao_x=0,wen_piao_y=0,angle_

33、rao_x=0,angle_rao_y=0;euler_angle real;double gyro_temp_drift_x,gyro_temp_drift_y,gyro_temp_drift_z;PPID axis13,axis24,axis_z;u16 run_time;double att33=1 , 0 , 0 ,0 , 1 , 0 ,0 , 0 , 1 ,;double datt33;void init_pid_para(void) axis24.rk=0; axis24.kp=100;/180; /210 axis24.kd=20000; /26000 axis24.ki=0.1

34、; /0 axis24.sek=0; axis24.ek1=0; axis13.rk=0; axis13.kp=100;/180; /210 axis13.kd=20000; /26000 axis13.ki=0.1; /0 axis13.sek=0; axis13.ek1=0; axis_z.rk=0; axis_z.kp=150; /200 axis_z.kd=7000; /8000 axis_z.ki=0.1; /0 axis_z.sek=0; axis_z.ek1=0;void acc_prepare_data_int(void)u8 i;for(i=0;i<FILTER_SIZ

35、E;i+)mpu6050_update();x_hist_tabi=ax+acc_x_cail;y_hist_tabi=ay+acc_y_cail;z_hist_tabi=az+acc_z_cail;x_sum+=x_hist_tabi;y_sum+=y_hist_tabi;z_sum+=z_hist_tabi;delay_ms(5);ave_acc_x=x_sum/FILTER_SIZE; ave_acc_y=y_sum/FILTER_SIZE; ave_acc_z=z_sum/FILTER_SIZE; void acc_prepare_data(void)static u8 idx=0;i

36、f(idx=FILTER_SIZE)idx=0;x_sum-=x_hist_tabidx;y_sum-=y_hist_tabidx;z_sum-=z_hist_tabidx;x_hist_tabidx=ax+acc_x_cail;y_hist_tabidx=ay+acc_y_cail;z_hist_tabidx=az+acc_z_cail;x_sum+=x_hist_tabidx;y_sum+=y_hist_tabidx;z_sum+=z_hist_tabidx;ave_acc_x=x_sum/FILTER_SIZE; ave_acc_y=y_sum/FILTER_SIZE; ave_acc_

37、z=z_sum/FILTER_SIZE; idx+;void TIM3_IRQHandler(void) if(TIM3->SR&0X0001)/溢出中斷mpu6050_update();acc_prepare_data();piancha_x=(atan2(ave_acc_y,ave_acc_z)*180/PI)-angle_rao_x;/this piancha is -,and it's unit is du /wen_piao_x+=KI_T*piancha_x;/this wen_piao_x is - /v_angle_rao_x=(gx/16.4+wen_p

38、iao_x)+KP_T*piancha_x;angle_rao_x+=v_angle_rao_x*0.002;/angle=v_angle*time/piancha_y=(-atan2(ave_acc_x,ave_acc_z)*180/PI)-(angle_rao_y);/this piancha is -,and it's unit is du /wen_piao_y+=KI_T*piancha_y;/this wen_piao_x is - /v_angle_rao_y=(gy/16.4+wen_piao_y)+KP_T*piancha_y;angle_rao_y+=v_angle

39、_rao_y*0.002;/angle=v_angle*time/axis24.uk=axis24.kp*axis24.ek+axis24.kd*(axis24.ek-axis24.ek1)+axis24.ki*axis24.sek;axis13.uk=axis13.kp*axis13.ek+axis13.kd*(axis13.ek-axis13.ek1)+axis13.ki*axis13.sek;axis_z.uk=axis_z.kp*axis_z.ek+axis_z.kd*(axis_z.ek-axis_z.ek1)+axis_z.ki*axis_z.sek;run_time= TIM3-

40、>CNT;TIM3->SR&=(1<<0);/清除中斷標志位 void Timer3_Init(u16 arr,u16 psc)RCC->APB1ENR|=1<<1;/TIM2時鐘使能 TIM3->ARR=arr; /設(shè)定計數(shù)器自動重裝值 TIM3->PSC=psc; /預分頻器7200,得到10Khz的計數(shù)時鐘TIM3->DIER|=1<<0; /允許更新中斷TIM3->DIER|=1<<6; /允許觸發(fā)中斷TIM3->CR1|=0x01; /使能定時器2 MY_NVIC_Init(0,1,TIM3_IRQChannel,2);/搶占3，子優(yōu)先級3，組2(組2中優(yōu)先級最低的) void ium_init(v