PID调节电机转速 配合MPU6050

【实例截图】

【核心代码】

#include "timer.h"
#include "led.h"
#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "pid.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 
#include "motor.h"
#include "math.h"
#include "oled.h"
#include "key.h"

int main()
{
	extern float val;
	delay_init();	    	 		    	 		     //延时函数初始化
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); 	 //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
	uart_init(115200);	 	    	 		    	 	 //串口初始化为115200
	MPU_Init();				  							 //初始化MPU6050
	LED_Init();			     							 //LED端口初始化
	TIM3_PWM_Init(1899,0);						      	 //不分频。PWM频率=72000000/900=80Khz
	OLED_Init();
	Pid_init();
	Motor_init();
	while(mpu_dmp_init());
	LED0 = 0;
	TIM6_init(9,3599);
	TIM_Cmd(TIM6,DISABLE);
	KEY_Init();
	OLED_ShowString(0,0,"yaw = ",24);
	OLED_ShowString(0,24,"val = ",24);
	OLED_Refresh_Gram();								 //更新显示到OLED
	while(1)
	{
		if(KEY0==0)
		{
			delay_ms(10);
			if(KEY0==0)
			{
				TIM_Cmd(TIM6,ENABLE);
			}
			while(!KEY0);
		}
		if(KEY1==0)
		{
			delay_ms(10);
			if(KEY1==0)
			{
				TIM6_init(0,3599);
				Motor1_A=1;
				Motor1_B=0;
				Motor2_A=1;
				Motor2_B=0;
				Motor3_A=1;
				Motor3_B=0;
				TIM_SetCompare1(TIM3,1100);
				TIM_SetCompare2(TIM3,1100);
				TIM_SetCompare3(TIM3,1100);
			}
			while(!KEY1);
			if(KEY0==0)
			{
				delay_ms(10);
				if(KEY0==0)
				{
					Motor1_A=0;
					Motor1_B=1;
					Motor2_A=1;
					Motor2_B=0;
					Motor3_A=1;
					Motor3_B=1;
					TIM_SetCompare1(TIM3,1250);
					TIM_SetCompare2(TIM3,1250);
					TIM_SetCompare3(TIM3,1250);
				}
			}
			while(!KEY0);
		}
		if(WK_UP==1)
		{
			delay_ms(10);
			if(WK_UP==1)
			{
				TIM_Cmd(TIM6,DISABLE);
				Motor1_A=1;
				Motor1_B=1;
				Motor2_A=1;
				Motor2_B=1;
				Motor3_A=1;
				Motor3_B=1;
			}
			while(WK_UP);
		}
	}
}