#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */

#define centre 99
#define kp1    24
#define kd1    10
static  uchar  Max;
static  uchar  Min;
static  uchar  Position;
static  uchar  data;
static  int    line[2];
static  int    deta_servo;
static  uint   pre_pwm;
static  uchar  evalue;
static  uchar  straight;
static  uchar  roadstate;
static  uchar  pixel[1200];
static  uchar  Interrupt_num;
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"

static uchar DATA[20] = {
  0x00,// 0;b159~152,
  0x00,// 1;b151~144,
  0x33,// 2;b143~136,
  0xe0,// 3;b135~128,
  0x15,// 4;b127~120,
  0x2c,// 5;b119~112,
  0x5d,// 6;b111~104,
  0x00,// 7;b103~ 96,
  0x0f,// 8;b 95~ 88,
  0xff,// 9;b 87~ 80,
  0xf0,//10;b 79~ 72,
  0x00,//11;b 71~ 64,
  0x00,//12;b 63~ 56, 
  0x00,//13;b 55~ 48,
  0x52,//14;b 47~ 40,
  0x09,//15;b 39~ 32,
  0xd1,//16;b 31~ 24,
  0x8d,//17;b 23~ 16,
  0x37,//18;b 15~  8,
  0x32 //19;b  7~  0,
};

void Init_Spi0(void)
{
   MODRR = 0x00;
   SPI0BR = 0x02;
   SPI0CR2 = 0x10;
   SPI0CR1 = 0x5e;
}

void Init_Clk(void)
{
  asm
  {
    LDAB  #1
    STAB  REFDV
    LDAB  #3
    STAB  SYNR								 ;System Clock Setting ->Bus Clk=32MHz
  Wait:
    BRCLR CRGFLG,#$08,*
    BSET  CLKSEL,#$80  				 ;Enable PLL Module
    
    LDAB  #$00								 
    STAB  PEAR								 ;Enable External Clock
  }
}

void Upload(uint i,uint j)
{
  uint counter;
  switch(i)	    
  {
    case  0: DATA[4] = 0x15;DATA[5] = 0x28;break;//75 
    case  1: DATA[4] = 0x15;DATA[5] = 0x3c;break;//80
    case  2: DATA[4] = 0x15;DATA[5] = 0x50;break;//85
    case  3: DATA[4] = 0x15;DATA[5] = 0x64;break;//90
    case  4: DATA[4] = 0x15;DATA[5] = 0x78;break;//95
    case  5: DATA[4] = 0x15;DATA[5] = 0x8c;break;//100
    default: DATA[4] = 0x15;DATA[5] = 0x50;break;//set ystart,b119~112	 ystart:b121~113
  }
  switch(j)
  {
    case  0: DATA[14] = 0x52;break;//brightest
    case  1: DATA[14] = 0x5a;break;
    case  2: DATA[14] = 0x62;break;
    case  3: DATA[14] = 0x6a;break;
    case  4: DATA[14] = 0x72;break;
    case  5: DATA[14] = 0x7a;break;//darkest
    default: DATA[14] = 0x6a;break;//set integration time,b47~40
  }
  PTJ = 0xc3;
  SPI0DR = DATA[0];
  for(counter=0;counter<20;counter++)
  {
    while(!(SPI0SR&0x20))
    {}
    SPI0DR = DATA[counter];
  }
  for(counter=0;counter<100;counter++)
  {asm("NOP");}
  PTJ = 0xc2;
  for(counter=0;counter<50;counter++)
  {asm("NOP");}
}

void ResetPer(void)
{
  uint i;
  PTJ = 0x00;
  for(i;i<2000;i++)
  {asm("NOP");}
  PTJ = 0xc2;
  for(i=0;i<20000;i++)
  {asm("NOP");}
}

void ResetFIFO(void)
{
  uchar i;
  PTJ = 0x02;
  for(i=0;i<200;i++)
  {asm("NOP");}
  PTJ = 0xc2;
  for(i=0;i<10;i++)
  {asm("NOP");}
}

void Get_Ref(void)
{
  uint i;
  Min = pixel[10];
  Max = pixel[10];
  for(i=12;i<188;i++)
  {
    data = pixel[i];
    if(Max<data) Max = data;
    if(Min>data) Min = data;
  }
}

void  Init_PWM(void)
{
  PWME     = 0;          //PWM禁止
  PWMPRCLK = 0x22;       //Clock A=bus clock/4=8MHz,Clock B=bus clock/4=8MHz;Bus Clk = 32MHz 
  PWMSCLA  = 0;	         //PWM時鐘SA的選擇 Clock SA=Clock A/2*PWMSCLA,//0=256
  PWMSCLB  = 1; 	       //PWM時鐘SB的選擇 Clock SB=Clock B/2*PWMSCLB,//0=256
  PWMCLK   = 0xcf;       //7,6,3,2選擇時鐘SB,4，5選擇時鐘A，1，0選擇時鐘SA
  PWMPOL   = 0xff;       //PWM各通道極性的選擇先高后低
  PWMCAE   = 0;		       //PWM各通道對齊方式的選擇 左對齊
  PWMCTL   = 0xf0;       //16位PWM波方式，奇數通道有效
  PWMDTY23 = 6454;	     //PWM3用來產生舵機的pwm波7.5%為中間
  PWMDTY45 = 9000;       //PWM5用來產生電機的pwm波
  PWMPER23 = 20480;      //320*4*32 //對應jp5
  PWMPER45 = 20000;      //PWM5用來產生電機的pwm波
  PWME     = 0x00;       //PWM2,3,4,5通道使能 
}

uchar Sample(void)			 //賽道種子點采集算法，待提高
{
  uchar num,eflag,pos,start;
  uint i,imm,end;
  imm = (Min+Max)/2;
  evalue = imm;
  num = 0;
  eflag = 0;
  if(!straight)
  {
    start = 201;
    end   = 377;
  }
  else
  {
    start = 220;
    end   = 360;
  }
  for(i=start;i<end;i++)
  {
    data = pixel[i];
    if(data<evalue)
    {
      num++;
    }
    else
    {
      if(num>5)
      {
        eflag++;
        pos = i-202;
      }
      num=0;
    }
  }
  if(eflag==0)
  {
    return(0);
  }
  else
  {
    return(pos);
  }
}

void Deliver(void)
{
  asm
  {
    BRCLR SCI0SR1,#$80,*
    LDAB  #$00
    STAB  SCI0DRL
    BRCLR SCI0SR1,#$80,*
    LDAB  evalue
    STAB  SCI0DRL
    BRCLR SCI0SR1,#$80,*
    LDAB  Min
    STAB  SCI0DRL
    BRCLR SCI0SR1,#$80,*
    LDAB  Max
    STAB  SCI0DRL
    BRCLR SCI0SR1,#$80,*
    LDAB  Position
    STAB  SCI0DRL
    BRCLR SCI0SR1,#$80,*
    LDAB  #$FF
    STAB  SCI0DRL
  }
}

void Init_Sci0(void)
{
  asm
  {
    LDAB #$0C
    STAB SCI0CR2
    LDAB #$00
    STAB SCI0BDH
    LDAB #$34
    STAB SCI0BDL
  }
}

void control(void)							//控制算法，待提高
{
  switch(roadstate)
  {
    case 0:											//彎道：減速，參數調小
    {
      PWME = 0x0c;
      PWMDTY45 = 9000;
      PWME = 0x3c;
    };break;
    case 1:											//直道：加速，參數調大
    {
      PWME = 0x0c;
      PWMDTY45 = 8000;
      PWME = 0x3c;
    };break;
    default:										//默認為直道
    {
      PWME = 0x0c;
      PWMDTY45 = 9000;
      PWME = 0x3c;
    };break;
  }
  PWME    = 0x30;
  deta_servo = (kp1*line[1]+kd1*(line[1]-line[0]))/2;
  pre_pwm = 6454+deta_servo;		//舵機控制
  if(pre_pwm>7500) pre_pwm = 7500;
  if(pre_pwm<5400) pre_pwm = 5400;
  PWMDTY23= pre_pwm;
  PWME	  = 0x3c;
}

void Init_RTI(void)
{
  RTICTL  = 0x77;								//98Hz  freq=oscclk/((b[3:0]+1)*2^(b[6:4]+9))
  CRGINT |= 0x80;
  Interrupt_num = 0;
}

interrupt 7 void RTIF(void)
{
  uchar key,emptyflag;
  uint i;
  asm
  {
    BSET CRGFLG,#$80
  }
  Interrupt_num++;
  PORTB = Interrupt_num;
  DisableInterrupts;
  emptyflag = 0;
  ResetFIFO();
  if(straight)
  {
    Upload(3,5);
  }
  else
  {
    Upload(0,5);
  }
  while(!emptyflag)		//kong->xunhuan
  {
    key = PTIT;
    key = key&0x10;
    if(key)
    {emptyflag = 1;}  //fei kong
    else
    {emptyflag = 0;}  //kong
  }
  i = 0;
  while(emptyflag)
  {
    asm
    {
      ;BRCLR SCI0SR1,#$80,*
      LDAB #$00
      STAB PORTK
      LDAA PORTA
      STAA data
      ;STAA SCI0DRL
      LDAB #$80
      STAB PORTK
    }
    pixel[i] = data;
    i++;
    key = PTT;
    key = key&0x10;
    if(!key)
    {
      emptyflag = 0;
    }
  }
  Get_Ref();
  Position = Sample();
  if(Position!=0)
  {
    line[0] = line[1];
    line[1] = Position-centre;
  }
  if(line[1]>-10||line[1]<10)
  {
    straight++;
  }
  else
  {
    straight = 0;
  }
  if(straight>20)
  {
    roadstate = 1;
    straight = 0;
  }
  else
  {
    roadstate = 0;
  }
  control();
  Deliver();
  ResetFIFO();
  EnableInterrupts;
}

void main(void) 
{
  uint i,key,keyflag,emptyflag;
  DisableInterrupts;
  Init_Spi0();
  Init_Sci0();
  ResetPer();
  Init_Clk();
  Init_PWM();
  Init_RTI();
  DDRJ  = 0xff;
  PTJ   = 0xff;
  DDRB  = 0xff;
  PORTB = 0x0f;
  DDRK  = 0xff;
  DDRT  = 0x00;
  PORTK = 0xff;
  keyflag = 0;
  emptyflag = 0;
  Upload(0,5);
  while(!emptyflag)		//kong->xunhuan
  {
    key = PTIT;
    key = key&0x10;
    if(key)
    {emptyflag = 1;}  //fei kong
    else
    {emptyflag = 0;}  //kong
  }
  for(i=0;i<1000;i++)
  {asm("NOP");}
  ResetFIFO();
  key = 1;
  while(key)
  {
    key = PTIT;
    key = key&0x20;
    keyflag = 0;
    while(!key)
    {
      key = PTIT;
      key = key&0x20;
      keyflag = 1;
    }
    if(keyflag) key = 0;
  }
  ResetFIFO();
  Upload(1,5);
  while(!emptyflag)		//kong->xunhuan
  {
    key = PTIT;
    key = key&0x10;
    if(key)
    {emptyflag = 1;}  //fei kong
    else
    {emptyflag = 0;}  //kong
  }
  /*for(i=0;i<1200;i++)
  {
    data = pixel[i];
    asm
    {
      BRCLR SCI0SR1,#$80,*
      LDAB  data
      STAB  SCI0DRL
    }
  }*/
  //Get_Ref();
  //Position = Sample();
  //Deliver();
  PORTB = 0x5a;
  /* put your own code here */
  EnableInterrupts;
  for(;;) {} /* wait forever */
}