基于stm32f103zet6+MPU6050的風(fēng)板控制系統(tǒng)源程序

ID:324282 · 發(fā)表于 2019-7-28 15:06

系統(tǒng)采用stm32f103ZET6作為控制器，使用MPU6050作為角度反饋，通過位置式PID控制風(fēng)板到任意角度。歡迎技術(shù)交流。聯(lián)系1194735786.

單片機(jī)源程序如下:

#include "led.h"
#include "key.h"
#include "timer.h"
#include "contral.h"
#include "oled.h"
#include "mpu6050.h"
#include "beep.h"
#include "delay.h"
#include "sys.h"
#include "usart.h"
#define PWM_INT 3000
/************************************************
風(fēng)機(jī)：左（PA6）右（PA7）
OLED GND 電源地
VCC 接5V或3.3v電源
D0 接PD6（SCL） KEY0: PEin(4) //PE4
D1 接PD7（SDA） KEY1: PEin(3) //PE3
RES 接PD4 WK_UP: PAin(0) //PA0 WK_UP
DC 接PD5 BEEP: PBout(8)
CS 接PD3
陀螺儀 TX：PA10 LED0：PB5
RX：PA9 LED1: PE5
************************************************/
/***********全局變量**************/
vu8 key=0;
int fengji_zuo,fengji_you;
u16 led0pwmval=0,ax;
u8 dir=1;
int pwm_left,pwm_right;
int ek;
void pid_ctrl(u16 kp,u16 kd,int experd)
{
// int exp;
// if(experd<0)
// exp = experd -0;
// else if(experd <15)
// exp = experd -0;
// else
// exp = experd +0;
ek = experd - angle[0];
if(ek < 0)
{
pwm_left = PWM_INT + kp*ek + kd*w[0];
pwm_right= PWM_INT - kp*ek - kd*w[0];
}
else
{
pwm_left = PWM_INT + kp*ek + kd*w[0];
pwm_right= PWM_INT - kp*ek - kd*w[0];
}
//low_data
if(pwm_left < 1500)
pwm_left = 1500;
else
pwm_left = pwm_left;
if(pwm_right < 1500)
pwm_right = 1500;
else
pwm_right = pwm_right;
//hight_data
if(pwm_left > 7200)
pwm_left = 7200;
else
pwm_left = pwm_left;
if(pwm_right > 7200)
pwm_right = 7200;
else
pwm_right = pwm_right;
}
int main(void)
{
delay_init(); //延時(shí)函數(shù)初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級(jí)，2位響應(yīng)優(yōu)先級(jí)
uart_init(115200); //串口初始化為115200
LED_Init(); //LED端口初始化
OLED_Init(); //OLED???
OLED_Clear(); //??
BEEP_Init(); //初始化蜂鳴器端口
KEY_Init(); //初始化與按鍵連接的硬件接口
TIM3_PWM_Init(7199,0); //不分頻。PWM頻率=72000000/7200=10Khz
OLED_ShowCHinese(00,0,1); //加
OLED_ShowCHinese(16,0,2); //速
OLED_ShowCHinese(32,0,3); //度
OLED_ShowCHinese(00,2,0); //角
OLED_ShowCHinese(16,2,2); //速
OLED_ShowCHinese(32,2,3); //度
OLED_ShowCHinese(00,4,0); //角
OLED_ShowCHinese(16,4,3); //度
while(1)
{
mpu6050();
key_tihao();
/*************PID********************/
pid_ctrl(400,5,exp_set);
OLED_ShowNum(72,0,a[0],3,16);
OLED_ShowNum(72,2,w[0],3,16);
TIM_SetCompare1(TIM3,pwm_left ); // pwm_left 2100
TIM_SetCompare2(TIM3,pwm_right); // pwm_right
if(w[0] < 0)
{
OLED_ShowChar(64,2,'-');
OLED_ShowNum(72,2,-w[0],3,16);
}
else
{
OLED_ShowChar(64,2,' ');
OLED_ShowNum(72,2,w[0],3,16);
}
/************角度顯示******************/
if(angle[0] < 0)
{
OLED_ShowChar(48,4,'-');
ax = -angle[0];
OLED_ShowNum(64,4,-angle[0],3,16);
}
else
{
OLED_ShowChar(48,4,' ');
OLED_ShowNum(64,4,angle[0],3,16);
}
if(exp_set < 0)
{
OLED_ShowChar(96,4,'-');
OLED_ShowNum(102,4,-exp_set,3,16);
}
else
{
OLED_ShowChar(96,4,' ');
OLED_ShowNum(102,4,exp_set,3,16);
}
OLED_ShowNum(0,6,pwm_left,4,16);
OLED_ShowNum(72,6,pwm_right,4,16);
/************************************/
// delay_ms(10);
//
// if(dir)led0pwmval++;
// else led0pwmval--;
// if(led0pwmval>36000)dir=0;
// if(led0pwmval==0)dir=1;
// TIM_SetCompare1(TIM3,led0pwmval);
// TIM_SetCompare2(TIM3,led0pwmval);
// LED1=1;
// delay_ms(300); //延時(shí)300ms
// LED1=0;
// delay_ms(300); //延時(shí)300ms
}
}