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

#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"

#define DC_Limit 500


#define MID   5500
#define MAXTURN   800
#define AD_LEVEL  170



#define SPEED 70                  //170,97         168,96       160,95
#define DEG   950/12      //    500/13=38.46;       600/13=46.15




volatile unsigned int stop_flag=0;
volatile unsigned int timer_overflow=0;

volatile int NowSpeed;
volatile unsigned int IC0Counter=0;			  //2.083ms one int
volatile unsigned int IC0Counter_1=0;			  //
    
volatile unsigned int GearWidth[4]={0} ;
volatile unsigned int  GearIndex=0;

    unsigned long GearWidthSum;
    unsigned long AveGearWidth ;
    unsigned long AveSpeed ;
    unsigned int pulse_counter=0; //計數   
   int Sensor;

sCaiXinBoPID SpdPID;
int DCDuty;
int Object_Speed=SPEED;		//100cm/s

int time_up=0;

int atd_value[14]; //量化為0-9的數組


 unsigned int AD_Value[14];

void ATD_Init(void)
  {   ATD0CTL2=0xc0;//不開中斷2;  ////上電，標志位快速清零	...	 ATD隊列轉換完成中斷使能
      ATD0CTL3=0x00;//sequence length:8   0;	 //轉換隊列長度8，非FIFO存儲模式，
      ATD0CTL4=0x85;  //8位精度，AD時鐘為bus_clk/(2*(5+1))
      //ATD0CTL5=0x91; //右對齊，無符號數，單次轉換隊列模式，ch 1 begin,6 channels;多通道8路，從通道0開始
  
        ATD1CTL2=0xc0;//ADPU,fast flag clear
        ATD1CTL3=0x30;//sequence length:6,not fifo mode,
        ATD1CTL4=0x85;//8 bit decision,ATD clock:bus_clk/(2*(5+1))
  }
  
  
  
  
 //***************Servo_Init()***************
void Servo_Init(void)      //舵機 16位PWM控制
{
         PWME_PWME1=0;     //disable PWM1  servo 
         PWMPRCLK|= 0x03;  //CLKA:8分頻
         PWMCLK=0x00 ;    //這就是默認值//通道1用clock A時鐘源
                           //24MHZ/8=3M                         
         PWMPOL_PPOL1 = 1; //先高電平
         
         PWMCAE=0X00;			//對齊方式默認 左對齊
         
         PWMCTL_CON01 = 1; //通道1:16BIT 連接
         PWMPER01  = 60000;//60000:20ms;;;20000//舵機的頻率是150Hz	   1/3M * x=1/150Hz
                           //1ms 脈沖 到2ms 脈沖
                           //1.5ms脈沖對應0度
         PWMDTY01 =4500;//4500:1.5ms;	 //控制3500左極限 5500右極限  4500正中央
         PWMCNT01  = 0;		 //寫計數寄存器,會使PWMDTYx,PWMPERx進入鎖存器,同時使能通道時會從此值