從0開始的智能車代碼（5）

環島處理方法存在一個嚴重的 bug，在入環或過環時會出現反向打角的現象。以左側入環為例，小車首先會發現左側存在一個 V 字跳變點，同時根據這個跳變點補右邊線，這是正確的。然而，當小車向左打角到一定程度之后，原先的 V 字跳變點會被認為是右側的跳變點，然后進行反向補線。

解決這個問題的方法是判斷出環島方向。具體的方法是在遇環時判斷跳變點的方向或者在入環時判斷第一次發現跳變點的方向。

另一個 bug 是當 V字跳變點位於圖像正中時，在兩側都會判斷出跳變點，根據之前的判斷方法，兩側都出現跳變點會被判定成十字。我的解決方案是添加一段特殊情況處理代碼。

由於出環補線的程序運行效果不太理想，我更改了出環的程序。在圖像中判斷存在出環跳變點時，停止刷新偏差值，保持原有的偏差值直到發現足夠多的右側邊界點。

void imageBinary(void)
{
  uint16 grayCount[256]={0};
  uint32 graySum=0;
  uint16 pixelSum=0;
  float graypro[256];
  
  uint8 threshold;
  uint16 i,j;
  uint16 step=2;//間隔
  
  for(i=0;i<MT9V032_H;i+=step){
    for(j=0;j<MT9V032_W;j+=step){
      grayCount[image[i][j]]++;
      graySum+=image[i][j];
      pixelSum++;
    }
  }
  
  for(i=0;i<256;i++){
    graypro[i]=(float)grayCount[i]/pixelSum;
  }
  
  float w0,w1,u0Temp,u1Temp,u0,u1,u,deltaTmp,deltaMax=0;
  for(i=0;i<256;i++){
    w0=w1=u0Temp=u1Temp=u0=u1=u=deltaTmp=0;
    for(j=0;j<256;j++){
      if(j<=i){
        w0+=graypro[j];
        u0Temp+=j*graypro[j];
      }
      else{
        w1+=graypro[j];
        u1Temp+=j*graypro[j];
      }
    }
    u0=u0Temp/w0;
    u1=u1Temp/w1;
    u=u0Temp+u1Temp;
    deltaTmp=w0*(u0-u)*(u0-u)+w1*(u1-u)*(u1-u);
    if(deltaTmp>deltaMax){
      deltaMax=deltaTmp;
      threshold=i;
    }
  }
  
  for(i=0;i<MT9V032_H;i++){
    for(j=0;j<MT9V032_W;j++){
      binaryImage[i][j]=(image[i][j]>threshold)?255:0;
    }
  }
  if(threshold<168){//出界停車
    DisableInterrupts;
    ctimer_pwm_duty(TIMER1_PWMCH0_A18,0);
    ctimer_pwm_duty(TIMER2_PWMCH1_B4,0);
  }
}

void imageProcess(void){
  uint16 i,j;
  uint16 edgeL[MT9V032_H];
  uint16 edgeR[MT9V032_H];
  uint16 midline[MT9V032_H];
  
  edgeL[MT9V032_H-1]=0;
  edgeR[MT9V032_H-1]=MT9V032_W-1;
  midline[MT9V032_H-1]=MT9V032_W/2;
  
  for(i=MT9V032_H-2;;i--){
    edgeL[i]=0;
    edgeR[i]=MT9V032_W-1;
    
    for(j=midline[i+1];j>0;j--){
      if(binaryImage[i][j]==0){
        edgeL[i]=j;
        break;
      }
    }
    
    for(j=midline[i+1];j<MT9V032_W-1;j++){
      if(binaryImage[i][j]==0){
        edgeR[i]=j;
        break;
      }
    }
    
    midline[i]=(edgeL[i]+edgeR[i])/2;//考慮環島補線后不能邊判斷邊線邊計算中線，這個中線只用來下一步尋找邊界
    
    if(i==0){
      break;
    }
  }
  
  /*----環島處理在這里開始----*/
  
  uint8 jumpFlagL=0;
  uint8 jumpFlagR=0;
  
  uint16 pointLX=0;//左邊線跳變點X坐標
  uint16 pointLY=0;//左邊線跳變點Y坐標
  uint16 pointRX=0;//右邊線跳變點X坐標
  uint16 pointRY=0;//右邊線跳變點Y坐標
  uint16 pointX=0;//跳變點X坐標
  uint16 pointY=0;//跳變點Y坐標
  
  uint8 pointSide=0;//跳變點方向，1表示左，2表示右
  uint8 pointType=0;//跳變點分類，1表示A字跳變點，2表示V字跳變點
  
  for(i=MT9V032_H*1/3;i<MT9V032_H-1;i++){//只判斷下2/3部分圖像的跳變點
    //左側A字跳變點
    if(edgeL[i]-edgeL[i-1]>20){
      jumpFlagL=1;
      pointLX=edgeL[i];
      pointLY=i;
      pointSide=1;
      pointType=1;
    }
    //左側V字跳變點
    else if(edgeL[i-1]-edgeL[i]>20){
      jumpFlagL=1;
      pointLX=edgeL[i-1];
      pointLY=i-1;
      pointSide=1;
      pointType=2;
    }
    //右側A字跳變點
    if(edgeR[i-1]-edgeR[i]>20){
      jumpFlagR=1;
      pointRX=edgeR[i];
      pointRY=i;
      pointSide=2;
      pointType=1;
    }
    //右側V字跳變點
    else if(edgeR[i]-edgeR[i-1]>20){
      jumpFlagR=1;
      pointRX=edgeR[i-1];
      pointRY=i-1;
      pointSide=2;
      pointType=2;
    }
  }
  
  uint8 jumpFlag=(jumpFlagL^jumpFlagR)&&((pointLY>MT9V032_H/2)||(pointRY>MT9V032_H/2));//防止十字誤判
  if((pointLX==MT9V032_W/2)&&(pointRX==MT9V032_W/2)&&(pointLY>MT9V032_H/2)&&(pointRY>MT9V032_H/2)){
    jumpFlag=1;//入環V形特殊情況
  }
  
  if(jumpFlagL==1){
    pointX=pointLX;
    pointY=pointLY;
  }
  else if(jumpFlagR==1){
    pointX=pointRX;
    pointY=pointRY;
  }
  
  //四步過環，遇環、入環、出環、過環
  static uint8 meetRingFlag=0;//遇環標志位，第一個A字標志點——第一個V字標志點
  static uint8 enterRingFlag=0;//入環標志位，第一個V字標志點——第二個A字標志點
  static uint8 leaveRingFlag=0;//出環標志位，第二個A字標志點——第二個V字標志點
  static uint8 passRingFlag=1;//過環標志位，第二個V字標志點——下一個A字標志點
  static uint8 ringSide=0;//環島類型，1表示左，2表示右
  
  uint16 pointLLCX=0;
  uint16 pointLLCY=MT9V032_H-1;
  uint16 pointLRCX=MT9V032_W-1;
  uint16 pointLRCY=MT9V032_H-1;
  
  float stepLength;//橫坐標插值步長
  
  if(jumpFlag){//若有有效跳變點
    if(pointType==1){//若為A字跳變點，說明出環或遇環
      if(passRingFlag==1){//遇環//遇環僅判斷方向用
        meetRingFlag=1;
        passRingFlag=0;
      }
      else if(enterRingFlag==1){//出環
        leaveRingFlag=1;
        enterRingFlag=0;
      }
    }
    else if(pointType==2){//若為V字跳變點，說明入環或過環
      if(meetRingFlag==1){//入環
        enterRingFlag=1;
        meetRingFlag=0;
      }
      else if(leaveRingFlag==1){//過環
        passRingFlag=1;
        leaveRingFlag=0;
      }
    }
  }
  
  if(meetRingFlag==1){
    if(jumpFlag==1){
      ringSide=pointSide;
    }
  }
  else if(enterRingFlag==1){
    if(jumpFlag==1){
      //入環補線
      //入環標志點（pointAX，pointAY），圖像左下角（pointLLCX，pointLLCY），圖像右下角（pointLRCX，pointLRCY）
      if(ringSide==1){//入環標志點在左邊，從右下角點到入環標志點連線，補右邊線
        stepLength=(float)(pointLRCX-pointX)/(float)(pointLRCY-pointY);
        for(i=0;i<pointLRCY-pointY;i++){
          edgeR[pointLRCY-i]=pointLRCX-(int)(i*stepLength);
          binaryImage[pointLRCY-i][edgeR[pointLRCY-i]]=0;
        }
      }
      else if(ringSide==2){//入環標志點在右邊，從左下角點到入環標志點連線，補左邊線
        stepLength=(float)(pointX-pointLLCX)/(float)(pointLLCY-pointY);
        for(i=0;i<pointLLCY-pointY;i++){
          edgeL[pointLLCY-i]=pointLLCX+(int)(i*stepLength);
          binaryImage[pointLLCY-i][edgeL[pointLLCY-i]]=0;
        }
      }
    }
  }
  else if(leaveRingFlag==1){
    if(ringSide==1){//若左圓環
      for(i=MT9V032_H*3/4;i<MT9V032_H-1;i++){//判斷右邊線下部分
        if(edgeR[i]==MT9V032_W-1){//若丟線
          return;//不改變偏差
        }
      }
    }
    if(ringSide==2){
      for(i=MT9V032_H*3/4;i<MT9V032_H-1;i++){
        if(edgeL[i]==0){
          return;
        }
      }
    }
  }
  
  else if(passRingFlag==1){
    if(jumpFlag==1){
      if(ringSide==1){
        stepLength=(float)(pointX-pointLLCX)/(float)(pointLLCY-pointY);
        for(i=0;i<pointLLCY-pointY;i++){
          edgeL[pointLLCY-i]=pointLLCX+(int)(i*stepLength);
          binaryImage[pointLLCY-i][edgeL[pointLLCY-i]]=0;
        }
      }
      else if(ringSide==2){
        stepLength=(float)(pointLRCX-pointX)/(float)(pointLRCY-pointY);
        for(i=0;i<pointLRCY-pointY;i++){
          edgeR[pointLRCY-i]=pointLRCX-(int)(i*stepLength);
          binaryImage[pointLRCY-i][edgeR[pointLRCY-i]]=0;
        }
      }
    }
  }
  
  /*----環島處理在這里結束----*/
  
  for(i=0;i<MT9V032_H;i++){//重新計算中線
    midline[i]=(edgeL[i]+edgeR[i])/2;
    binaryImage[i][midline[i]]=binaryImage[i][midline[i]-1]=binaryImage[i][midline[i]+1]=0;//粗線
  }
  lcd_showint8(0,5,jumpFlag);
  lcd_showint16(0,6,meetRingFlag*1000+enterRingFlag*100+leaveRingFlag*10+passRingFlag);
  
  offset=MT9V032_W/2-midline[88];
}