2013年电赛倒立摆 STM32代码高精度金属电位器速度环+位置环PID控制

ID:293035 · 发表于 2020-3-25 09:35

2013年全国电子设计竞赛倒立摆代码 STM32 + 高精度金属电位器速度环+位置环PID控制

制作出来的实物图如下：
A`)%0]I[A[DB0%6I}_Y9W5J.png

单片机源程序如下:

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "lcd.h"
#include "remote.h"
#include "motor.h"
#include "pwm.h"
#include "timer.h"
#include "math.h"
#include "key.h"
#include "adc.h"
void LCD_Display(void);
float Volt_To_Angle(float volt);
void Start_Action(void);
void Q_1(void);
void Q_2(void);
void Q_3(void);
void Q_4(void);
void Q_5(void);
void Q_6(void);
extern u8 TIM5CH1_CAPTURE_STA; //输入捕获状态
extern u32 Frequency; //输入捕获值
extern float Period;
extern u8 Motor_Flag; //Motor_flag = 0;反转 = 1时正转
extern u32 pwm_count;
u32 adc1;
int pwm = 1500;
float Err_Angle,Angle,Angle_Last,Err_Angle_Sum,Err_Angle_Last,volt1;
float Kp=30,Ki,Kd;
float Aim_Angle;
float R_x,adc2;
u8 count; //记录第一次情况
u8 mode = 1;//电机正反转标志
int main(void)
{
u8 key;
u8 t=0;
u8 *str=0;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168); //初始化延时函数
delay_ms(1000);
delay_ms(1000);
uart_init(115200); //初始化串口波特率为115200
KEY_Init();
Motor_Init(); //电机初始化
Adc_Init(); //adc初始化
TIM3_Init(100-1,8400-1); //50HZ 每20ms进入一次中断
TIM14_PWM_Init(5000-1,21-1);
TIM5_CH1_Cap_Init();
//LED_Init(); //初始化LED
LCD_Init();
Remote_Init(); //红外接收初始化
POINT_COLOR=RED;//设置字体为红色
LCD_ShowString(30,50,200,16,16,"Kp:");
LCD_ShowString(30,70,200,16,16,"Ki:");
LCD_ShowString(30,90,200,16,16,"Kd:");
LCD_ShowString(30,110,200,16,16,"PWM:");
LCD_ShowString(30,130,200,16,16,"volt1:");
LCD_ShowString(30,150,200,16,16,"Angle:");
LCD_ShowString(30,170,200,16,16,"Err_Angle:");
LCD_ShowString(30,190,200,16,16,"PWM_Count:");
while(1)
{
adc1=Get_Adc_Average(ADC_Channel_5,20);//获取通道5的转换值，20次取平均
adc2=(float)adc1*(3.3/4096);
key = KEY_Scan(0);
switch(key)
{
case 1: Kp += 5;break;
case 2: Kp -= 5;break;
}
//Start_Action();
//Motor_Drive(0);
//Q_2();
TIM_SetCompare1(TIM14,pwm);
Angle_Last = Angle;
//printf("%d\t%5d\r\n",pwm_count,mode);
/*
if(Motor_Flag == 1)
{
printf("正转\r\n");
}else
printf("反转\r\n");
//TIM5CH1_CAPTURE_STA=0; //开启下一次捕获
*/
LCD_Display(); //显示参数
delay_ms(1);
}
}
u8 count1 = 0;
void TIM3_IRQHandler()
{
if (TIM_GetITStatus(TIM3, TIM_IT_Update)!=RESET)//判断是否是溢出中断
{
/* 计算实际电压值（近似估算的），如需准确，请进行校准 */
Aim_Angle = 276.0;
//Kp = 30;
Ki = 0;
Kd = 800;
printf("adc1 = %d\tadc2 = %f\r\n",adc1,adc2);
Err_Angle = Aim_Angle-Angle;
Err_Angle_Sum += Err_Angle;
if(Err_Angle_Sum > 3000) Err_Angle_Sum = 3000;
if(Err_Angle_Sum <-3000) Err_Angle_Sum =-3000;
if(Angle >= 230 && Angle <= 340)
{
if(Err_Angle > 0)
{
pwm = 300 + Kp*Err_Angle + Ki*Err_Angle_Sum + Kd*(Err_Angle-Err_Angle_Last);
}else
pwm = -300 + Kp*Err_Angle + Ki*Err_Angle_Sum + Kd*(Err_Angle-Err_Angle_Last);
if(pwm > 4000) pwm = 4000;
if(pwm <-4000) pwm =-4000;
if(pwm > 0) Motor_Drive(0);
if(pwm < 0) Motor_Drive(1);
pwm = abs(pwm);
}
else
Q_1();
Err_Angle_Last = Err_Angle;
TIM_SetCompare1(TIM14,pwm);
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);//清除溢出中断标志位
}
void Start_Action()
{
if(count == 0)
{
mode = 0;
Motor_Drive(mode);
pwm = 1300;
pwm_count = 0;
}
else
{
if(pwm_count >= 10)
{
pwm_count = 0;
mode = ~mode;
pwm = 1300 - 130*pwm_count;
Motor_Drive(mode);
}
count = 1;
}
count++;
}
void Q_1()
{
if(count == 0)
{
pwm = 1500;
TIM_SetCompare1(TIM14,pwm);
delay_ms(500);
}else
{
pwm = 2000;
if(Angle >= 90)
{
if(Angle > Angle_Last)
{
Motor_Drive(1);
TIM_SetCompare1(TIM14,pwm);
}
else
{
Motor_Drive(0);
TIM_SetCompare1(TIM14,pwm);
}
}else
{
if(Angle > Angle_Last)
{
Motor_Drive(0);
TIM_SetCompare1(TIM14,pwm);
}
else
{
Motor_Drive(1);
TIM_SetCompare1(TIM14,pwm);
}
}
count = 1;
}
count++;
}
void Q_2()
{
if(count == 0)
{
pwm = 4000;
Motor_Drive(1);
TIM_SetCompare1(TIM14,pwm);
delay_ms(400);
count = 1;
}
else if(count == 1)
{
pwm = 3000;
count = 2;
Motor_Drive(0);
//Motor_Stop();
TIM_SetCompare1(TIM14,pwm);
delay_ms(200);
}
else
{
pwm = 0;
count = 3;
Motor_Stop();
//delay_ms(1000);
}
}
void LCD_Display(void)
{
LCD_Showfloat(86,50,Kp,4,16);
LCD_Showfloat(86,70,Ki,4,16);
LCD_Showfloat(86,90,Kd,4,16);
LCD_ShowNum(86,110,pwm,4,16);
LCD_Showfloat(86,130,volt1,4,16);
LCD_Showfloat(86,150,Angle,5,16);
LCD_Showfloat(86,170,Err_Angle,5,16);
LCD_ShowNum(86,190,pwm,3,16);
}
float Volt_To_Angle(float volt)
{
R_x = 5100*volt / (4.10 - volt);
Angle = (R_x / 4860) * 360;
return Angle;
}
/*
key=Remote_Scan();
if(key)
{
LCD_ShowNum(86,130,key,3,16); //显示键值
LCD_ShowNum(86,150,RmtCnt,3,16); //显示按键次数
switch(key)
{
case 0:str="ERROR";break;
case 162:str="POWER";break;
case 98:str="UP";break;
case 2:str="PLAY";break;
case 226:str="ALIENTEK";break;
case 194:str="RIGHT";break;
case 34:str="LEFT";break;
case 224:str="VOL-";break;
case 168:str="DOWN";break;
case 144:str="VOL+";break;
case 104:str="1";break;
case 152:str="2";break;
case 176:str="3";break;
case 48:str="4";break;
case 24:str="5";break;
case 122:str="6";break;
case 16:str="7";break;
case 56:str="8";break;
case 90:str="9";break;
case 66:str="0";break;
case 82:str="DELETE";break;
}
LCD_Fill(86,170,116+8*8,170+16,WHITE); //清楚之前的显示
LCD_ShowString(86,170,200,16,16,str); //显示SYMBOL
}else delay_ms(10);
t++;
if(t==20)
{
t=0;
LED0=!LED0;
}
*/

复制代码

所有资料51hei提供下载:

倒立摆.7z (356.13 KB, 下载次数: 87)

ID:758611 · 发表于 2020-8-11 19:40

这个感觉速度环没有加吧

帐号		自动登录	找回密码
密码			立即注册

2013年电赛倒立摆 STM32代码 高精度金属电位器 速度环+位置环PID控制

相关帖子

2013年电赛倒立摆 STM32代码高精度金属电位器速度环+位置环PID控制