大家好，又见面了，我是你们的朋友全栈君。
以前也想过要写博客，但是却一直没有付诸于实践，作为第一篇原创，我还是选择将以前电赛时的作品拿出来，毕竟当初可是花费了好多心血的，汗~
旋转倒立摆是控制组校内赛练手的题目，需要对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); } } }
发布者：全栈程序员栈长，转载请注明出处：https://javaforall.cn/171009.html原文链接：https://javaforall.cn
