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;  //下次寻线起始点、终止点
        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>=9){	 //当无效行大于等于10时图象无效
    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=(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=30;
  P1_en=1;
  I1_en=0;
  D1_en=1;
  
  a_last=0;
  Pa_en=1;
  Da_en=1;
  Da=-20;
  
  PID_out=0;
  PID1_out=0;
  PIDa_out=0;
  servo_out=0;
}
                     
//前轮舵机控制
void ServoControl(void){
  //根据第2组拨码开关第3位确定基准行位置
  //0:低速看近
  //1:高速看远

  if(PTM_PTM2==1){
    if(Speed_cnt>140){
      line=0;
      while(diff[line]>100&&line<12&&line>=0)line++;
    }
    else if(Speed_cnt>100){
      line=4;
      while(diff[line]>100&&line<12&&line>=0)line++;
    }
    else{
      line=7;
      while(diff[line]>100&&line<12&&line>=0)line++;     
    }
  }
  //弯道版
  else if(PTM_PTM2==0){
    line=4;
    while(diff[line]>100&&line<12&&line>=0)line++;
  }
  
  
  //再次判断图像有效性
  if(line>=0&&line<12){
  }
  else{
    BadFrame=1;
  }
 
  
  //坏帧则保持上次控制结果，使用上次输出判断，舵机转角最大化
  if(BadFrame==1){
    LED=0;   //坏帧蓝灯亮
    BadFrameCnt++;
    
    if(BadFrameCnt>10||Speed_cnt>100){
      PID_out=PID_out*2;
    }
    else{
      if(PID_out>0){
        PID_out+=35;
      }
      else if(PID_out<0){
        PID_out-=35;
      }
    }

    
    //PID_out=PID_out1*5/2;

    
    if(PID_out>SERVO_R){
      PID_out=SERVO_R;
    }
    else if(PID_out<SERVO_L){ 
      PID_out=SERVO_L; 
    }
    
    //太切内道矫正,舵机极限位置减小
    if(PTM_PTM3==1&&BadFrameCnt>40){  
      if(PID_out>0){
        PID_out-=200;
      }
      else{
        PID_out+=200;
      }
    }
        
    
		servo_out=PID_out;
		PWMDTY45=SERVO_MID+servo_out;  //输出平滑
	
		PID_out3=PID_out2;
    PID_out2=PID_out1;
    PID_out1=PID_out;
		
    return;									 
  }
  else{
    LED=1;    //好帧蓝灯亮
    BadFrameCnt=0;
  }
  
  /*××××××××××××××××××××××××
  单行偏差的PID控制
  ××××××××××××××××××××××××*/
  //分段P参数,在小误差应用积分环节，避免积分饱和
  
  //小调节量
  if(diff[line]>=-2&&diff[line]<=2){
    P1_out=0;
    I1_en=0;
  }
  else if(diff[line]>2&&diff[line]<=8){
    P1_out=23*(diff[line]-2);
    I1_en=0;
  }
  else if(diff[line]<-2&&diff[line]>=-8){
    P1_out=23*(diff[line]+2);
    I1_en=0;
  }
  else if(diff[line]>8){
    P1_out=42*diff[line]-198;
    I1_en=0;
  }
  else if(diff[line]<-8){
    P1_out=42*diff[line]+198;
    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=30*(diff[line]-2);
    I1_en=0;
  }
  else if(diff[line]<-2&&diff[line]>=-8){
    P1_out=30*(diff[line]+2);
    I1_en=0;
  }
  else if(diff[line]>8){
    P1_out=55*diff[line]-260;
    I1_en=0;
  }
  else if(diff[line]<-8){
    P1_out=55*diff[line]+260;
    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_sum*I1*I1_en+(diff[line]-diff_last)*D1*D1_en;
 
 
  /*******************
      斜率控制部分
  ********************/
  
  if(a<13&&a>-13){
    Pa_out=0;
  }
  else if(a>=13){
    Pa_out=(-80)*(a-13);
  }
  else if(a<=-13){
    Pa_out=(-80)*(a+13);
  }
  
  PIDa_out=Pa_out*Pa_en;//+(a-a_last)*Da*Da_en;
  if(PIDa_out>SERVO_R){
    PIDa_out>SERVO_R;
  }
  else if(PIDa_out<SERVO_L){
    PIDa_out=SERVO_L;
  }
  
  /***********
    PID输出
  ************/
  //根据第2组拨码开关第5位决定是否加入斜率控制
  PID_out=(PID1_out*7+PIDa_out*1)/8;
  /*
  if(Ee2>1000&&(a>-12&&a<12)){
      PID_out=(PID1_out*3+PIDa_out)/4;
  }
  else{
      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*1+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;
}


//速度控制函数，闭环控制车速

void SpeedControl(void){
  signed int tmp_speed;
  
  //get Speed_cnt  
  Speed_cnt=PACN1;
  PACN1=0;
  
  
  

  
  //看不见情况   
  if(BadFrame){
    if(PTM_PTM4==1&&BadFrameCnt>60){
      Speed_set=Speed_bad-10;
    }
    else if(BadFrameCnt>=10){
      Speed_set=Speed_bad;
    }
    else{
      Speed_set=(Speed_set*3+Speed_bad)/4;
    }
  }
  //直道
  else if((a>-12&&a<12)&&Ee2<310){
    Speed_set=Speed_zhi;
  }
  //弯道
  else if(Ee2>500&&(a>22||a<-22)){
    Speed_set=Speed_wan;
  }    
  //S弯
  else if(Ee2>1000&&(a>-12&&a<12)){
    Speed_set=Speed_s;
  }
  //其他
  else{
    Speed_set=Speed_zhi-10;
  }
  /*
  if(BadFrameCnt>30){
    Speed_set=Speed_bad-5;
  }
  */
  //速度偏差计算
  Speed_d_last=Speed_d;
  Speed_d=Speed_set-Speed_cnt;
  

  
  if(Speed_d>=6){
    PWMPOL=0x2C;
    PWMDTY2=0xFC;
  }
  else if(Speed_d<-10){
    PWMPOL=0x20;
    /*
    if(Speed_cnt>110){
      PWMDTY2=0xA8;
    }
    else{
      PWMDTY2=0x90;
    }
    */
    PWMDTY2=0x90;
  }  
  else if(Speed_d<=-8){
    PWMPOL=0x2C;
    PWMDTY2=0x20;
  }
  else if(Speed_d<6&&Speed_d>-8){
    PWMPOL=0x2C;
    tmp_speed=0xA0+Speed_d*4+(Speed_d-Speed_d_last)*2;
    if(tmp_speed<0){
      tmp_speed=0;
    }
    else if(tmp_speed>254){
      tmp_speed=254;
    }
    PWMDTY2=tmp_speed;
  }

  if(Speed_cnt<80){
    PWMPOL=0x2C;
    PWMDTY2=0xf0;
  }
  
  //发送速度监测信息
  //与速度设定程序冲突，关闭之
  /*  
  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<<1)+BadFrame); //设定速度+BadFrame
    sci_putchar(PWMDTY2);
    sci_putchar(0xEF);    //结束位
  }
  */
}