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黑下载附件…………

复制代码