#include "common.h"
#include "include.h"
#include "ysz.h"

//IO口连接
//野火摄像头SCCB CLK:PTA14 SDA:PTA17 DATA:PTD0~D7 VSYNC:PTB1 PCLK:PTC0 /VCC:3.3 
//NRF2401L SCK:PTC5 MOSI:PTC6 MISO:PTC7 IRQ:PTC8 CE:PTC9 PCSN:PTC10  /VDD:3
//OLED 使用LQ板子设计 SCL:PTA15 SDA:PTA16 RST:PTA13 DC:PTA12   /VCC:3.3MCU
//按键 OK:PTC11 CANCEL:PTC12 ++:PTC13 --:PTC14 小灯:PTE6
//编码器硬件计数:PTA19
//舵机: PTB18 /6.2 
//电机:PTC3,PTC4  
//TF:PTE0~E5 /3.3MCU
//AD:C1 C2 E25 


#define PPY_MID CAMERA_W/2
#define PPY_CAMERA_W CAMERA_W/8
#define PPY_SPEED 100
#define PPY_A 10   //查找范围
#define PPY_B 4   //查找范围
#define PPY_TEZHENG 4   //找前几行的方法
#define PPY_BIANXIAN 16   //边线值> < 不同的找线方法
#define PPY_CAMERA_OK 58   //太远无效
u8 img_mid_jiaozheng[CAMERA_H]= //中线矫正数组
{
75,74,73,72,61,70,69,68,68,67,66,65,
64,63,62,61,60,59,58,57,56,55,54,53,
52,51,50,49,48,47,46,45,44,43,42,41,
40,39,38,37,36,35,34,33,32,31,30,29,
28,27,26,25,24,23,22,21,20,19,18,17
};   //前3后4自己编的

u8 duan;       //没记错是一个标志 判断虚线
volatile s16 ppy_=0;     //加权后中线
volatile s16 carNow;
volatile s16 Mid_XieLv;

volatile s16 Mid_XieLvj;
volatile s16 Mid_XieLvy;
extern u16 jiance;
extern u16 duShu;
extern u32 LPT_INT_count;//脉冲计数
extern u8 *img_bin_buff ;
extern s16 img_mid[CAMERA_H];
extern u32 DUOJI,DIANJI1,DIANJI2;
extern u8  ppy_qianzhan;  
extern u16 Factor_A,Factor_d,A_;
extern u16 ppy_qian;
extern u16 ppy_C,ppy_D,ppy_E,Factor_i;
extern u16 KP,KI,KD;        //速度PID
extern u16 ppy_speed;             //期望速度
extern u8  nrf_buff3[ MAX_ONCE_TX_NUM];     //预多 2401发送包   发送示波器参数
extern u8   *nrf_3u32;            //发送指针
extern ppy_car_e py;             //控制
extern ppy_model_e ppyModel;
extern u8 img_buf[CAMERA_W*CAMERA_H];



u8  qianzhan=0;
u8 ppy_img_mid(s16 * mid,u8 * img)       //返回前瞻量 中线数组,图像数组
{   
       volatile u8 i=0,j,k,a,s=1,temp_i;  //k前面用的标志  s为双标记 是否找到连续两个SHUANG temp_i作为比较i是否连续 r为一开始断
      volatile s16 l_temp=PPY_MID-3*PPY_A,r_temp=PPY_MID+3*PPY_A;   //记录上一排的值
      volatile s16 l_,r_,temp_a;         //这一排
      volatile u8 l_flag=1,r_flag=1;   //左右标志位   
       volatile u8  duanxian;   //左右标志位
      volatile    u8  BLACK_dotl,BLACK_dotr,WHITE_dot;
       
       duan=0;
     qianzhan=0;
     
   
      
        
        
      for(duanxian=1;duanxian!=4;i++)
      {
        l_flag=1;   //重置标志位
        r_flag=1;
        
             for(j=l_temp+PPY_A+20;j>0;j--)                  //从中间往左找
                   { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                      {   l_=j;                             //记录此时边线值到左中
                        l_flag=0  ;                          //标志位
                        break;}                        //第一次找左线                 
                    }             
            
             for(j=r_temp-PPY_A-20;j<CAMERA_W-1;j++)            //同理往右找
                     { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                       { r_=j;
                           r_flag=0;
                            break;}                //第一次找右线
                        
                     }  
             if(l_flag==0&&r_flag==1)   //只找到左
             {
                    *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                    if(duanxian==1&&i>0)
                                {
                                
                                  buXian2(qianzhan,i);
                                }
                      l_temp=l_;  //更新TEMP
                      duanxian++;    //断线=4即继续往下找
                      qianzhan++;     //
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
                *mid++= trapezium_r(r_,i);
                 if(duanxian==1&&i>0)
                                {
                               
                                   buXian2(qianzhan,i);
                                }
                  r_temp=r_;
                  duanxian++;
                  qianzhan++;
             }
              else if(l_flag==0&&r_flag==0)  //都
             {
                *mid++ = ( r_-l_     )/2 +l_; //都找 到了  
            //    l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
            //    r_error=r_-r_temp;
                if(duanxian==1&&i>0)
                                {
                                  buXian2(qianzhan,i);
                                }
                r_temp=r_;
                l_temp=l_;
                duanxian++;
                qianzhan++;
             } 
              else if(l_flag==1&&r_flag==1)  //断
             {
               
               
               if(i==3)
               duan++;   //前面没找到
            //   qianpai++;
             } 
             
             if(i%2==0&&duanxian==1) //从未找到
                i++;//保证从偶行开始
             
                                   /*      if(i<3)       //起跑线线
                                    {
                                    BLACK_dotr=0;   BLACK_dotl=0;WHITE_dot=0;
                                      for(j=PPY_CENTER;j<130;j++) //
                                      {
                                         if( (*(img+j)==PBLACK) )
                                           BLACK_dotr++;//
                                         else
                                           WHITE_dot++;//
                                      }
                                      for(j=PPY_CENTER;j>30;j--) //
                                      {
                                         if( (*(img+j)==PBLACK) )
                                           BLACK_dotl++;//
                                         else
                                           WHITE_dot++;//
                                      }
                                      if((WHITE_dot>5&&BLACK_dotl>10&&BLACK_dotr>2)||(WHITE_dot>5&&BLACK_dotr>10&&BLACK_dotl>2))
                                      {
                                        ppy_stop();
                                        return 0;
                                      }
                                    }  */
             
          
                  
            
           //  mid++:
               img+=CAMERA_W;              //指针指向下一排
               
      }
      
     
         
         
                if(l_flag==0&&r_flag==1)   //只找到左
             {
                   r_temp=CAMERA_W-PPY_A;
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
               
                 l_temp=PPY_A;
               
             }
      
     
         for(k=PPY_BIANXIAN,duanxian=0;i<PPY_CAMERA_OK;i++)    //隔两扫描
                  {
                           l_flag=1;   //重置标志位
                            r_flag=1;
                            
                          
                            if(i==5||i==6)
                                  {
                                    
                                    BLACK_dotr=0;   BLACK_dotl=0;WHITE_dot=0;
                                      for(j=PPY_CENTER;j<130;j++) //
                                      {
                                         if( (*(img+j)==PBLACK) )
                                           BLACK_dotr++;//
                                         else
                                           WHITE_dot++;//
                                      }
                                      for(j=PPY_CENTER;j>30;j--) //
                                      {
                                         if( (*(img+j)==PBLACK) )
                                           BLACK_dotl++;//
                                         else
                                           WHITE_dot++;//
                                      }
                                      if((WHITE_dot>5&&BLACK_dotl>10&&BLACK_dotr>2)||(WHITE_dot>5&&BLACK_dotr>10&&BLACK_dotl>2))
                                      {
                                        ppy_stop();
                                        return 0;
                                      }
                                      
                                    }  
                             ////////////////////////////////////////     
                        if(k==2*PPY_BIANXIAN)  //程序出口改变
                        {
                           for(;i<PPY_CAMERA_OK;i++)
                               {      l_flag=1;      
                                
                                 for(j=l_temp+PPY_A+10;j>l_temp;j--)                  //从中间往左找
                                      { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                                         {   l_=j;                             //记录此时边线值到左中
                                            l_flag=0;
                                        break;}                                      
                                       } 
                                 if(l_flag==0)    //找到了
                              {  if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                                qianzhan++;
                              }
                           //  if(((*(img+l_temp+2)==PBLACK)&&(*(img+l_temp+1)==PWHITE))||((*(img+l_temp+1)==PBLACK)&&(*(img+l_temp)==PWHITE)))// 回拐了                   
                         //    return(i);      //返回第几排
                              l_temp=l_;     //为下一次做准备
                                                       
                            if(l_temp>150)  //急弯到左边了
                                    return(qianzhan);      //返回第几排
                              }
                              
                                if(l_flag==1) //下一排没找到
                                   return(qianzhan);      //返回上一排 
                          //  mid++;   //隔行采
                            img+=CAMERA_W;              //指针指向下一排
                           
                            }
                          
                        }
                  ////////////////////////////////
                if(k==0)      //程序出口改变
                {
                        for(;i<PPY_CAMERA_OK;i++)
                 {      r_flag=1;
                            
                             for(j=r_temp-PPY_A-10;j<r_temp;j++)            //往右找
                         { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                           { r_=j;
                                r_flag=0;                 
                                break;}                //第一次找右线
                            
                          }                           
                            
                         if( r_flag==0)
                         {
                           if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_r(r_,i);  //带入找右边的中线位置
                                qianzhan++;
                              }
                   //  if(((*(img+r_temp-2)==PBLACK)&&(*(img+r_temp-1)==PWHITE))||((*(img+r_temp-1)==PBLACK)&&(*(img+r_temp)==PWHITE)))// 回拐了                   
                    //         return(i-5);      //返回第几排
                        r_temp=r_;     //为下一次做准备
                                            
                            if(r_temp<10)  //急弯到左边了
                                    return(qianzhan);      //返回第几排   
                         }
                         
                      if(r_flag==1) //下一排没找到
                                   return(qianzhan);      //返回上一排 
                           //   mid++;
                            img+=CAMERA_W;              //指针指向下一排
                 }
                }
              
                 /////////正常双边巡线
                              temp_a=l_temp-PPY_A-12;                     //限定最大限,不溢出
                                   if(temp_a<3)
                                     temp_a=0;
                                    for(j=l_temp+PPY_A+25;j>temp_a;j--)                  //从中间往左找
                        { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                           {   l_=j;                             //记录此时边线值到左中
                             l_flag=0  ;                          //py变成找到左线
                              break;}                        //第一次找左线                 
                        }                   
            
                        temp_a=r_temp+PPY_A+12;                     //限定最大限,不溢出
                                   if(temp_a>157)
                                     temp_a=CAMERA_W-1;
                             for(j=r_temp-PPY_A-25;j<temp_a;j++)            //往右找
                          { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                             { r_=j;
                               r_flag=0;
                               break;}                //第一次找右线
                        
                           }
                         
               if(l_flag==0&&r_flag==1)   //只找到左
             {
                if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                               
                                
                                
                                qianzhan++;
                              }
                  //l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
                      l_temp=l_;  //更新TEMP
                     
                      if(i>25)
                      {
                          k++;       //单线5排后找线方法改变
                      }
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
                 if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_r(r_,i);  //带入找右边的中线位置
                              
                                qianzhan++;
                              }
               //   r_error=r_-r_temp;
                  r_temp=r_;  
                    
                  if(i>25)
                      {
                          k--;       //单线5排后找线方法改变
                      }
             }
              else if(l_flag==0&&r_flag==0)  //都
             {
                 if(i%2==0)  //偶数排计数
                              {
                                *mid++ =(r_+l_)/2;
                               
                                qianzhan++;
                              }
             //   l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
             //   r_error=r_-r_temp;
                r_temp=r_;
                l_temp=l_; 
               
             } 
              else if(l_flag==1&&r_flag==1)  //断
             {
               duanxian=1;
                if(i<45)
              
              duan++;   //断一次记一次
             }   
           //    mid++;
           img+=CAMERA_W;              //指针指向下一排
             ////////////////////////////////////////////////   
                if(duanxian==1)           //断线
                {
                  u8 kduan=1;
                  for(i++;duanxian!=0;i++)       //
      {
        l_flag=1;   //重置标志位
        r_flag=1;
        if(i>50)
          return(qianzhan);//大于一个值退了算了
               temp_a=l_temp-PPY_A-15;                     //限定最大限,不溢出  //普通搜素范围-12~+22
                                   if(temp_a<3)
                                     temp_a=0;
                                    for(j=l_temp+PPY_A+20;j>temp_a;j--)                  //从中间往左找
                        { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                           {   l_=j;                             //记录此时边线值到左中
                             l_flag=0  ;                          //py变成找到左线
                              break;}                        //第一次找左线                 
                        }                   
            
                        temp_a=r_temp+PPY_A+15;                     //限定最大限,不溢出
                                   if(temp_a>157)
                                     temp_a=CAMERA_W-1;
                             for(j=r_temp-PPY_A-20;j<temp_a;j++)            //往右找
                          { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                             { r_=j;
                               r_flag=0;
                               break;}                //第一次找右线
                        
                           }
             if(l_flag==0&&r_flag==1)   //只找到左
             { 
             
                if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                                 if(kduan==0)
                                 buXian(qianzhan,i);
                                qianzhan++;
                                  duanxian=0;
                                  
                              }
                      l_temp=l_;  //更新TEMP
                   //   r_temp=l_+2*img_mid_jiaozheng[i];
                    
                  
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
              
                  if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_r(r_,i);  //带入找右边的中线位置
                                 if(kduan==0)
                                 buXian(qianzhan,i);
                                qianzhan++;
                                  duanxian=0;
                              }
                  r_temp=r_;
                 
                
                 
             }
              else if(l_flag==0&&r_flag==0)  //都
             {
                 if(i%2==0)  //偶数排计数
                              { 
                                *mid++ = ( r_+l_     )/2 ; //都找 到了
                                 if(kduan==0)
                               buXian(qianzhan,i);
                                qianzhan++;
                                 duanxian=0;
                              }
              
            //    l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
            //    r_error=r_-r_temp;
                r_temp=r_;
                l_temp=l_;
               
                
             } 
              else if(l_flag==1&&r_flag==1)  //断
             {
               duanxian++;
            //   qianpai++;
             //  duan++;
             } 
           //  mid++:
             if(duan==1&&((duanxian==5&&(i%2==0))||(duanxian==6&&(i%2==0))))  //十字湾  
             {
               u8 tempx=0; //1为左更新L 2为右
              kduan=0;//补线
                 tempx = panDuan(qianzhan);//除去数组中杂点
                 if (tempx == 1)
                 {
                     l_temp = 68;
                     r_temp -= 8;  //10内收
                 }

                 else if (tempx == 2)
                 {
                     l_temp += 8;
                     r_temp = 96;
                 }
                 else
                 {
                     r_temp -= 8;  //10内收
                     l_temp += 8;
                 }
             }
               
               img+=CAMERA_W;              //指针指向下一排
               
      }
                }
                  }  

         
      return(qianzhan);
   
}


void buXian2(u8 x,u8 i)
{
 s16 d= img_mid[0];
 for(u8 k=0;k<i/2;k++)
 {
   img_mid[x]=d;
   qianzhan++;
    x++;
   
 }
 img_mid[qianzhan]=d;
  
}

u8 panDuan(u8 x) //判断是否有效 返回值
{
  s16 k,p=0; //p 1 you 2 zuo
  while(x>1)
  {     
      k=  img_mid[x-1]-img_mid[x-2];
      if(  k>7)
        p=2;
        
       else if(k<-7)
         p=1;
       
      else
          
      {if(p==1)
          return 1;
      else if(p==2)
        return 2;
        else
              return 0; //没有杂点
              
      }
       x--;
      qianzhan--;
  }
  
}

void buXian(u8 x,u8 i)//十字补线
{
  s16 m=(img_mid[x-1]+img_mid[x-2])/2;
  s16 d=img_mid[x-1];
  u8 j=i,p=x;
  for(u8 k=0;k<j/2-p;k++)                             //(j+2)/2-1
  {
    img_mid[x]=m;
    qianzhan++;
    x++;
  }
      img_mid[qianzhan-1]=d;
  
  
}


   
s16 trapezium(s16  x,u8 y)//左或者右
{ 
  s16 xtemp;
  
  xtemp=13100*(s16)(x-(s16)PPY_CENTER)/(64193-866*y);//简化后的梯形失真
  
  return(xtemp);
}

s16 trapezium_l(s16  x,u8 y)//左
{ 
  s16 xtemp;
 s16 ROADWIDTH;
  ROADWIDTH=  y/5+14;  
  xtemp=13100*(s16)(x-(s16)PPY_CENTER)/(64193-866*y);//简化后的梯形失真
  xtemp+=ROADWIDTH;
   xtemp=      (xtemp*(64193-866*y)/13100)+PPY_CENTER; //反解到坐标系
  return(xtemp);
}
s16 trapezium_r(s16  x,u8 y)//右
{ 
  s16 xtemp;
   s16 ROADWIDTH;
 
         ROADWIDTH=  y/5+14;   
  xtemp=13100*(s16)(x-(s16)PPY_CENTER)/(64193-866*y);//简化后的梯形失真
  xtemp-=ROADWIDTH;
  xtemp=      (xtemp*(64193-866*y)/13100)+PPY_CENTER;
  return(xtemp);
}
   
void ppy_buxian( u8 ppy_buxian,s16 * mid)   //前瞻为1~55
{
  
            s16  s=0; //求和
            s16  i=0;
            carNow = (s16)((mid[0] + mid[1] + mid[2]) / 3);
            
        

            if (ppy_buxian <=10)
            {
                for (; i <=ppy_buxian; i++)
                {
                    s += mid[i];
                }
                ppy_ = (s16)(s /ppy_buxian);
            }
            else if (ppy_buxian < 20)
            {
               s+= mid[0];
               s += mid[3];
               s += mid[6];
               s += mid[9];
               for (i=11; i <ppy_buxian; i++)
               {
                   s += mid[i];
               }
               ppy_ = (s16)(s / (ppy_buxian-7));
            }
            else
            {
                s += mid[0];
                s += mid[3];
                s += mid[6];
                s += mid[9];
                s += mid[11];
                s += mid[13];
                s += mid[15];
                s += mid[17];
                s += mid[19];
                
                for (i = 20; i < ppy_buxian; i++)
                {
                    s += mid[i];
                }
                ppy_ = (s16)(s / (ppy_buxian - 11));
            }
    
        
}


 u8 shache;

void ppy_duoji(u8 qianzhan)
{
 
  volatile u16 ppy_high;
  volatile s16 Factor_p,i,p,d,v,error_;
  static s16 error_1 , error_2;
 if(qianzhan==0)
  return;
 
 
  
 if(duan==1)  //十字  
 {
   if(PPY_CENTER>=ppy_)    
   Factor_p=Factor_A*2/3;
   else  Factor_p=Factor_A*2/3;
   
    error_=(s16)(ppy_-(s16)PPY_CENTER);    //直线
 
   
 }
 else if(duan>1)  //虚线  
 {
  
   Factor_p=Factor_A*5/4;
 
    error_=(s16)(ppy_-(s16)PPY_CENTER);    //直线
  
 }
 
 
 else //if()             //入弯
 {
 
 if(PPY_CENTER>=ppy_)    
   Factor_p=Factor_A+(PPY_CENTER-ppy_)*A_;
   else  Factor_p=Factor_A+(ppy_-PPY_CENTER)*A_;
 
     if(qianzhan>=ppy_qian)
         error_=(s16)(img_mid[ppy_qian-1]-(s16)PPY_CENTER);        //增大偏差
     else error_=(s16)(img_mid[qianzhan-1]-(s16)PPY_CENTER);

 }
 
      p=(s16)(Factor_p*error_);
      d=(s16)(Factor_d*(error_-error_1));
      
      DUOJI=duoJiMid+p+d;
      
       
      
      

   
      if(DUOJI <duoJiLeft ) 
      DUOJI=duoJiLeft;
   
      else if(  DUOJI> duoJiRight )
       DUOJI=duoJiRight;   
     FTM_PWM_Duty(FTM2,CH0,DUOJI);
                    error_2 =error_1;
                    error_1=error_;                   
     
   
  
}

void ppy_duojinrf(u8 qianzhan)
{
 
  volatile u16 ppy_high;
  volatile s16 Factor_p,i,p,d,v,error_;
  static s16 error_1 , error_2;
 if(qianzhan==0)
  return;
 
 
 
 if(duan==1)  //十字  
 {
   if(PPY_CENTER>=ppy_)    
   Factor_p=Factor_A*2/3;
   else  Factor_p=Factor_A*2/3;
   
    error_=(s16)(ppy_-(s16)PPY_CENTER);    //直线
 
    ppy_speed=ppy_D+10;
   
 }
 else if(duan>1)  //虚线  
 {

   Factor_p=Factor_A*5/4;
 
    error_=(s16)(ppy_-(s16)PPY_CENTER);    //直线
 
        ppy_speed=ppy_C;
  
 }
 
 
 else if(((ppy_-PPY_CENTER)<16&&(ppy_-PPY_CENTER)>-16)&&qianzhan>=28)   //直线
 {
 
 if(PPY_CENTER>=ppy_)    
   Factor_p=Factor_A+(PPY_CENTER-ppy_)*A_;
   else  Factor_p=Factor_A+(ppy_-PPY_CENTER)*A_;
 
     if(qianzhan>=ppy_qian)
         error_=(s16)(img_mid[ppy_qian-1]-(s16)PPY_CENTER);        //增大偏差
     else error_=(s16)(img_mid[qianzhan-1]-(s16)PPY_CENTER);
      ppy_speed=ppy_C;
 }
 
 
 else        //其他
 {

 Factor_p=Factor_A;
     if(qianzhan>=ppy_qian)
         error_=(s16)(img_mid[ppy_qian-1]-(s16)PPY_CENTER);        //增大偏差
     else error_=(s16)(img_mid[qianzhan-1]-(s16)PPY_CENTER);
 ppy_speed=ppy_D;
 }
 
 

      p=(s16)(Factor_p*error_);
      d=(s16)(Factor_d*(error_-error_1));
      
      DUOJI=duoJiMid+p+d;
      
       
      

   
      if(DUOJI <duoJiLeft ) 
      DUOJI=duoJiLeft;
   
      else if(  DUOJI> duoJiRight )
       DUOJI=duoJiRight;   
     FTM_PWM_Duty(FTM2,CH0,DUOJI);
                    error_2 =error_1;
                    error_1=error_;                   
     
   
  
}

void Qiu_Mid_XieLvnrf(void)
{
  u8 i;
 s32 item_1=0,item_2=0,item_3=0,item_4=0;

  u8 Start;
  u8 End;

  Start=5;
  End=ppy_qianzhan;

    for (i =Start;i <End/2;i++) 
  {
    item_1 += i*img_mid[i];
    item_2 += i;
    item_3 += img_mid[i];
    item_4 += i*i;
  }

    Mid_XieLvj=100*(float)((item_1-(float)((item_2*item_3)/(End-Start)))/(item_4-(float)((item_2*item_2)/(End-Start))));

   Start=End/2;
 // End=ppy_qianzhan;
 item_1=0,item_2=0,item_3=0,item_4=0;
    for (i =Start;i <End;i++) 
  {
    item_1 += i*img_mid[i];
    item_2 += i;
    item_3 += img_mid[i];
    item_4 += i*i;
  }

    Mid_XieLvy=100*(float)((item_1-(float)((item_2*item_3)/(End-Start)))/(item_4-(float)((item_2*item_2)/(End-Start))));
  
    
    
}

void Qiu_Mid_XieLv(void)
{
  u8 i;
 s32 item_1=0,item_2=0,item_3=0,item_4=0;

  u8 Start;
  u8 End;

  Start=5;
  End=ppy_qianzhan-3;

    for (i =Start;i <End;i++) 
  {
    item_1 += i*img_mid[i];
    item_2 += i;
    item_3 += img_mid[i];
    item_4 += i*i;
  }

    Mid_XieLv=100*(float)((item_1-(float)((item_2*item_3)/(End-Start)))/(item_4-(float)((item_2*item_2)/(End-Start))));

}

void ppy_speedcol(u8 qianzhan)
{ 
  if(qianzhan==0)
  return;
    static u32 a=0; //测试每秒次数
  u32 count;
  s16 speed_1 ,E_,p,i,d;
 static s32 uid=16000;             //控制量
  static s16 E_1,    E_2;
  volatile u32 uid2;
  u32 m;
      count=LPTMR0_CNR;
    lptmr_counter_clean();
  speed_1=(s16)(LPT_INT_count*1000 + (s16)count);  //当前
  LPT_INT_count=0;               //各种清0
  
 
  
   E_=(s16)(ppy_speed-speed_1);                //期望
 

  
   p= (s16)(KP*(E_-E_1));
   i=(s16)(KI*E_);
   d=(s16)(KD*(E_+E_2-2*E_1));
   uid=uid+ p + i +d;
   
  
  
   
      if(uid>99999)
   {
     uid=99999;
      FTM_PWM_Duty(FTM0,CH2,(u32)uid);
       FTM_PWM_Duty(FTM0,CH3,0);
   }
   else if(uid<0&&uid>=-30000)     
   {
    
      FTM_PWM_Duty(FTM0,CH2,0);
  FTM_PWM_Duty(FTM0,CH3,(u32)(-uid));
   }
 else if(uid<-30000)     
   {
     uid=-30000;
      FTM_PWM_Duty(FTM0,CH2,0);
  FTM_PWM_Duty(FTM0,CH3,(u32)(-uid));
   }
  else
  {
  FTM_PWM_Duty(FTM0,CH2,(u32)uid);
  FTM_PWM_Duty(FTM0,CH3,0);
  }
   
  if(duShu&0x80)     //最后一位做无线开关
  {
   //以下为测试代码看示波器   //不能与SD卡同时用  60侦
    * ((u16 *) nrf_3u32)=ppy_speed;         //期望
        *((s16 *)(nrf_3u32+4))=speed_1;   //当前
        *((s32 *)(nrf_3u32+8))=uid;   //控制量
   disable_irq(89); //PTC无线中断 
  NRF_MSG_send(COM_3U32,nrf_buff3);    		//发送数据，发送长度由com对应的函数决定
   enable_irq(89); 	//C中断
  
  }
  E_2=E_1;
  E_1=E_;
  a++;  //测试每秒做的次数
}

void ppy_speedcolnrf(u8 qianzhan)
{ 
  if(qianzhan==0)
  return;
    static u32 a=0; //测试每秒次数
  u32 count;
  s16 speed_1 ,E_,p,i,d;
 static s32 uid=16000;             //控制量
  static s16 E_1,    E_2;
  volatile u32 uid2;
  u32 m;
      count=LPTMR0_CNR;
    lptmr_counter_clean();
  speed_1=(s16)(LPT_INT_count*1000 + (s16)count);  //当前
  LPT_INT_count=0;               //各种清0
  

  
   E_=(s16)(ppy_speed-speed_1);                //期望
 

  
   p= (s16)(KP*(E_-E_1));
   i=(s16)(KI*E_);
   d=(s16)(KD*(E_+E_2-2*E_1));
   uid=uid+ p + i +d;
   
 
  
   
      if(uid>99999)
   {
     uid=99999;
      FTM_PWM_Duty(FTM0,CH2,(u32)uid);
       FTM_PWM_Duty(FTM0,CH3,0);
   }
   else if(uid<0&&uid>=-30000)     
   {
    
      FTM_PWM_Duty(FTM0,CH2,0);
  FTM_PWM_Duty(FTM0,CH3,(u32)(-uid));
   }
 else if(uid<-30000)     
   {
     uid=-30000;
      FTM_PWM_Duty(FTM0,CH2,0);
  FTM_PWM_Duty(FTM0,CH3,(u32)(-uid));
   }
  else
  {
     FTM_PWM_Duty(FTM0,CH2,(u32)uid);
  FTM_PWM_Duty(FTM0,CH3,0);
  }
   
  if(duShu&0x80)     //最后一位做无线开关
  {
   //以下为测试代码看示波器   //不能与SD卡同时用  60侦
    * ((u16 *) nrf_3u32)=ppy_speed;         //期望
        *((s16 *)(nrf_3u32+4))=speed_1;   //当前
        *((s32 *)(nrf_3u32+8))=uid;   //控制量
   disable_irq(89); //PTC无线中断 
  NRF_MSG_send(COM_3U32,nrf_buff3);    		//发送数据，发送长度由com对应的函数决定
   enable_irq(89); 	//C中断
  
  }
  E_2=E_1;
  E_1=E_;
  a++;  //测试每秒做的次数
}


//压缩二值化图像解压（空间 换 时间 解压）反序版 先处理进的再到远
void img_extract(u8 * dst,u8 * src,u32 srclen)
{
	u8 colour[2]={PWHITE,PBLACK};	//0 白和 1 黑			
	u8 tmpsrc;
        src+=CAMERA_SIZE-1;
	while(srclen --)
	{
		tmpsrc = *src--;
		*dst++ = colour[ (tmpsrc >> 0 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 1 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 2 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 3 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 4 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 5 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 6 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 7 ) & 0x01 ];
	}
}
  
 /* 

//压缩二值化图像解压（空间 换 时间 解压）正常版
void img_extract(u8 * dst,u8 * src,u32 srclen)
{
	u8 colour[2]={PWHITE,PBLACK};	//0 白和 1 黑			
	u8 tmpsrc;        
	while(srclen --)
	{
		tmpsrc = *src++;
		*dst++ = colour[ (tmpsrc >> 7 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 6 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 5 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 4 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 3 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 2 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 1 ) & 0x01 ];
		*dst++ = colour[ (tmpsrc >> 0 ) & 0x01 ];
	}
}
  */	

u8 ppy_img_mid2(s16 * mid,u8 * img)       //返回前瞻量 中线数组,图像数组
{   
       volatile u8 i=0,j,k,a,s=1,temp_i;  //k前面用的标志  s为双标记 是否找到连续两个SHUANG temp_i作为比较i是否连续 r为一开始断
      volatile s16 l_temp=PPY_MID-3*PPY_A,r_temp=PPY_MID+3*PPY_A;   //记录上一排的值
      volatile s16 l_,r_,temp_a;         //这一排
      volatile u8 l_flag=1,r_flag=1;   //左右标志位   
       volatile u8  duanxian;   //左右标志位
       duan=0;
     qianzhan=0;
      for(duanxian=1;duanxian!=4;i++)
      {
        l_flag=1;   //重置标志位
        r_flag=1;
        
             for(j=l_temp+PPY_B;j>0;j--)                  //从中间往左找
                   { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                      {   l_=j;                             //记录此时边线值到左中
                        l_flag=0  ;                          //标志位
                        break;}                        //第一次找左线                 
                    }             
            
             for(j=r_temp-PPY_B;j<CAMERA_W-1;j++)            //同理往右找
                     { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                       { r_=j;
                           r_flag=0;
                            break;}                //第一次找右线
                        
                     }  
             if(l_flag==0&&r_flag==1)   //只找到左
             {
                    *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                    if(duanxian==1&&i>0)
                                {
                                
                                  buXian2(qianzhan,i);
                                }
                      l_temp=l_;  //更新TEMP
                      duanxian++;    //断线=4即继续往下找
                      qianzhan++;     //
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
                *mid++= trapezium_r(r_,i);
                 if(duanxian==1&&i>0)
                                {
                               
                                   buXian2(qianzhan,i);
                                }
                  r_temp=r_;
                  duanxian++;
                  qianzhan++;
             }
              else if(l_flag==0&&r_flag==0)  //都
             {
                *mid++ = ( r_-l_     )/2 +l_; //都找 到了  
            //    l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
            //    r_error=r_-r_temp;
                if(duanxian==1&&i>0)
                                {
                                  buXian2(qianzhan,i);
                                }
                r_temp=r_;
                l_temp=l_;
                duanxian++;
                qianzhan++;
             } 
              else if(l_flag==1&&r_flag==1)  //断
             {
               duan++;   //前面没找到
            //   qianpai++;
             } 
             
             if(i%2==0&&duanxian==1) //从未找到
                i++;//保证从偶行开始
              if(i>45)
              {return 0;}
             
           
                  
            
           //  mid++:
               img+=CAMERA_W;              //指针指向下一排
               
      }
      
    
         
         
                if(l_flag==0&&r_flag==1)   //只找到左
             {
                   r_temp=CAMERA_W-PPY_A;
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
               
                 l_temp=PPY_A;
               
             }
      
      
         for(k=PPY_BIANXIAN,duanxian=0;i<PPY_CAMERA_OK;i++)    //隔两扫描
                  {
                           l_flag=1;   //重置标志位
                            r_flag=1;
                             ////////////////////////////////////////     
                        if(k==2*PPY_BIANXIAN)  //程序出口改变
                        {
                           for(;i<PPY_CAMERA_OK;i++)
                               {      l_flag=1;      
                                
                                 for(j=l_temp+PPY_A+10;j>l_temp;j--)                  //从中间往左找
                                      { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                                         {   l_=j;                             //记录此时边线值到左中
                                            l_flag=0;
                                        break;}                                      
                                       } 
                                 if(l_flag==0)    //找到了
                              {  if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                                qianzhan++;
                              }
                           //  if(((*(img+l_temp+2)==PBLACK)&&(*(img+l_temp+1)==PWHITE))||((*(img+l_temp+1)==PBLACK)&&(*(img+l_temp)==PWHITE)))// 回拐了                   
                         //    return(i);      //返回第几排
                              l_temp=l_;     //为下一次做准备
                                                       
                            if(l_temp>150)  //急弯到左边了
                                    return(qianzhan);      //返回第几排
                              }
                              
                                if(l_flag==1) //下一排没找到
                                   return(qianzhan);      //返回上一排 
                          //  mid++;   //隔行采
                            img+=CAMERA_W;              //指针指向下一排
                           
                            }
                          
                        }
                  ////////////////////////////////
                if(k==0)      //程序出口改变
                {
                        for(;i<PPY_CAMERA_OK;i++)
                 {      r_flag=1;
                            
                             for(j=r_temp-PPY_A-10;j<r_temp;j++)            //往右找
                         { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                           { r_=j;
                                r_flag=0;                 
                                break;}                //第一次找右线
                            
                          }                           
                            
                         if( r_flag==0)
                         {
                           if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_r(r_,i);  //带入找右边的中线位置
                                qianzhan++;
                              }
                   //  if(((*(img+r_temp-2)==PBLACK)&&(*(img+r_temp-1)==PWHITE))||((*(img+r_temp-1)==PBLACK)&&(*(img+r_temp)==PWHITE)))// 回拐了                   
                    //         return(i-5);      //返回第几排
                        r_temp=r_;     //为下一次做准备
                                            
                            if(r_temp<10)  //急弯到左边了
                                    return(qianzhan);      //返回第几排   
                         }
                         
                      if(r_flag==1) //下一排没找到
                                   return(qianzhan);      //返回上一排 
                           //   mid++;
                            img+=CAMERA_W;              //指针指向下一排
                 }
                }
              
                 /////////正常双边巡线
                              temp_a=l_temp-PPY_A;                     //限定最大限,不溢出
                                   if(temp_a<3)
                                     temp_a=0;
                                    for(j=l_temp+PPY_A+5;j>temp_a;j--)                  //从中间往左找
                        { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                           {   l_=j;                             //记录此时边线值到左中
                             l_flag=0  ;                          //py变成找到左线
                              break;}                        //第一次找左线                 
                        }                   
            
                        temp_a=r_temp+PPY_A;                     //限定最大限,不溢出
                                   if(temp_a>157)
                                     temp_a=CAMERA_W-1;
                             for(j=r_temp-PPY_A-5;j<temp_a;j++)            //往右找
                          { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                             { r_=j;
                               r_flag=0;
                               break;}                //第一次找右线
                        
                           }
                         
               if(l_flag==0&&r_flag==1)   //只找到左
             {
                if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                               
                                
                                
                                qianzhan++;
                              }
                  //l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
                      l_temp=l_;  //更新TEMP
                     
                      if(i>20)
                      {
                          k++;       //单线5排后找线方法改变
                      }
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
                 if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_r(r_,i);  //带入找右边的中线位置
                              
                                qianzhan++;
                              }
               //   r_error=r_-r_temp;
                  r_temp=r_;  
                    
                  if(i>20)
                      {
                          k--;       //单线5排后找线方法改变
                      }
             }
              else if(l_flag==0&&r_flag==0)  //都
             {
                 if(i%2==0)  //偶数排计数
                              {
                                *mid++ =(r_+l_)/2;
                               
                                qianzhan++;
                              }
             //   l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
             //   r_error=r_-r_temp;
                r_temp=r_;
                l_temp=l_; 
               
             } 
              else if(l_flag==1&&r_flag==1)  //断
             {
               duanxian=1;
              duan++;
             }   
           //    mid++;
           img+=CAMERA_W;              //指针指向下一排
             ////////////////////////////////////////////////   
                if(duanxian==1)           //断线
                {
                  u8 kduan=1;
                  for(i++;duanxian!=0;i++)       //
      {
        l_flag=1;   //重置标志位
        r_flag=1;
        if(i>50)
          return(qianzhan);//大于一个值退了算了
               temp_a=l_temp-PPY_A-2;                     //限定最大限,不溢出  //普通搜素范围-12~+22
                                   if(temp_a<3)
                                     temp_a=0;
                                    for(j=l_temp+PPY_A+12;j>temp_a;j--)                  //从中间往左找
                        { if(   (*(img+j)==PBLACK)&&(*(img+j+1)==PWHITE) )//如果找到了
                           {   l_=j;                             //记录此时边线值到左中
                             l_flag=0  ;                          //py变成找到左线
                              break;}                        //第一次找左线                 
                        }                   
            
                        temp_a=r_temp+PPY_A+2;                     //限定最大限,不溢出
                                   if(temp_a>157)
                                     temp_a=CAMERA_W-1;
                             for(j=r_temp-PPY_A-12;j<temp_a;j++)            //往右找
                          { if(   (*(img+j)==PBLACK)&&(*(img+j-1)==PWHITE) )
                             { r_=j;
                               r_flag=0;
                               break;}                //第一次找右线
                        
                           }
             if(l_flag==0&&r_flag==1)   //只找到左
             { 
             
                if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_l(l_,i);  //带入找右边的中线位置
                                 if(kduan==0)
                                 buXian(qianzhan,i);
                                qianzhan++;
                                  duanxian=0;
                                  
                              }
                      l_temp=l_;  //更新TEMP
                   //   r_temp=l_+2*img_mid_jiaozheng[i];
                    
                  
             }
             else if(l_flag==1&&r_flag==0)  //右
             {
              
                  if(i%2==0)  //偶数排计数
                              {
                                *mid++ =trapezium_r(r_,i);  //带入找右边的中线位置
                                 if(kduan==0)
                                 buXian(qianzhan,i);
                                qianzhan++;
                                  duanxian=0;
                              }
                  r_temp=r_;
                 
                
                 
             }
              else if(l_flag==0&&r_flag==0)  //都
             {
                 if(i%2==0)  //偶数排计数
                              { 
                                *mid++ = ( r_+l_     )/2 ; //都找 到了
                                 if(kduan==0)
                               buXian(qianzhan,i);
                                qianzhan++;
                                 duanxian=0;
                              }
              
            //    l_error=l_-l_temp;           //更新ERROR 当S>2时ERROR有效
            //    r_error=r_-r_temp;
                r_temp=r_;
                l_temp=l_;
               
                
             } 
              else if(l_flag==1&&r_flag==1)  //断
             {
               duanxian++;
            //   qianpai++;
               duan++;
             } 
           //  mid++:
             if((duanxian==8&&(i%2==0))||(duanxian==9&&(i%2==0)))  //十字湾  
             {
               u8 tempx=0; //1为左更新L 2为右
              kduan=0;//补线
                 tempx = panDuan(qianzhan);//除去数组中杂点
                 if (tempx == 1)
                 {
                     l_temp = 70;
                     r_temp -= 8;  //10内收
                 }

                 else if (tempx == 2)
                 {
                     l_temp += 8;
                     r_temp = 98;
                 }
                 else
                 {
                     r_temp -= 8;  //10内收
                     l_temp += 8;
                 }
             }
               
               img+=CAMERA_W;              //指针指向下一排
               
      }
                }
                  }  

         
      return(qianzhan);
   
}

void ppy_buxian2( u8 ppy_buxian,s16 * mid)   //前瞻为1~55
{
  
            s16  s=0; //求和
            s16  i=0;
            carNow = (s16)((mid[0] + mid[1] + mid[2]) / 3);
            if (ppy_buxian <=10)
            {
                for (; i <=ppy_buxian; i++)
                {
                    s += mid[i];
                }
                ppy_ = (s16)(s /ppy_buxian);
            }
            else if (ppy_buxian < 20)
            {
               s+= mid[0];
               s += mid[3];
               s += mid[6];
               s += mid[9];
               for (i=11; i <ppy_buxian; i++)
               {
                   s += mid[i];
               }
               ppy_ = (s16)(s / (ppy_buxian-7));
            }
            else
            {
                s += mid[0];
                s += mid[3];
                s += mid[6];
                s += mid[9];
                s += mid[11];
                s += mid[13];
                s += mid[15];
                s += mid[17];
                s += mid[19];
                
                for (i = 20; i < ppy_buxian; i++)
                {
                    s += mid[i];
                }
                ppy_ = (s16)(s / (ppy_buxian - 11));
            }
}

 
void ppy_duoji2(u8 qianzhan)
{
  volatile u16 ppy_high;
  volatile s16 Factor_p,i,p,d,v,error_;
  static s16 error_1 , error_2;
 if(qianzhan==0)
  return;

 if(qianzhan<=18)
 error_=img_mid[qianzhan-1]-PPY_CENTER;
 else 
   error_=img_mid[18]-PPY_CENTER;
  
  if(PPY_CENTER>=ppy_)    
   Factor_p=200+(PPY_CENTER-ppy_)/A_;
   else  Factor_p=200+(ppy_-PPY_CENTER)/A_;
  
  // Factor_p=Factor_A;
      p=(s16)(Factor_p*error_);
    //  d=(s16)(Factor_d*(error_-error_1));
      
      DUOJI=duoJiMid+p;
   
      if(DUOJI <duoJiLeft ) 
      DUOJI=duoJiLeft;
     // error_2=(s16)DUOJI;
      else if(  DUOJI> duoJiRight )
       DUOJI=duoJiRight;   
     FTM_PWM_Duty(FTM2,CH0,DUOJI);
                    error_2 =error_1;
                    error_1=error_;                   
     
   
  
}

void ppy_speedcol2(u8 qianzhan)
{ 
  if(qianzhan==0)
  return;
    static u32 a=0; //测试每秒次数
  u32 count;
  s16 speed_1 ,E_,p,i,d;
 static s32 uid=16000;             //控制量
  static s16 E_1,    E_2;
  volatile u32 uid2;
  u32 m;
      count=LPTMR0_CNR;
    lptmr_counter_clean();
  speed_1=(s16)(LPT_INT_count*1000 + (s16)count);  //当前
  LPT_INT_count=0;               //各种清0
  
   E_=(s16)(ppy_speed-speed_1);                //期望
 

   // FTM_PWM_Duty(FTM0,CH3,0);           //PTc4
   p= (s16)(KP*(E_-E_1));
   i=(s16)(KI*E_);
   d=(s16)(KD*(E_+E_2-2*E_1));
   uid=uid+ p + i +d;
   if(uid>99999)
   {
     uid=99999;
      FTM_PWM_Duty(FTM0,CH2,(u32)uid);
       FTM_PWM_Duty(FTM0,CH3,0);
   }
   else if(uid<0&&uid>=-5000)     
   {
    
      FTM_PWM_Duty(FTM0,CH2,0);
  FTM_PWM_Duty(FTM0,CH3,(u32)(-uid));
   }
 else if(uid<-5000)     
   {
     uid=-5000;
      FTM_PWM_Duty(FTM0,CH2,0);
  FTM_PWM_Duty(FTM0,CH3,(u32)(-uid));
   }
  else
  {
     FTM_PWM_Duty(FTM0,CH2,(u32)uid);
  FTM_PWM_Duty(FTM0,CH3,0);
  }
   
  
  
  E_2=E_1;
  E_1=E_;
  a++;  //测试每秒做的次数
}

     
   
   










              
                              
                                 
                              
                  
                            
              
              
              