智能车制作

标题: 程序控制 [打印本页]

作者: chenggongmjh 时间: 2011-5-27 18:58
标题: 程序控制
附表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/2

WM 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 时间: 2011-5-27 19:07
我很急啊，毕业设计的题目，程序读到那不懂了6月1号就要交了，大家帮帮忙啊，跪求啊。。。。

欢迎光临智能车制作 (http://111.231.132.190/) Powered by Discuz! X3.2