control.c

飞思卡尔智能汽车大赛的小车程序

 //************************
//    路径识别部分
//************************

//找到黑线位置，存入diff[SIZE_Y]中
//扫描式黑线查找法
void ScanLine(void){
  static signed char left,right;    //每一行的搜索范围，左起始和右结束
  static signed char i,j;
  static unsigned char start,end;   //记录黑线起始位置
  left=0;																									 
  right=59;
  ErrLineCnt=0;
  for(i=SIZE_Y-1;i>=0;i--){
    for(j=left;j<=right-2;j++) 	 //寻找黑线起始点
      if((VideoData[i][j]-VideoData[i][j+2]>Threshold)/*&&VideoData[i][j+2]<100*/){
        start=j+2; 
        break;
      }
    if(j<right-2) {
      for(j+=2;j<=right-2;j++){   //寻找黑线结束点
        if((VideoData[i][j+2]-VideoData[i][j]>Threshold)/*&&VideoData[i][j]<100*/){
          end=j;
          break;
        }
      }
      if((j<right-2)&&(end-start)<=6){  //计算黑线中心及初始化下次寻线参数
        diff[i]=(start+end)/2;
        left=diff[i]-15;  //下次寻线起始点、终止点		 15
        right=diff[i]+15;
        if(left<0)left=0;	 //调整下次寻线起始点、终止点
        if(right>59)right=59;
        diff[i]=diff[i]-29;	 //计算距离中心的偏移量
        continue;	 //转入下一行寻线
      }
    }
    diff[i]=126;  //未找到有效值
    
    ErrLineCnt++;
    left=0; //调整下次寻线起始点、终止点
    right=59;
  }
  if(ErrLineCnt>=10){	 //当无效行大于等于8时图象无效
    BadFrame=1;
  }
  else{
    BadFrame=0; 
  }
}

  

//根据黑线位置拟合出一条直线，得到斜率、截距、残差，用作路径趋势判断
void CalcuLine(void){
  static long mX,mY,mXY,mYY,tmp;
  unsigned char sample_cnt;
  unsigned char line_now;
  if(BadFrame)return;
  sample_cnt=0;
  mX=0;
  mY=0;
  mXY=0;
  mYY=0;
  tmp=0;
  for(line_now=0;line_now<SIZE_Y;line_now++){
    if(diff[line_now]<100){
      sample_cnt++;
      mX+=diff[line_now];
      mY+=line_now;
      mXY+=line_now*diff[line_now];
      mYY+=line_now*line_now;
    }
  }
    //若采样点数不超过4点，则直接跳出，采用上次计算结果
  if(sample_cnt>6){
    //a_last=a;
    a=(sample_cnt*mXY-mX*mY)*10/(sample_cnt*mYY-mY*mY);
    b=(10*mX-a*mY)/sample_cnt;
    Ee2=0;
    for(line_now=0;line_now<SIZE_Y;line_now++){
      if(diff[line_now]<100){
        tmp=a*line_now+b-10*diff[line_now];
        Ee2+=tmp*tmp;
      }
    }
  }
  else{
    return;
  }
}


//***********************
//    小车控制部分
//***********************

//控制参数初始化
void init_Control(void){
  diff_sum=0;
  diff_last=0;
  I1=10;
  D1=20;
  P1_en=1;
  I1_en=0;
  D1_en=1;
  
  a_last=0;
  Pa_en=1;
  Da_en=1;
  Da=-20;
  
  if(PTT_PTT2==0&&PTT_PTT3==0){
  Speed_z=130;			 //120
  slow_en=1;
  }
  else if(PTT_PTT2==0&&PTT_PTT3==1){
  Speed_z=140;			 //130
  middle_en=1; 
  }
  else if(PTT_PTT2==1&&PTT_PTT3==0){
  Speed_z=150;			//140
  fast_en=1;
  }
  else {
  Speed_z=160;			//150
  fast_en=1;
  }
  
  if(PTT_PTT4==0&&PTT_PTT5==0)Speed_w=85; //65
  else if(PTT_PTT4==0&&PTT_PTT5==1)Speed_w=95;  //75
  
  else if(PTT_PTT4==1&&PTT_PTT5==0)Speed_w=105;  //85
  else Speed_w=115;
  
  if(PTT_PTT6==0)Speed_bad=85; 		 //75
  else Speed_bad=95;				//85
  
}										//	PTM_PTM0
  


                     
//前轮舵机控制
void ServoControl(void)
{
  //根据当前车速选择基准行作为偏差
   
   if(PTT_PTT7==0){				 //slow
    line=8;
    while(diff[line]>100&&line<12&&line>=0)line--;
  } 
   
   else {    					//Fast
    if(Speed_cnt>125){
    line=1;
    while(diff[line]>100&&line<12&&line>=0)line++;
  } 
  
  else if(Speed_cnt>90){
    line=5;
    while(diff[line]>100&&line<12&&line>=0)line++;
  }
  else{
    line=10;
    while(diff[line]>100&&line<12&&line>=0)line--;
  }
   }


   
 
 
  
  
  //坏帧则保持上次控制结果，使用上次输出判断，舵机转角最大化
  if(BadFrame==1)
  {
    
  if(BadFrameCnt>=10)				//8
    {
    LED=0;   //坏帧蓝灯亮
    
		PID_out=2*PID_out1;		//2
   
    if(PID_out>SERVO_R)
    {
      PID_out=SERVO_R;
      
    }
    else if(PID_out<SERVO_L)
    { 
      PID_out=SERVO_L;
    } 
    }
    
    else
   {
    LED=0;   //坏帧蓝灯亮
    if(Speed_cnt>130) 
    {
    PID_out=2*PID_out1;
    Bad_jia=0;
    Bad_jian=0;
    } 
    else 
    {
    Bad_jia+=50;
    Bad_jian-=50;
    }
    
    if(PID_out1>0){
      PID_out=PID_out1+Bad_jia;
    } 
    else if(PID_out1<0){
      PID_out=PID_out1+Bad_jian;
    }
    
    if(PID_out>SERVO_R)
    {
      PID_out=SERVO_R;
    }
    else if(PID_out<SERVO_L)
    { 
      PID_out=SERVO_L; 
    } 
  }
  
   
    																			
		servo_out=(4*PID_out+2*PID_out1+PID_out2)/7;		//4 2 1
	 
		PWMDTY45=SERVO_MID+servo_out;  //输出平滑
	
		PID_out3=PID_out2;
    PID_out2=PID_out1;
    PID_out1=PID_out;
											 
  }
  else{
    LED=1;    //好帧蓝灯灭
  
  
  
  /*××××××××××××××××××××××××
  单行偏差的PID控制
  ××××××××××××××××××××××××*/
  
  //分段P参数,在小误差应用积分环节，避免积分饱和
 
   
 /* if(diff[line]>=-3&&diff[line]<=3){
    P1_out=0;
    I1_en=0;
  }
  else if(diff[line]>3&&diff[line]<=8){
    P1_out=20*(diff[line]-3);
    I1_en=0;
  }
  else if(diff[line]<-3&&diff[line]>=-8){
    P1_out=20*(diff[line]+3);
    I1_en=0;
  }
   else if(diff[line]>8&&diff[line]<=20){
    P1_out=30*diff[line]-120;
    I1_en=0;
  }
  else if(diff[line]<-8&&diff[line]>=-20){
    P1_out=30*diff[line]+120;
    I1_en=0;
  }	 
 else if(diff[line]>20){
    P1_out=40*diff[line]-300;
    I1_en=0;
  }
  else if(diff[line]<-20){
    P1_out=40*diff[line]+300;
    I1_en=0;
  }	 		 */
  
  
  if(diff[line]>=-2&&diff[line]<=2){
    P1_out=0;
    I1_en=0;
  }
  else if(diff[line]>2&&diff[line]<=8){
    P1_out=25*(diff[line]-2);
    I1_en=0;
  }
  else if(diff[line]<-2&&diff[line]>=-8){
    P1_out=25*(diff[line]+2);
    I1_en=0;
  }
   else if(diff[line]>8){
    P1_out=35*diff[line]-180;
    I1_en=0;
  }
  else if(diff[line]<-8){
    P1_out=35*diff[line]+180;
    I1_en=0;
  }	 
 
  
  if(P1_out>SERVO_R){
    P1_out=SERVO_R;
  }
  else if(P1_out<SERVO_L){
    P1_out=SERVO_L;
  }
  
  //如果有积分环节，计算diff_sum,设置累计误差的范围
  if(I1_en){
    diff_sum+=diff[line];
    if(diff_sum>15)diff_sum=15;
    else if(diff_sum<-15)diff_sum=-15;
  }
  
  //PID算式
  PID1_out=P1_out+(diff[line]-diff_last)*D1*D1_en;
 
 
  /*******************
      斜率控制部分
  ********************/
  
  if(a<12&&a>-12){
    Pa_out=0;
  }
  
  else if(a>=12){
    Pa_out=(-40)*(a-12);
  }
  else if(a<=-12){
    Pa_out=(-40)*(a+12);
  }
  
  PIDa_out=Pa_out*Pa_en+Da_en*Da*(a-a_last);
  if(PIDa_out>SERVO_R){
    PIDa_out>SERVO_R;
  }
  else if(PIDa_out<SERVO_L){
    PIDa_out=SERVO_L;
  }
  
  /***********
    PID输出
  ************/
   
  PID_out=(PID1_out*7+PIDa_out)/8;		//7 1
  //PID_out=PID1_out;
  //PID输出高低限，与舵机转角极限对应
   
  if(PID_out>SERVO_R){
    PID_out=SERVO_R;
  }
  else if(PID_out<SERVO_L){
    PID_out=SERVO_L;
  }


  //PWM输出控制舵机，最终输出值为前4次计算结果的综合，平滑输出
  servo_out=(PID_out*4+PID_out1*2+PID_out2+PID_out3)/8;
  //servo_out=PID_out;
  PWMDTY45=SERVO_MID+servo_out;
  

  diff_last=diff[line];

  PID_out3=PID_out2;
  PID_out2=PID_out1;
  PID_out1=PID_out;

  }
}

/*速度控制函数，本函数在100ms中断中调用，闭环控制车速*/	
/*给定车速计算，当前采用定值控制，遇见以下几种情况限速：
  1.Ee2>450,前方弯道
  2.BadFrame==1,图像采集错误
  3.b>45,最小二乘法拟合直线，预测前方将有较大偏差，小车即将冲出跑道
  4.diff[line]>25，当前误差偏大
  (Ee2>450||BadFrame==1||b>45||diff[line]>25)
  今后改进方向：
  1.给定车速根据前方赛道情况动态计算，大弯低速,小弯和连续弯道较高速通过
  2.弯道判断算法改进
*/ 
void SpeedControl(void)
{
  signed int tmp_speed;
  Speed_cnt=PACN1;
  PACN1=0;
  
  if(BadFrame)
  {
  BadFrameCnt++;
  }
  else{
  BadFrameCnt=0;
  Bad_jia=0;
  Bad_jian=0;
  }
  
  //坏帧
  if(BadFrame)
  {
     //LED=0;
     Speed_set=(Speed_setlast+2*Speed_bad)/3;
     if(Speed_set<Speed_bad)Speed_set=Speed_bad;
  }
  //直道
  else if(a>-13&&a<13&&Ee2<310){
    
    Speed_set=Speed_z;
  }
  //弯道		 加入大小弯判断
  else if(Ee2>350&&(a>18||a<-18))			//Ee2>350&&(a>18||a<-18)
  {
     
    Speed_set=Speed_w;
  }
  //s弯道		 
  else if(Ee2>500&&(a>-18&&a<18))						//Ee2>500&&(a>-18&&a<18)
  {
    
     Speed_set=Speed_z+10;	 
   
  }
  //其他
  else if(slow_en==1){
     
    Speed_set=90;			//90
  }
  else if(middle_en==1){
     
    Speed_set=100;		 //100
  }
  else if(fast_en==1)
  {
     
    Speed_set=110;	 //110
  }
  
  //坏帧累计
  if(BadFrameCnt>10){
    Speed_set=Speed_bad;
  } 

  


  //速度偏差计算
  Speed_d_last=Speed_d;
  Speed_d=Speed_set-Speed_cnt;
  Speed_setlast=Speed_set;

  
  if(Speed_d>=4){
    PWMPOL=0x2C;
    PWMDTY2=0xFC;
  }
  else if(Speed_d<-5){
    PWMPOL=0x20;
    if(Speed_cnt>120){
      PWMDTY2=0xa0;		//a0
    }
    else if(Speed_cnt>100){
      PWMDTY2=0x80;				//80
    }
    else{
      PWMDTY2=0x55;				 //55
    }
  }  
  else if(Speed_d<=-3){
    PWMPOL=0x2C;
    PWMDTY2=0x20;
  }
  else if(Speed_d<4&&Speed_d>-3){
    PWMPOL=0x2C;
    tmp_speed=0xa0+Speed_d*8+(Speed_d-Speed_d_last)*4;
    if(tmp_speed<0){
      tmp_speed=0;
    }
    else if(tmp_speed>254){
      tmp_speed=254;
    }
    PWMDTY2=tmp_speed;
  }


  
    
  /*if(PTT_PTT7&&DebugEn){
    sci_putchar(0xFE);     //起始位
    sci_putchar(Speed_cnt);     //速度
    //sci_putchar(diff[line]+30);			//偏差
    //sci_putchar((1000+servo_out)/10);    //舵机转角
    sci_putchar(Speed_set); //设定速度+BadFrame
    sci_putchar(PWMDTY2);
    sci_putchar(0xEF);    //结束位
  }		 */
  
}