/*富士通微電子杯MCU競賽-----GPF_Source*/

#include "mb89201.h" 

int h,g;

void Delay(int m,int n){
  int i,j;
  for(i=0;i<m;i++)
    for(j=0;j<n;j++){;}
}

void PWM_ServoMotor(void){
  //1ms脈沖舵機右轉至極限位置,1.5ms脈沖舵機靜止,2ms脈沖舵機左轉至極限位置
  //車模右轉極限40度,左轉極限32度
  //(244,-45度),(243,-37.5度),(242,-30度),(241,-22.5度),(240,-15度),(239,-7.5度),(238,0度),
  //(237,7.5度),(236,15度),(235,22.5度),(234,30度),(233,37.5度),(232,45度)
  CNTR = 0xA2;  //PWM Wave Cycle:21ms
  COMR = 238;  //n=244(1ms),n=238(1.5ms),n=232(2ms) 
  CNTR_TPE = 1;
}  

void SquareWave_Motor(void){
  TCR0 = 0x02;  //Stop the Counter, Clock Source as 4tinst
  TCR1 = 0x02;  //Stop the Counter, Clock Source as 4tinst
  TCR2 = 0x02;  //Timer 0 as the output timer, And P34 as the output pin(TO)
  TDR0 = 187;  //Setting the register as 187 and Using 12MHz Crystal, the output freq is about 997.3Hz
  TDR1 = 187;  //Setting the register as 187 and Using 12MHz Crystal, the output freq is about 997.3Hz
  //TCR0_TSTR0 = 1;  //*Enable Counter of Timer
}

void PPG_Motor(int n){
  int a,b;
  a = n*4/64;
  b = n*4%64;
  RCR21 = b;  //Count clock 2tinst
  RCR22 = a;  //Duty Cycle is n%
  //RCR23 = 0x10;
  //RCR24 = 0x06;  //PPG output Freq is 937.5Hz
  //RCR23_RCEN = 1;  //Enable the PPG timer
}

void SearchLine1(void){
  if(PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==0&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==1){
  COMR = 238;
  PPG_Motor(30);
  } //1110111
  else if(PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==0&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==1||PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==0&&PDR0_P03==0&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==1){
  COMR = 237;
  PPG_Motor(25);
  } //1101111 or 1100111
  else if(PDR0_P00==1&&PDR0_P01==0&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==1||PDR0_P00==1&&PDR0_P01==0&&PDR0_P02==0&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==1){
  COMR = 236;
  PPG_Motor(20);
  } //1011111 or 1001111
  else if(PDR0_P00==0&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==1||PDR0_P00==0&&PDR0_P01==0&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==1){
  COMR = 235;
  PPG_Motor(20);
  } //0111111 or 0011111
  else if(PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==0&&PDR0_P06==1&&PDR0_P07==1||PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==0&&PDR0_P05==0&&PDR0_P06==1&&PDR0_P07==1){
  COMR = 239; 
  PPG_Motor(25);
  } //1111011 or 1110011
  else if(PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==0&&PDR0_P07==1||PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==0&&PDR0_P06==0&&PDR0_P07==1){
  COMR = 240;
  PPG_Motor(15);
  } //1111101 or 1111001
  else if(PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==1&&PDR0_P07==0||PDR0_P00==1&&PDR0_P01==1&&PDR0_P02==1&&PDR0_P03==1&&PDR0_P05==1&&PDR0_P06==0&&PDR0_P07==0){
  COMR = 241;
  PPG_Motor(15);
  } //1111110 or 1111100
}

void SearchLine2(void){
  if(PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==0&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==1) 
  COMR = 238; //1110111
  else if(PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==0&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==1||PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==0&&PDR4_P40==0&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==1) 
  COMR = 239; //1101111 or 1100111
  else if(PDR7_P70==1&&PDR7_P71==0&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==1||PDR7_P70==1&&PDR7_P71==0&&PDR7_P72==0&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==1) 
  COMR = 240; //1011111 or 1001111
  else if(PDR7_P70==0&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==1||PDR7_P70==0&&PDR7_P71==0&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==1) 
  COMR = 241; //0111111 or 0011111
  else if(PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==0&&PDR4_P42==1&&PDR4_P43==1||PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==0&&PDR4_P41==0&&PDR4_P42==1&&PDR4_P43==1) 
  COMR = 237; //1111011 or 1110011
  else if(PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==0&&PDR4_P43==1||PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==0&&PDR4_P42==0&&PDR4_P43==1) 
  COMR = 236; //1111101 or 1111001
  else if(PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==1&&PDR4_P43==0||PDR7_P70==1&&PDR7_P71==1&&PDR7_P72==1&&PDR4_P40==1&&PDR4_P41==1&&PDR4_P42==0&&PDR4_P43==0) 
  COMR = 235; //1111110 or 1111100
}

void SearchLine3(void){
  if(g==0) SearchLine1();
  else if(g==1){
    RCR23_RCEN = 0;
    PDR0 = 0x10;
    PDR3 = 0xFF;
    SquareWave_Motor();
    TCR0_TSTR0 = 1;
    PDR0 = 0x00;
    SearchLine2();
    }
}

void main (void){
  
  SYCC = 0x95;  //ClockSpeed=Fch/16, OscWaitTime=Fch/2^17,Fch=12MHz
  
  Delay(100,50);
  
  h=0;
  g=0;
  
  DDR0 = 0x10;
  PDR0 = 0x10;
  DDR3 = 0x90;
  PDR3 = 0xFF;
  PUL3 = 0x6F;
  DDR5 = 0x01;
  
  RCR23 = 0x10;
  RCR24 = 0x06;  //PPG output Freq is 937.5Hz
  
  if(PDR3_P30==1&&PDR3_P31==1&&PDR3_P32==1){
    PPG_Motor(30);
    RCR23_RCEN = 1;
    PDR0 = 0x00;
    }
  if(PDR3_P30==0&&PDR3_P31==1&&PDR3_P32==1){
    SquareWave_Motor();
    TCR0_TSTR0 = 1;
    PDR0 = 0x00;
    }
  if(PDR3_P30==1&&PDR3_P31==0&&PDR3_P32==1){
    PPG_Motor(30);
    RCR23_RCEN = 1;
    PDR0 = 0x00;
    }
  
  PWM_ServoMotor();
  
  TBTC = 0x55;  //Time interval 2^18/Fch, Disable Interrupt,clear interrupt flag
  
  //InitIrqLevels();  //Initialize Interrupt Level Register, see vector.c
  //__EI();  //Enable all Interrupts
  //__set_il(3);  //Enable all IRQ levels
  
  TBTC_TBR = 0;  //Clear the Timebase Timer Counter
  
  while(1){
    if(TBTC_TBOF == 1){
      TBTC_TBOF = 0;  //Clear interrupt flag
      if(h>3000) h=0;
      if(h==1000) g=1;
      if(PDR3_P30==1&&PDR3_P31==1&&PDR3_P32==1) SearchLine1();
      else if(PDR3_P30==0&&PDR3_P31==1&&PDR3_P32==1) SearchLine2();
      else if(PDR3_P30==1&&PDR3_P31==0&&PDR3_P32==1) SearchLine3();
      h=h+1;
    }
    else ;
  }
}