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

大家好，又见面了，我是你们的朋友全栈君。如果您正在找激活码,请点击查看最新教程,关注关注公众号 “全栈程序员社区” 获取激活教程,可能之前旧版本教程已经失效.最新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);
      }
    }
}