电子设计竞赛控制组——完整旋转倒立摆程序

大家好，又见面了，我是你们的朋友全栈君。如果您正在找激活码,请点击查看最新教程,关注关注公众号 “全栈程序员社区” 获取激活教程,可能之前旧版本教程已经失效.最新Idea2022.1教程亲测有效,一键激活。

Jetbrains全系列IDE使用 1年只要46元售后保障童叟无欺

以前也想过要写博客，但是却一直没有付诸于实践，作为第一篇原创，我还是选择将以前电赛时的作品拿出来，毕竟当初可是花费了好多心血的，汗~

旋转倒立摆是控制组校内赛练手的题目，需要对PID非常熟悉才能调好参数，以下代码是自己搭建好结构后调试出来的程序，其中的参数会根据不同的结构作出调整。

结构组成：

K60开发板（带液晶屏和按键），角度编码器，直流减速电机（带编码器），12V的电机驱动，金属摆臂，12V的电池组。

开发环境：

IAR 7.2

另：以下就是main函数部分了，详细的工程就不展示，核心部分基本都在这儿了，这些能看懂的话，作品也基本没什么问题了，就这样了，哈！

/********************************************************
――――――――――――――――――――――――――――――
**************************旋转倒立摆*********************
――――――――――――――――――――――――――――――
********************************************************/
# include “include.h”
# include “common.h”

#define angle ADC0_SE11// 定义角度传感器引脚为A8

//角度传感器
int16 angle_var = 0;//定义角度变量
int16 banlace_angle=2390;//定义摆杆平衡位置//2390

//编码器变量
int16 Voltage=0;//电机转速
int16 PWM = 0;//最终占空比
int16 val=0;//定义电机转速积分,经过低通滤波

//电机变量
int16 moto1,moto2,dir;

//函数声明
void PIT0_IRQHandler(void);
void Angle_PD_Realize(int16 angle_var,int16 real_angle);
void Speed_PID_Realize(int16 speed);
int16 PID_PWM();
void ADC(void);
void Init_All(void);

/*********************************************************
PID结构体定义
*********************************************************/
struct Speed_PID
{

int16 Last_Speed;//定义上一次速度
int16 Error_Now;//定义现在速度偏差
int16 Error_Next;//定义上一次偏差
int16 Error_Last;//定义上上次偏差
float Speed_KP,Speed_KI,Speed_KD;//定义PID参数
int16 integral_Voltage;//定义电机转速积分
int16 SPEED_PWM;//定义速度偏差补偿占空比
}sp;

struct Angle_PD
{

int16 Set_Angle;//期望角度
int16 Real_Angle;//实际角度
int16 Error_Now;//现在偏差
int16 Error_Next;//上一次偏差
float Angle_KP,Angle_KI,Angle_KD;//PD参数
int16 ANGLE_PWM;//定义角度偏差补偿占空比
}ag;

/******************************************************
角度PID算法实现
******************************************************/
void Angle_PD_Realize(int16 banlace_angle,int16 angle_var)
{

ag.Set_Angle = banlace_angle;
ag.Real_Angle = angle_var;
ag.Error_Now = ag.Set_Angle – ag.Real_Angle;//现在角度偏差
ag.ANGLE_PWM = (int16)(ag.Angle_KP*ag.Error_Now +ag.Angle_KD*(ag.Error_Now-ag.Error_Next));// 输出与偏差成正比，与偏差率成正比
ag.Error_Next = ag.Error_Now;//保存这次的误差

}

/******************************************************
速度PID算法实现
******************************************************/
void Speed_PID_Realize(int16 speed)
{

sp.Error_Now = Voltage-sp.Last_Speed;//现在速度偏差
sp.SPEED_PWM = (int16)(sp.Speed_KP*val+sp.Speed_KI*sp.integral_Voltage + sp.Speed_KD*(sp.Error_Now-2*sp.Error_Next+sp.Error_Last));
sp.Last_Speed=Voltage;//保存本次速度
sp.Error_Last = sp.Error_Next;//保存上一次误差
sp.Error_Next = sp.Error_Now;//保存本次误差
}

/******************************************************
最终占空比调节
******************************************************/
int16 PID_PWM()
{

PWM = ag.ANGLE_PWM – sp.SPEED_PWM;
if(PWM>=100){PWM = 100;}
if(PWM<=-100){PWM = -100;}
return PWM;
}

/******************************************************
电机PWM控制
******************************************************/
void moto(void)
{
dir = PID_PWM();
if(dir<=0){moto1=-dir;moto2=0;}
else{moto1=0;moto2=dir;}
ftm_pwm_duty(FTM0, FTM_CH3,moto1);
ftm_pwm_duty(FTM0, FTM_CH4,moto2);
}

/******************************************************
AD转换角度采集
******************************************************/
void ADC(void)
{

int32 sum = 0;
for(int i=0;i<20;i++)
{

angle_var = (adc_once(ADC0_SE11,ADC_8bit)*3300/(1<<8)-1);
sum += angle_var;
}
angle_var = sum/20;
sum = 0;
}

/******************************************************
定时中断函数
******************************************************/
void PIT0_IRQHandler(void)
{

//ADC采集角度传感器
ADC();

//取编码器脉冲值
//FTM1_A -> PTA12*****A相
//FTM1_B -> PTA13*****B相
Voltage = ftm_quad_get(FTM1);
sp.integral_Voltage+=Voltage;
val=(int16)(val*0.95+Voltage*6.5);//一阶低通滤波器
val=val-2;

//清 FTM 正交解码的脉冲数
ftm_quad_clean(FTM1);

//获取角度和速度
Angle_PD_Realize(banlace_angle,angle_var);
Speed_PID_Realize(0);

//获取PWM最终占空比值
PID_PWM();

//清中断标志位
PIT_Flag_Clear(PIT0);

}

//初始化所有模块
void Init_All(void)
{
/***************speed******************/
sp.Error_Now = 0;
sp.Error_Next = 0;
sp.Error_Last = 0;
sp.Speed_KP = 0.8;//0.8
sp.Speed_KI = 0.8;//0.4
sp.Speed_KD = 5;//5
sp.SPEED_PWM = 0;
sp.integral_Voltage=0;

/***************angle******************/
ag.Set_Angle = 0;
ag.Real_Angle = 0;
ag.Error_Now = 0;
ag.Error_Next = 0;
ag.Angle_KP = 0.65;
ag.Angle_KD = 4;
ag.ANGLE_PWM = 0;

//初始化FTM正交解码
ftm_quad_init(FTM1);

//初始化AD采集
adc_init(ADC0_SE11);

//初始化电机正转*****精度为100
ftm_pwm_init(FTM0,FTM_CH3,10*1000,0);//PTA6
ftm_pwm_init(FTM0,FTM_CH4,10*1000,0);//PTA7
// port_cfg.h 里配置 FTM0_CH3 对应为 PTA6
// 使能端输入为 0

//定时中断5msVoltage
pit_init_ms(PIT0,5);
set_vector_handler(PIT0_VECTORn,PIT0_IRQHandler);
enable_irq(PIT0_IRQn);
}

/*――――――――――――――――――――――――
――――――――――主函数――――――――――――
――――――――――――――――――――――――*/
void main()
{

int16 i;
int16 j;
Init_All();
key_init(KEY_A);

/**********************************************
起摆
**********************************************/
while(1)
{

if(key_check(KEY_A) == KEY_DOWN)
{

DELAY_MS(500);
break;
}
else
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
}
}
//左右来回摆动
for(j=0;j<2;j++)
{

for(i= 10;i<=40;i+=2)
{

ftm_pwm_duty(FTM0,FTM_CH3,i);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}
for(i= 10;i<=40;i+=2)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,i);
DELAY_MS(20);
}
}

//延时等待最佳时机反向
for(i= 0;i<=3;i++)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}

//瞬间反向起摆
for(i= 35;i<85;i+=5)
{

ftm_pwm_duty(FTM0,FTM_CH3,i);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}

//反向缓冲
for(i= 40;i<90;i+=10)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,i);
DELAY_MS(20);
}
/**********************************************
动态停机，稳定倒立
**********************************************/
while(1)
{

if(angle_var<=banlace_angle+350&&angle_var>=banlace_angle-350)
{

moto();
}
else
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
}
if(key_check(KEY_A) == KEY_DOWN)
{

DELAY_MS(500);
key_init(KEY_A);
break;
}
}
/**********************************************
圆周起摆
**********************************************/
while(1)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
if(key_check(KEY_A) == KEY_DOWN)
{

DELAY_MS(500);
break;
}
}
//左右来回摆动
for(j=0;j<2;j++)
{

for(i= 10;i<=40;i+=2)
{

ftm_pwm_duty(FTM0,FTM_CH3,i);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}
for(i= 10;i<=40;i+=2)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,i);
DELAY_MS(20);
}
}

//延时等待最佳时机反向
for(i= 0;i<=3;i++)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}

//瞬间反向起摆
for(i= 30;i<100;i+=5)
{

ftm_pwm_duty(FTM0,FTM_CH3,i);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}
/**********************************************
左右摇摆
**********************************************/
while(1)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
if(key_check(KEY_A) == KEY_DOWN)
{

DELAY_MS(500);
break;
}
}
//左右来回摆动
for(j=0;j<1;j++)
{

for(i= 10;i<=40;i+=2)
{

ftm_pwm_duty(FTM0,FTM_CH3,i);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}
for(i= 10;i<=40;i+=2)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,i);
DELAY_MS(20);
}
}
while(1)
{

for(j=0;j<2;j++)
{

for(i= 10;i<=20;i+=1)
{

ftm_pwm_duty(FTM0,FTM_CH3,i);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}
for(i= 0;i<=5;i++)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}
for(i= 10;i<=20;i+=1)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,i);
DELAY_MS(20);
}
for(i= 0;i<=5;i++)
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
DELAY_MS(20);
}
}
if(key_check(KEY_A) == KEY_DOWN)
{

DELAY_MS(1000);
Init_All();
break;
}
}
/**********************************************
倒立快速旋转
**********************************************/
while(1)
{

if(angle_var<=banlace_angle+350&&angle_var>=banlace_angle-350)
{

moto();
sp.integral_Voltage = 200;
}
else
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
}
if(key_check(KEY_A) == KEY_DOWN)
{

DELAY_MS(500);
Init_All();
break;
}
}
/**********************************************
动态停机，稳定倒立
**********************************************/
while(1)
{

if(key_check(KEY_A) == KEY_DOWN)
{

DELAY_MS(500);
Init_All();
}
if(angle_var<=banlace_angle+350&&angle_var>=banlace_angle-350)
{

moto();
}
else
{

ftm_pwm_duty(FTM0,FTM_CH3,0);
ftm_pwm_duty(FTM0,FTM_CH4,0);
}
}
}