#include <hidef.h>      /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include <MC9S12XS128.h>     /* derivative information */     
#pragma  LINK_INFO DERIVE"MC9S12XS128.h"

void uart_init(void);
void uart_putchar(unsigned char ch);
void delay(long j);
void printp(void (*PortToPut)(), char* ctrl, ...);

#define  IMAGE_ROW     35      // < v9008  10
#define  IMAGE_COLUMN  100// 87//110-23 // 86//87
#define IMAGE_ROW_DIS  2//2
#define JIANGE_HANG 3  //这个数据必须大于3      当然也不能太大
#define  xcs_maxphoto   112
#define left_limit  2900
#define right_limit 4030
#define steer_mid  3334
unsigned char g_car_show_yn ;
unsigned char jiange_m=0;    //是不是间隔(jiange)了8行
unsigned char chang_count = 0;   //测试行场信号是否接对
unsigned char hang_i=0; //行中断里面用的记录每行点的个数的变量
unsigned char *H_puTemp= NULL;
unsigned char heixian=0; // 采集行的计数
 

unsigned char uca_Buffer1[IMAGE_ROW][IMAGE_COLUMN],PhotoLineInfoOld[IMAGE_ROW];;//图像数据缓存  
unsigned char uca_Buffer2[IMAGE_ROW][IMAGE_COLUMN];
 
 
unsigned char *puca_BufferSample=&uca_Buffer1[0][0];//指向当前采集数据采样缓存首地址的指针
 
unsigned char *puca_BufferProcess=&uca_Buffer2[0][0];//指向当前处理数据采样缓存首地址的指针 

unsigned char *puca_BufferTemp=&uca_Buffer1[0][0];

int e,a;
unsigned int distance_1=0,distance_2=99,g_row,g_line=0,end_p,pre_pluse,start_p;
int black_x[IMAGE_ROW]=0;
int st_err=0,st_err1,st_err2,c=0;
 
unsigned int b;
int m=0,k=0,d=0,steer;
int st_kp=0,x;

void delay_hang(unsigned char tt)
{

  while(tt-->0) {
 
   asm{
   nop;
   }
  }
}

void SetBusCLK_80M(void)
{
       
  CLKSEL=0x00;
  PLLCTL_PLLON=1;       //turn on PLL

  SYNR =0xc0 | 0x09;    //80M                    
  REFDV=0x80 | 0x01;
  
  POSTDIV=0x00;         //pllclock=2*16*(1+SYNR)/(1+REFDV)=xxMHz;
 
  while(CRGFLG_LOCK!=1);//Wait Till VCO Run Stable
  CLKSEL_PLLSEL=0x01;  
 
}

void Interrupts_init(void)
{             

   TIOS=0x00;         //外部输入捕捉0,1,2,3通道
  
   TSCR1_TFFCA=0;     // FAST FLAG CLEAR  读取即清零
 
   TCTL4_EDG0B = 0;   //Capture on rising edges only
   TCTL4_EDG0A = 1;
  
   TCTL4_EDG1B = 1;   //Capture on falling edges only
   TCTL4_EDG1A = 0;
 
   TIE_C0I = 0;
   TIE_C1I = 0;
   TIE_C2I = 0;
 
   PACTL = 0x40;    //脉冲计数 下降沿

   PACNT = 0x0000;  // Pulse Accumulators Count Registers (PACNT)
   
   TSCR1_TEN=1;     //timer enable    //这个语句必须要，虽然脉冲可以脱离时钟，但是系统中的时钟是不能不开启的
 
}

void PortInit(void)
{    
   DDRA=0X00; //   端口A 的方向输入
   DDRB=0XFF;    // 端口的方向输出
   PORTB=0xff;  
}

void PWM_init(void)
{

   PWME=0;
       
   PWMPRCLK|=0X05;        //选择总线时钟  2.5M
   PWMPOL_PPOL5=1;
   PWMCAE_CAE5=0;        //左对齐
   PWMCLK_PCLK5=0;       // 选择时钟A 舵机 
   PWMCTL_CON45=1;       //45通道级联
   PWMPER45=50000;        //50Hz
   PWMDTY45=3334;
  
   PWME=0Xff;
 
}

/*void Handle(void)
{
  unsigned char *t;
  unsigned int j,k,s,m,sa,ed;
 
  for(k=0;k<10;k++)
  {       
     for(j=0; j<IMAGE_COLUMN; j++)//
     {  
         t=puca_BufferProcess+k*IMAGE_COLUMN+j;                                                 
         if(*t<xcs_maxphoto)
            s=j;
     }
    
     for(j=IMAGE_COLUMN-1;j>0;j--)
     {
         t=puca_BufferProcess+k*IMAGE_COLUMN+j;
         if (*t<xcs_maxphoto)
            m=j;
     }
     sa=(s+m)/2;
  }
 
  for(k=10;k<IMAGE_ROW;k++)
  {       
     for(j=0; j<IMAGE_COLUMN; j++)//
     {  
         t=puca_BufferProcess+k*IMAGE_COLUMN+j;                                                 
         if(*t>=xcs_maxphoto)
            s=j;
     }
    
     for(j=IMAGE_COLUMN-1;j>0;j--)
     {
         t=puca_BufferProcess+k*IMAGE_COLUMN+j;
         if (*t>=xcs_maxphoto)
            m=j;
     }
     ed=(s+m)/2;
  }
  e=ed-sa;
 
  if(e==0)
    PWMDTY45=3334;
  else if(e)
} */

void get_start_p(void)
{
    unsigned char *pbuf;
    unsigned int left_p=0,right_p=0,ab;
    ab=black_x[start_p];
  
   for(g_row=0;g_row<10;g_row++)
   {
      pbuf=puca_BufferProcess+g_row*IMAGE_COLUMN;
      for(g_line=distance_1;g_line<=distance_2;g_line++)
      {     //g_line与distance的数据类型要一致
         if(*(pbuf+g_line)<112)
         {
            left_p=g_line;
            break;
         }
         else
           left_p=0;
      }
     
      for(g_line=distance_2;g_line>=distance_1;g_line--)
      {
        if(*(pbuf+g_line)<112)
        {
           right_p=g_line;
           break;
        }
        else
           right_p=0; 
      }
     
      black_x[g_row]=(left_p+right_p+1)/2;
     
      if(black_x[g_row])
      {
        start_p=g_row;
        return;
      }
     
      if(black_x[g_row])
      {                            //检测
          if(black_x[g_row-1]<14)
              distance_1=0;
            else
              distance_1=black_x[g_row-1]-13;
           
          if(black_x[g_row-1]>80)
              distance_2=99;
            else
              distance_2=black_x[g_row-1]+13;
      }
   }
   if(!black_x[start_p])
      black_x[start_p]=ab;
  
}

void get_end_p(void)
{
    unsigned char *pbuf;
    unsigned int left_p=0,right_p=0;  
    for(g_row=start_p+1;g_row<IMAGE_ROW;g_row++)
    {
       pbuf=puca_BufferProcess+g_row*IMAGE_COLUMN;
                                //检测
          if(black_x[g_row-1]<14)
              distance_1=0;
            else
              distance_1=black_x[g_row-1]-13;
          if(black_x[g_row-1]>80)
              distance_2=99;
            else
              distance_2=black_x[g_row-1]+13;
       
      for(g_line=distance_1;g_line<=distance_2;g_line++)
      {     //g_line与distance_1的数据类型要一致
        if(*(pbuf+g_line)<112)
        {
           left_p=g_line;
             break;
        }
         else
           left_p=0;
      }
     
     for(g_line=distance_2;g_line>=distance_1;g_line--)
     {
         if(*(pbuf+g_line)<112)
         {
            right_p=g_line;
            break;
         }
          else
            right_p=0; 
      }
     
      black_x[g_row]=(left_p+right_p+1)/2;
     
      if(!left_p&&!right_p)
      {    
         // black_x[g_row]=0;
        if(g_row)
          end_p=g_row-1;
         else
           end_p=start_p+1;
        return;
       }
   }
}

void get_steer(void){
 
    int black_y[]={22,24,25,27,29,30,32,34,35,37,
                   39,40,42,43,45,47,48,50,52,53,
                   55,57,58,60,62,63,65,67,68,70,
                   72,73,75,77,78,80,82,83,85,87,
                   88,90,92,93,95,97,98,100};
    c=0;   
    if(end_p<16&&end_p>start_p){
    for(b=start_p;b<=end_p;b++){
     d=black_x[b]-50;
     c=c+d;
                    //x=d*black_x[b]/100;
    } 
    st_err=c/end_p;
    st_kp=st_err*st_err*2/3+60;//-st_err1+st_err2
    x= (black_x[end_p]-black_x[start_p])/(end_p-start_p);
    steer=st_kp*x;
   
    }
    if(end_p>15){
       for(b=start_p;b<=end_p;b++){
        d=black_x[b]-50;
        c=c+d*black_y[b]/100;               //x=d*black_x[b]/100;
       }
     steer=c;
    
    }
 
     if(steer>400)  steer=400;
     if(steer<-400) steer=-400;
                              
      PWMDTY45=steer_mid-steer;

   //if(PWMDTY23<left_limit) PWMDTY23=left_limit;
  // else if(PWMDTY23>right_limit) PWMDTY23=right_limit;             
}

#pragma CODE_SEG __NEAR_SEG NON_BANKED
#pragma TRAP_PROC
void  Port1_interrupt(void)
 {    
     //测试行场信号是否接对
      if(chang_count++>=50)
      {
       PORTB_PB0 = ~PORTB_PB0;
       chang_count =0;
      }
     
      TFLG1_C1F=1;  // 清场中断标志
                
      heixian =0; //从置 行里面的信息 
      
      jiange_m=0; //行数清零
      
     //交换图像采集和处理缓存
      puca_BufferSample=puca_BufferProcess; 
      puca_BufferProcess=puca_BufferTemp;
      puca_BufferTemp=puca_BufferSample;
      
     
       TFLG1_C0F=1; // 清行中断标志 --注意这个是必须的。切记！！！！
       TIE_C0I =1;// 行中断
              
}

/*
*****************************行中断*********************************************************************
*
************************* *******************************************************************************
*/
void   Port0_interrupt(void)
{
   TFLG1_C0F=1; // 清中断标志

   if(jiange_m>JIANGE_HANG)      //if JIANGE_HANG ==3 每隔5行采集一次。  if 4 每隔6行
   {
       jiange_m = 0;
        
       //----------------ver------ 2.0  ---------------核心代码--------------------------------        
       if(heixian<IMAGE_ROW - IMAGE_ROW_DIS )
        {
             delay_hang(20);//延时来过滤行消隐区
            
             H_puTemp =puca_BufferSample+heixian*IMAGE_COLUMN;
            
             for(hang_i=0;hang_i<IMAGE_COLUMN;hang_i++)
             {
              
                 *(H_puTemp+hang_i)=PORTA;
             }
              
             heixian ++;  //记录黑线的数目
                
             if(heixian ==(IMAGE_ROW - IMAGE_ROW_DIS))
             {
              g_car_show_yn = 1;//可以显示，可以处理这场数据了
             }
                           
        }    
      //----------------ver------ 2.0  ----------------核心代码--------------------------------       
   }
        
   jiange_m++; 

   TFLG1_C0F=1; // 清中断标志     
}  //end inter

#pragma CODE_SEG DEFAULT
void main(void) {
  /* put your own code here */
 
  SetBusCLK_80M();
  Interrupts_init();
  PWM_init();
  PortInit();

 EnableInterrupts;

  for(;;) {
 
  while(g_car_show_yn==1)
    {
      get_start_p();
      get_end_p() ;
      if(start_p!=end_p)
          get_steer();
      /*if(a>4002)
        a=3999;
       else if(a<2900)
        a=3000;
       PWMDTY45=a; */
    }
   
    _FEED_COP(); /* feeds the dog */
   
  } /* loop forever */
  /* please make sure that you never leave main */
}