2015年電賽練習風力擺代碼 STM32F4 + jy61姿態傳感器四路pwm 四個空心杯電機 PID

ID:293035 · 發表于 2020-3-25 09:55

2015年全國電子設計競賽練習風力擺代碼 STM32F4 + jy61姿態傳感器四路pwm控制四個空心杯電機 PID控制風力擺畫圓，直線等等

制作出來的實物圖如下：

單片機源程序如下:

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "lcd.h"
#include "remote.h"
#include "pwm.h"
#include "motor.h"
#include "math.h"
#include "timer.h"
//void Get_Angle();//獲取角度
//void Get_Accelerate();//獲取角度
//void Get_Palstance();//獲取角度
void LCD_display(void);
extern u8 Re_buf[11],counter;
extern u8 jiasudu[6],jiaosudu[6],jiaodu[6];
extern float Angle[3],Accelerate[3],Palstance[3],Temperature;
float Err_X,Err_Y,Err_X_Last,Err_Y_Last;
float X,Y;
float Aim_X,Aim_Y;
float Kp,Ki,Kd;
u32 pwm1 = 0,pwm2 = 0,pwm3 = 0,pwm4 = 0;
int main(void)
{
u8 key;
u8 *str=0;
Kp = 100;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//設置系統中斷優先級分組2
delay_init(168); //初始化延時函數
uart_init(115200); //初始化串口波特率為115200
USART2_Init(115200); //串口2初始化
delay_ms(1000);
TIM12_PWM_Init(10000-1,21-1);//PWM1
TIM13_PWM_Init(10000-1,21-1);//PWM2
TIM14_PWM_Init(10000,21-1);//PWM3
TIM5_PWM_Init(10000-1,21-1);//PWM4
TIM3_Init(200-1,8400-1); //定時器3初始化
Motor_Init(); //四路空心杯電機初始化
//LED_Init(); //初始化LED
LCD_Init();
Remote_Init(); //紅外接收初始化
POINT_COLOR=RED;//設置字體為紅色
LCD_ShowString(30,130,200,16,16,"PWM1:");
LCD_ShowString(30,150,200,16,16,"PWM2:");
LCD_ShowString(30,170,200,16,16,"PWM3:");
LCD_ShowString(30,190,200,16,16,"PWM4:");
LCD_ShowString(30,210,200,16,16,"X_Angle:");
LCD_ShowString(30,230,200,16,16,"Y_Angle:");
LCD_ShowString(30,250,200,16,16,"Z_Angle:");
Motor_Stop(); //電機置1高電平
//Motor_Stop1();
while(1)
{
LCD_display();
}
}
void TIM3_IRQHandler(void)//20ms進入一次中斷
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //溢出中斷
{
Aim_X = 12.0;
Aim_Y = 0;
X = Angle[0];
Y = Angle[1];
Err_X = X - Aim_X;
Err_Y = Y - Aim_Y;
if(Err_X <= 0)
{
Motor_Drive(2,1); //電機2正轉
Motor_Drive(4,0); //電機4反轉
pwm2 += Kp*(fabs(Err_X))*0.02;
pwm4 += Kp*(fabs(Err_X))*0.02;
}
else
{
Motor_Drive(2,1); //電機2反轉
Motor_Drive(4,0); //電機4正轉
pwm2 -= Kp*Err_X*0.02;
pwm4 -= Kp*Err_X*0.02;
}
if(Err_Y <= 0)
{
Motor_Drive(1,1); //電機1正轉
Motor_Drive(3,0); //電機3反轉
pwm1 += Kp*(fabs(Err_Y))*0.02;
pwm3 += Kp*(fabs(Err_Y))*0.02;
}
else
{
Motor_Drive(1,0); //電機1反轉
Motor_Drive(3,1); //電機3正轉
pwm1 += Kp*Err_Y*0.02;
pwm3 += Kp*Err_Y*0.02;
}
//限幅
if(pwm1 >= 9000) pwm1 = 9000;
if(pwm2 >= 9000) pwm2 = 9000;
if(pwm3 >= 9000) pwm3 = 9000;
if(pwm4 >= 9000) pwm4 = 9000;
TIM_SetCompare1(TIM13,pwm1);
TIM_SetCompare1(TIM14,pwm2);
TIM_SetCompare1(TIM12,pwm3);
TIM_SetCompare1(TIM5,pwm4);
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update); //清除中斷標志位
}
void LCD_display()
{
LCD_ShowNum(86,130,pwm1,5,16);
LCD_ShowNum(86,150,pwm2,5,16);
LCD_ShowNum(86,170,pwm3,5,16);
LCD_ShowNum(86,190,pwm4,5,16);
if(Angle[0] >= 0)
{
LCD_ShowChar(128,210,' ',16,0);
LCD_Showfloat(144,210,Angle[0],4,16);
}
else
{
LCD_ShowChar(128,210,'-',16,0);
LCD_Showfloat(144,210,fabs(Angle[0]),4,16);
}
if(Angle[1] >= 0)
{
LCD_ShowString(128,230,200,16,16," ");
LCD_Showfloat(144,230,Angle[1],4,16);
}
else
{
LCD_ShowString(128,230,200,16,16,"-");
LCD_Showfloat(144,230,fabs(Angle[1]),4,16);
}
if(Angle[2] >= 0)
{
LCD_ShowString(128,250,200,16,16," ");
LCD_Showfloat(144,250,Angle[2],4,16); //顯示鍵值
}
else
{
LCD_ShowString(128,250,200,16,16,"-");
LCD_Showfloat(144,250,fabs(Angle[2]),4,16); //顯示鍵值
}
LCD_Fill(86,170,116+8*8,170+16,WHITE); //清楚之前的顯示
}
/*
void Get_Angle()//獲取角度
{
Angle[0] = ((short)((jiaodu[1]<<8)|jiaodu[0]))/32768.0*180;//x軸角度
Angle[1] = ((short)((jiaodu[3]<<8)|jiaodu[2]))/32768.0*180;//Y軸角度
Angle[2] = ((short)((jiaodu[5]<<8)|jiaodu[4]))/32768.0*180;//Z軸角度
printf("x軸角度:%.3f,Y軸角度:%.3f,Z軸角度:%.3f\r\n",Angle[0],Angle[1],Angle[2]);
//delay_ms(1);
}
void Get_Accelerate()//獲取加速度
{
Accelerate[0] = ((short)((jiasudu[1]<<8)|jiasudu[0]))/32768.0*16;//X軸加速度
Accelerate[1] = ((short)((jiasudu[3]<<8)|jiasudu[2]))/32768.0*16;//Y軸加速度
Accelerate[2] = ((short)((jiasudu[5]<<8)|jiasudu[4]))/32768.0*16;//Z軸加速度
printf("x軸加速度:%.3f,Y軸加速度:%.3f,Z軸加速度:%.3f\r\n",Accelerate[0],Accelerate[1],Accelerate[2]);
delay_ms(10);
}
void Get_Palstance()//獲取角速度
{
Palstance[0] = ((short)((jiaosudu[1]<<8)|jiaosudu[0]))/32768.0*2000;//X軸角速度
Palstance[1] = ((short)((jiaosudu[3]<<8)|jiaosudu[2]))/32768.0*2000;//Y軸角速度
Palstance[2] = ((short)((jiaosudu[5]<<8)|jiaosudu[4]))/32768.0*2000;//Z軸角速度
printf("x軸角速度:%.2f,Y軸角速度:%.2f,Z軸角速度:%.2f\r\n",Palstance[0],Palstance[1],Palstance[2]);
delay_ms(10);
}
*/
/*
key=Remote_Scan();
if(key)
{
LCD_ShowNum(86,130,pwm1,5,16); //顯示鍵值
LCD_ShowNum(86,150,pwm2,5,16); //顯示鍵值
LCD_ShowNum(86,170,pwm3,5,16); //顯示鍵值
LCD_ShowNum(86,190,pwm4,5,16); //顯示鍵值
switch(key)
{
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………