以前也想过要写博客，但是却一直没有付诸于实践，作为第一篇原创，我还是选择将以前电赛时的作品拿出来，毕竟当初可是花费了好多心血的，汗~
旋转倒立摆是控制组校内赛练手的题目，需要对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);
}
}
}
