必威体育Betway必威体育官网
当前位置：首页 > IT技术

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

时间:2019-06-19 03:44:14来源:IT技术作者:seo实验室小编阅读:74次「手机版」
　

旋转倒立摆

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

旋转倒立摆是控制组校内赛练手的题目，需要对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);

}

}

}

相关阅读

如何让Word2003中文本框任意旋转

许多时候我们在Word中制作流程图、公章等各种图形的时候都会在图形里面插入&ldquo;文本框&rdquo;，然后在里面输入文字。如果图形旋

AE怎么制作旋转万花筒特效?

以前很喜欢万花筒，旋转的万花筒效果是一个比较好玩的效果，想要使用AE中制作这个旋转的万花筒的效果动画，下面我们就来看看ae制作万花

旋转木马哪里有卖！旋转木马多少钱一台？

大型旋转木马游乐设备，是一款更新换代比较快的产品，现在转马设备已经两极分化为豪华转马和简易转马两类，跟飞椅一样，大型旋转木马也是

ipad air屏幕旋转失灵怎么办？

ipad air屏幕旋转失灵怎么办?小编带来了ipad air屏幕不旋转解决方法，有很多果粉在使用ipad air的过程中遇到了同样的问题，该如何解

ipad air怎么设置屏幕不旋转？

在ipad air中怎样设置才能让屏幕不旋转呢？教大家两个简单的方法，一起来看一下具体的操作步骤吧工具/原料ipad air方法一1在主页面下

赞()
分享到:

猜你喜欢

栏目导航

推荐阅读

热门阅读

seo实验室提供seo优化服务，有网站优化、网站建设、运营推广等需求请联系我们!
Copyright © 2001-2017 .seo实验室 版权所有 网站地图