#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
//子函數聲明
void PORT_Init(void);
void PWM_Init(void) ;
void delay1ms(long a); 
void ECT_Init(void) ;
void Make_Turn(void);
//void delay1us (unsigned int us);
/*********************************************************************************
**********************   子函數定義  *********************************************
*
*
************************************************************************************/
/*********************************************************************************
*函數名:Init_PLL()
*入口函數:無
*出口函數:無
*功能:初使化時鐘頻率,sys clock=48MHz,BUS CLOCK=24MHz
**********************************************************************************/
void PLL_Init(void)
 {
   REFDV=0x01;//initiate the PLL clock
   SYNR=0x02; //Osc 16M,bus clock 24MHz
   while(!(CRGFLG&0x08))
    {}//wait until steady
   CLKSEL=0x80;//select the PLLCLK
   
   }  
/***********************************************************************************
*函數名:Init_PORT() 
*入口參數:無
*出口參數:無
*功能:端口初始化,包括H,K,A,B,其中A和B口用于LCD顯示,H和K口用于紅外傳感器信號輸入
************************************************************************************/   
void PORT_Init(void)
{
 
   DDRK = 0x00;     //八個端口設置為輸入
   PUCR_PUPKE = 1;  //設置上拉電阻
   DDRH = 0x00;     //八個端口設置為輸入
   RDRH = 0xff;     //降功率啟動
   PERH = 0xff;     //設置上拉下拉功能
  // PUCR_PUPAE=1;    //設置A口上拉
  // DDRA      = 0xff;     //A口初始化為輸出
  // PORTA = 0x00;
  // PUCR_PUPBE= 1;    //設置B口上拉
  // DDRB      = 0xff ;     //B口初始化為輸出
  // PORTB     = 0xff;
  // DDRJ = 0xc0;     //J口初始化,DDRJ7設置為輸出,用來驅動LED,DDRJ6設置為輸出,用來驅動SPEAK
                    //DDRJ0設置為輸入,用來驅動KEY1,DDRJ0用來驅動KEY2;
                    //J口默認情況下即為上拉
 }
/*************************************************************************************
*函數名:PWM_Init()
*入口參數:無
*出口參數:無
*功能:初始化PWM
**************************************************************************************/
 void PWM_Init(void) 
{
    PWMCTL_CON45=1;          //四五兩個八位的通道結合成十六位的通道從五通道輸出
    PWMCLK=0x0A;             //通道1選擇時鐘SA,通道3選擇時鐘SB,通道5選擇時鐘B
    PWMPRCLK=0x33;           //十六位輸出選用1/8時鐘頻率,八位輸出選用1/128時鐘頻率
    PWMSCLA=0x08;            // Clock A / (2 * PWMSCLA)
    PWMSCLB=0x08;            // Clock B / (2 * PWMSCLB)
    PWMPOL=0xf7;
    PWMPER45=60000;          //周期為20ms
 // PWMDTY45=4500;           // 為了設定占空比占空比為1.5ms?初始1200
    PWMPER1=100;
    PWMDTY1=40;              //占空比40%
 // PWMPER3=0;               //初始化為即為0;
 // PWMDTY3=0;               //占空比0%
    PWME=0x2A;               //電機1,3通道,舵機5通道使能  
    
}

/*************************************************************************************
*函數名: delay1ms()
*入口參數:無
*出口參數:無
*功能:準確延時
***************************************************************************************/
void delay1ms(long ms)
 {
volatile unsigned int i;
while (ms > 0)
{
i = 1846;
/* ------------------------ */
while (i > 0)
{ 
i = i - 1; 
} 
/* ------------------------ */
ms = ms - 1;
}
 }
/**************************************************************************************
*函數名: Make_Turn()
*入口參數:無
*出口參數:無
*功能: 首先判斷前后排傳感器的狀態,然后由模糊 
*Note:整體上屬于紅外傳感器模塊                   
***************************************************************************************/ 
void Make_Turn(void) 
{
 unsigned int  head_buffer1 = 0;
 int           Hhead;
// int           Bbody;
 int           angle = 0;   
 unsigned int  sensor_h=0;
 unsigned int  sensor_k=0;
 unsigned int  sensor_kh;
 unsigned int  head_buffer=0;
 unsigned int  body_buffer; 
 
 sensor_h = PTH;
 sensor_k = PORTK&0x3f;
 sensor_kh=   PORTK&0x01;
 head_buffer=0;
 body_buffer = (sensor_k>>1)%32;
 head_buffer =  0 + sensor_h ;
 if(sensor_kh)
 head_buffer+=256;
    //將傳感器得到的二進制信號用于小車轉向依據
        switch(head_buffer) 
    {
    case 0b111111111: Hhead =10;  break;  //黑線之外
    case 0b111011111: Hhead = 0;  break;  //處于中線上
    case 0b110011111: Hhead = 0;  break;  //處于中線與左1傳感器上
    case 0b110111111: Hhead = 0;  break;  //處于左1傳感器
    case 0b100111111: Hhead =-8;  break;  //處于左1與左2傳感器上
    case 0b101111111: Hhead =-12; break;  //處于左2傳感器
    case 0b101111110: Hhead =-16; break;  //處于左2與左3傳感器
    case 0b111111110: Hhead =-19; break;  //處于左3傳感器
    case 0b011111110: Hhead =-22; break;  //處于左3與左4傳感器
    case 0b011111111: Hhead =-25; break;  //處于左4傳感器
    case 0b111001111: Hhead = 0;  break;  //處于中線與右1傳感器
    case 0b111101111: Hhead = 0;  break;  //處于右1傳感器
    case 0b111100111: Hhead = 8;  break;  //處于右1與右2傳感器
    case 0b111110111: Hhead =12;  break;  //處于右2傳感器
    case 0b111110011: Hhead =16;  break;  //處于右2與右3傳感器
    case 0b111111011: Hhead =19;  break;  //處于右3傳感器
    case 0b111111001: Hhead =22;  break;  //處于右3與右4傳感器 
    case 0b111111101: Hhead =25;  break;  //處于右4傳感器
    case 0b110001111: Hhead = 9;  break;  //中線,左一,右一同時檢測到黑線,用于十字交叉線的檢測
    default:          break;
    }
   if(Hhead == 10)
    {
      PWMDTY1  = PWMDTY1;
      PWMDTY45 = PWMDTY45;
      delay1ms(1);
    } 
    else if(Hhead >=22 || Hhead <=-22)
     {  
      PWMDTY45 = 4500 + 30 * Hhead;
      PWMDTY1  = 55;
      delay1ms(1);
     } 
    else if(Hhead>=12 || Hhead<=-12) 
     {  
      PWMDTY45 = 4500 + 27 * Hhead;
      PWMDTY1  = 40;
      delay1ms(1);
     } 
   else if(Hhead >=8 || Hhead <=-8)
     {  
      PWMDTY45 = 4500 + 25 * Hhead;
      PWMDTY1  = 50;
      delay1ms(1);
     } 
   else 
   {
    PWMDTY45 = 4500;
    PWMDTY1  = 60;
    delay1ms(1);
   } 
}
void main(void)
{
   PLL_Init();
   PORT_Init();
   PWM_Init();
  // ECT_Init();
   EnableInterrupts;
  for(;;) 
  {
   Make_Turn();
  }

}