程序控制

chenggongmjh

附表1
#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
#include "Sheetdef.h"  // 首先从原文件的位置开始搜索指定文件


//此部分应当为参数设定
//int16 speed[5] = {15,25,40,48,58};
int16 speed[5] = {10,20,36,42,48};//3月28日——15秒
int16 angle_k[5] = {11,12,13,14,15 };
int16 angle_kd[5][5] = { {15,11,11,11,15},
                         {11, 9, 0, 9,11},
                         {4,  4, 0, 4, 4},
                         {11, 9, 0, 9,11},
                         {15,11,11,11,15} } ;
int16 DATA1=0,DATA2=0,DATA3=0,DATA4=0,DATA5=0,DATA6=0;
int16 DATA_S[6];
int16 DATA_A,DATA_B;
int A,B,L_X;
int16 L_POS,L_POS_G,L_POS_G1,L_POS_DELTA;
int16 err1=0;
int16 err2=0;
int16 temp=0;
int16 rud_lasterr=0;
int16 rud_curerr=0;
float mot_kp=1.2;
//float mot_kp=0.5;       //比例环节
float mot_kd=0.3;        //微分环节
//float mot_ki=0.05;       //积分环节
int16 motor_pwm=30;   //电机目标pwm值
int16 rudder_pwm=855;
int16 angle_mid=853;
int16 angle_min=725;
int16 angle_max=985;      //985
uint16 speednow=0;
int16 speedflg=0;       //
int16 speed_err1=0,speed_err2=0,speed_err3=0;
int16 Kp2;//角度的P变量
int16 Kd2;//角度的D变量
uint32 K=0;//模糊控制加乘在速度
int16 KK;//?
int16 err_a1;//
int16 err_a;//?
int16 kd2_kk;//?
int16 startline_flg=0;       //起跑线标志变量
int16 startline_delay=0;   //起跑线延迟
int16 pinh=0;                  //PTH电平值


int16 PV[5] = {0,0,0,0,0}; //PV为psoold 的分量
int16 PE1[5] = {0,0,0,0,0}; //err11的分量
int16 PE2[5] = {0,0,0,0,0};//


//此部分可以作为函数声明部分
void makedecision(void);
void path_recog(void);
void systemboot(void);
void pwm_init(void);
void rti_init(void);
void adc_init(void);
void ctrl(void);
void startline (void);

//此部分可以作为程序初始化的文件模式
void systemboot(void)
{
        CRGFLG_SCM=0;    // 使用外部时钟
        SYNR = 7;        //PLLCLK = 2 x OSCCLK x [SYNR + 1] / [REFDV + 1]
        REFDV = 3;
        PLLCTL = 0x71;   //打开锁相环电路
        while(CRGFLG_LOCK==0); //确保系统的频率可以工作
        CLKSEL = 0x80;   //PLLSEL=1,
        //源自晶振
       
        DDRA = 0x00;     
        PORTA=0x00;      //clean PortA
        DDRB = 0xFF;     //
        PORTB=0xFF;      //clean PortB
        DDRS = 0x00;     //PortS 为输入
        PTS=0x00;      //clean PortS
        DDRM = 0xc0;
        PTM = 0x00;
        DDRH = 0x00;
        PTH = 0x00;
        DDRK = 0x00;     //PortK为输入
        PORTK=0x00;
         
        pwm_init();
        rti_init();
        adc_init();
        rudder_pwm = 3*angle_mid;//初始舵机正中
}

void pwm_init(void)//PWM初始化
{

        PWMPOL = 0xFF;   //极性设置，一开始为高
        PWMCLK = 0x2A;   //每一个PWM可以选择两个，为SA/SB或者A/B,此处选前者;
        PWMPRCLK = 0x11; //设置频率A=bus clock/2=16MHz,B=bus clock/2=16MHz;
        PWMCAE = 0x00;   //左对齐方式
        PWMSCLA = 0x01;  //SA方式的算法：SA=A/2WM clock=bus clock/2/2/1=8MHz
        PWMSCLB = 0x05;  //SB方式的算法：SB=B/1;PWM clock=bus clock/2/2/5=1.6MHz
        PWMCTL = 0xF0;   //级联各个通道，成为16位PWM;
        PWMPER45 = 2000;  //输出频率的算法：SA clock/2000=4KHz;
        PWMDTY45 = 1000;  //初始时刻占空比;
        PWMPER23 = 32000; //23通道的输出频率：SB clock/16000=100Hz;
        PWMDTY23 = 3*angle_mid;  //初始时刻占空比；
        PWME=0x2A;        //打开01，23，45通道（只需开通高位即可?
}



void rti_init(void)    //1ms
{
  RTICTL=0X8f;   //1ms中断一次
  CRGFLG_RTIF=1;  //清中断标志位,RTI time-out has occurred.
  PACTL_PAEN=1;//1 Pulse Accumulator system enabled.
  PACTL_PAMOD=0;//0 Event counter mode.
  PACTL_PEDGE=1;//1 Rising edges on IOC7 pin cause the count to be incremented.
  CRGINT_RTIE=1;  //Interrupt will be requested whenever RTIF is set.
}

void adc_init(void)
{

  ATD0CTL1=0XD0;
  ATD0CTL2=0X40;
  ATD0CTL3=0XB0;
  ATD0CTL4=0X01;
  ATD0CTL5=0X30;
  
}



//此部分为中断地址，要设置中断向量地址
#pragma CODE_SEG __NEAR_SEG NON_BANKED

interrupt void RTI(void)
{

   DisableInterrupts;
   CRGFLG_RTIF=1;
    speedflg++;
    if(speedflg>10)
     {
        speednow=PACNT;
        PACNT=0;
        speedflg=0;
     }
    startline ();
    makedecision();//根据传感器做决策
    if((DATA1<1400)&&(DATA2<1400)&&(DATA3<1400)&&(DATA4<1400)&&(DATA5<1400)&&(DATA6<1400));
    else
          ctrl();
          EnableInterrupts;
}


#pragma CODE_SEG DEFAULT


//识别传感器信息
void path_recog(void)
{
   //write your code here
   int i,MAX_NUM,SEC_NUM;
   int16 DATA_MAX,DATA_SEC;
   ATD0CTL5=0X30;  //重启AD
   while(!ATD0STAT0_SCF);
  DATA1=ATD0DR0;
  DATA2=ATD0DR1;
  DATA3=ATD0DR2;
  DATA4=ATD0DR3;
  DATA5=ATD0DR4;
  DATA6=ATD0DR5;
  DATA_S[0]=DATA1;
  DATA_S[1]=DATA2;
  DATA_S[2]=DATA3;
  DATA_S[3]=DATA4;
  DATA_S[4]=DATA5;
  DATA_S[5]=DATA6;   
   DATA_MAX=DATA1;MAX_NUM=0;
   DATA_SEC=DATA2;SEC_NUM=1;
    for(i=1;i<=5;i++)
     if(DATA_MAX<DATA_S)
        {
         DATA_MAX=DATA_S;
         MAX_NUM=i;
         }
                  /*ZHAO ZUI DA ZHI*/
    if(MAX_NUM==0)
        {
         DATA_SEC=DATA_S[1];
         SEC_NUM=1;
        }
    else if(MAX_NUM==5)
        {
         DATA_SEC=DATA_S[4];
         SEC_NUM=4;
        }
    else
        {
         if(DATA_S[MAX_NUM-1]<DATA_S[MAX_NUM+1]) {
          DATA_SEC=DATA_S[MAX_NUM+1];SEC_NUM=MAX_NUM+1;
         }
         else{
          DATA_SEC=DATA_S[MAX_NUM-1];SEC_NUM=MAX_NUM-1;
         }
        }
        /*ZHAO CI DA ZHI*/
        
    DATA_A=DATA_MAX;A=MAX_NUM;
    DATA_B=DATA_SEC;B=SEC_NUM;     
    if(A>B) L_X=B;
    else L_X=A;
   
     
     if(A>B)
     L_POS_DELTA=(DATA_A-DATA_B+2200)*5/44;
     else
     L_POS_DELTA=(2200-DATA_A+DATA_B)*5/44;
    L_POS=L_X*500+L_POS_DELTA;
}
void makedecision(void)
{
    int16 POS;
           int16 err11;
          L_POS_G1 = L_POS_G;
                L_POS_G = (L_POS_G * 5 + L_POS)/6;
                err1 = (err1*5 + 10*(L_POS_G - L_POS_G1))/6;
    err11=-err1;
                if(L_POS <= 500) POS = 1; //NB
                else if(L_POS > 500  && L_POS <= 1000)  POS = 2; //NS
                else if(L_POS >= 1000 && L_POS < 1500)  POS = 3; //ZO
                else if(L_POS >= 1500 && L_POS <= 2000)  POS = 4; //PS
                else if(L_POS >= 2000) POS = 5; //PB
               
                        /*******************模糊化******************/
        if((L_POS <= 750) && (L_POS >= 250))
                {
                  PV[0] = (7500 - L_POS * 10) / 500;
                  PV[1] = (L_POS - 250) * 10/500;
                }
                else if((L_POS >= 750)  && (L_POS <= 1250))
                {
                  PV[1] = (1250 - L_POS ) * 10/500;
                  PV[2] = (10 * L_POS - 7500) /500;
                }
                else if((L_POS >= 1250) && (L_POS <= 1750))
                {
                  PV[2] = (17500 - 10 * L_POS)  / 500;
                  PV[3] = (L_POS - 1250) * 10 / 500;
                }
                else if(L_POS <= 2250 && L_POS >= 1750)
                {
                  PV[3] = (2250 - L_POS) * 10 / 500;
                  PV[4] = (L_POS * 10- 17500) / 500;
                }
                else if(L_POS < 250) {
                  PV[0] = 10;
                  PV[1] = 0;
                  PV[2] = 0;
                  PV[3] = 0;
                  PV[4] = 0;
                }
                else {
                  PV[0] = 0;
                  PV[1] = 0;
                  PV[2] = 0;
                  PV[3] = 0;
                  PV[4] = 10;
                  
                }
               
                if(err11<(-40)) PE1[0] = 10;
                else if((err11>=(-40))&&(err11<(-20))) PE1[0] = 10*(err11+20)/(-20);
                else PE1[0] = 0;
               
                if(err11<(-40)) PE1[1] = 0;
                else if((err11>=(-40))&&(err11<=(-20))) PE1[1] = 10*(err11+40)/20;
                else if((err11>(-20))&&(err11<0)) PE1[1] = 10*(-err11)/20;
                else PE1[1] = 0;
               
                if(err11<(-20)) PE1[2] = 0;
                else if((err11>=(-20))&&(err11<=0)) PE1[2] = 10*(err11+20)/20;
                else if((err11>0)&&(err11<=20)) PE1[2] = 10*(err11-20)/(-20);
                else PE1[2] = 0;
               
                if(err11<0) PE1[3] = 0;
                else if((err11>=0)&&(err11<=20)) PE1[3] = 10*err11/20;
                else if((err11>20)&&(err11<=40)) PE1[3] =10* (err11-40)/(-20);
                else PE1[3] = 0;
               
                if(err11<20) PE1[4] = 0;
                else if((err11>=20)&&(err11<=40)) PE1[4] = 10*(err11-20)/20;
                else PE1[4] = 10;
               
                /*************转角的模糊控制****************/
                  K = 0;
                Kd2 = 0;
   
    K = PV[0] * PE1[0] * angle_k[3];
    K += PV[0] * PE1[1] * angle_k[3];
    K += PV[0] * PE1[2] * angle_k[4];
    K += PV[0] * PE1[3] * angle_k[4];
    K += PV[0] * PE1[4] * angle_k[4];
    K += PV[1] * PE1[0] * angle_k[1];
    K += PV[1] * PE1[1] * angle_k[2];
    K += PV[1] * PE1[2] * angle_k[2];
    K += PV[1] * PE1[3] * angle_k[3];
    K += PV[1] * PE1[4] * angle_k[4];
    K += PV[2] * PE1[0] * angle_k[2];
    K += PV[2] * PE1[1] * angle_k[2];
    K += PV[2] * PE1[2] * angle_k[0];
    K += PV[2] * PE1[3] * angle_k[2];
    K += PV[2] * PE1[4] * angle_k[2];
    K += PV[3] * PE1[0] * angle_k[4];
    K += PV[3] * PE1[1] * angle_k[3];
    K += PV[3] * PE1[2] * angle_k[2];
    K += PV[3] * PE1[3] * angle_k[2];
    K += PV[3] * PE1[4] * angle_k[1];
    K += PV[4] * PE1[0] * angle_k[4];
    K += PV[4] * PE1[1] * angle_k[4];
    K += PV[4] * PE1[2] * angle_k[4];
    K += PV[4] * PE1[3] * angle_k[3];
    K += PV[4] * PE1[4] * angle_k[3];   
   
    Kd2 = PV[0] * PE1[0] * angle_kd[0][0] ;
    Kd2 += PV[0] * PE1[1] * angle_kd[0][1];
    Kd2 += PV[0] * PE1[2] * angle_kd[0][2];
    Kd2 += PV[0] * PE1[3] * angle_kd[0][3];
    Kd2 += PV[0] * PE1[4] * angle_kd[0][4];
    Kd2 += PV[1] * PE1[0] * angle_kd[1][0];
    Kd2 += PV[1] * PE1[1] * angle_kd[1][1];
    Kd2 += PV[1] * PE1[2] * angle_kd[1][2];
    Kd2 += PV[1] * PE1[3] * angle_kd[1][3];
    Kd2 += PV[1] * PE1[4] * angle_kd[1][4];
    Kd2 += PV[2] * PE1[0] * angle_kd[2][0];
    Kd2 += PV[2] * PE1[1] * angle_kd[2][1];
    Kd2 += PV[2] * PE1[2] * angle_kd[2][2];
    Kd2 += PV[2] * PE1[3] * angle_kd[2][3];
    Kd2 += PV[2] * PE1[4] * angle_kd[2][4];
    Kd2 += PV[3] * PE1[0] * angle_kd[3][0];
    Kd2 += PV[3] * PE1[1] * angle_kd[3][1];
    Kd2 += PV[3] * PE1[2] * angle_kd[3][2];
    Kd2 += PV[3] * PE1[3] * angle_kd[3][3];
    Kd2 += PV[3] * PE1[4] * angle_kd[3][4];
    Kd2 += PV[4] * PE1[0] * angle_kd[4][0];
    Kd2 += PV[4] * PE1[1] * angle_kd[4][1];
    Kd2 += PV[4] * PE1[2] * angle_kd[4][2];
    Kd2 += PV[4] * PE1[3] * angle_kd[4][3];
    Kd2 += PV[4] * PE1[4] * angle_kd[4][4];
   
  motor_pwm = PV[0] * PE1[0] * speed[1] ;
  motor_pwm += PV[0] * PE1[1] * speed[1];
  motor_pwm += PV[0] * PE1[2] * speed[2];
  motor_pwm += PV[0] * PE1[3] * speed[0];
  motor_pwm += PV[0] * PE1[4] * speed[0];
  motor_pwm += PV[1] * PE1[0] * speed[2];
  motor_pwm += PV[1] * PE1[1] * speed[3];
  motor_pwm += PV[1] * PE1[2] * speed[3];
  motor_pwm += PV[1] * PE1[3] * speed[2];
  motor_pwm += PV[1] * PE1[4] * speed[1];
  motor_pwm += PV[2] * PE1[0] * speed[2];
  motor_pwm += PV[2] * PE1[1] * speed[3];
  motor_pwm += PV[2] * PE1[2] * speed[4];
  motor_pwm += PV[2] * PE1[3] * speed[3];
  motor_pwm += PV[2] * PE1[4] * speed[2];
  motor_pwm += PV[3] * PE1[0] * speed[1];
  motor_pwm += PV[3] * PE1[1] * speed[2];
  motor_pwm += PV[3] * PE1[2] * speed[3];
  motor_pwm += PV[3] * PE1[3] * speed[3];
  motor_pwm += PV[3] * PE1[4] * speed[2];
  motor_pwm += PV[4] * PE1[0] * speed[0];
  motor_pwm += PV[4] * PE1[1] * speed[0];
  motor_pwm += PV[4] * PE1[2] * speed[2];
  motor_pwm += PV[4] * PE1[3] * speed[1];
  motor_pwm += PV[4] * PE1[4] * speed[1];
  
  KK = K/100;
  Kd2 = Kd2/100;
  motor_pwm = motor_pwm/100;
  
  Kp2 = (angle_max - angle_min) *0.4/100;
  err1 = err1/100;
  if(L_POS>=2250)
    rudder_pwm = angle_max ;
  else if(L_POS<=250)
    rudder_pwm = angle_min;
  else{
  err_a1 = err_a;                                   //角度的误差
        err_a = L_POS-1250;
        rudder_pwm = angle_mid + Kp2 * err_a * KK / 120 + Kd2 * err1 * KK*30;
        //rudder_pwm = angle_mid + Kp2 * err_a * KK / 120 + Kd2 * err1 * KK/5; 3月28日——15秒  
        if(rudder_pwm > angle_max)rudder_pwm = angle_max;
        if(rudder_pwm < angle_min)rudder_pwm = angle_min;
  }
        }

//#pragma CODE_SEG DEFAULT
//电机控制
void ctrl(void)
{

  speed_err3 = speed_err2;
  speed_err2 = speed_err1;
  speed_err1 = speednow-motor_pwm;
  temp = PWMDTY45 + (int16)(mot_kp * (float)speed_err1 + mot_kd * (float)(speed_err2-speed_err1));   
  if(temp>1000) PWMDTY45 = 1000;
  else
    if(temp<500) PWMDTY45 = 500;
  else PWMDTY45 =temp;
  if((rudder_pwm >= angle_min)&&(rudder_pwm <= angle_max)) PWMDTY23 = rudder_pwm;

}
//起跑线检测程序
void startline (void)
{  int i;
    if(startline_delay==0)
    { pinh=PTH;
     If  (pinh&0x01)
        { for(i=0;i<100;i++) ;
         pinh=PTH;
         if (pinh&0x01)
           { startline_flg++;
            startline_delay++;
           }
         }
    else
     { startline_delay++;
      if(startline_delay>400)startline_delay=0;
     }
    }
   }//主函数
void main(void)
{
  int i,j;
        DisableInterrupts;//关闭中断函数
        systemboot();//初始化系统
        for(i=0;i<4000;i++)
                  for(j=0;j<2000;j++);
        asm                                 
        {
                nop;
     
        }
       
        EnableInterrupts;//开启中断函数


        for(;;)
        {//主循环 读取传感器信息
         path_recog();
         if(startline_flg>=3)  
         {
                  DisableInterrupts;//关闭中断函数
            PWMDTY45 = 1000;  //初始时刻占空比;
            for(;;);
         }
  
   }

   
        }
谁可以帮我看一下这个程序里面 PV、PE是什么，后面的  K += PV[2] * PE1[3] * angle_k[2] /Kd2 += PV[3] * PE1[2] * angle_kd[3][2];等这类计算是什么啊，我快疯了，这是模糊控制里面的什么东西。。。急啊。。。

chenggongmjh

我很急啊，毕业设计的题目，程序读到那不懂了6月1号就要交了，大家帮帮忙啊，跪求啊。。。。