///////////////////////////////////////////////////////////////////
// 16位累加器測速程序(PACB累加器),PT0口輸入脈沖
//每次記錄脈沖時間可修改宏:TIME_INTER
//author:whut_wj
///////////////////////////////////////////////////////////////////
#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#include"pid.h"
#include"shache.h"
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"

#define TIME_INTER 1000  //1000個ms=1s
unsigned int Get_pulse;

#define direction 2940
#define speed 199
#define test 0
unsigned char jishu;
void Start_PLL(void)   //busCLK=32M
{
  REFDV=0x01;          // PLLCLK =2*OSCCLK*(SYNR + 1)/(REFDV + 1)
  SYNR=0x03;	         //        =2*16M*3/2=64M
  asm{
    BRCLR CRGFLG,#$08,*
    BSET CLKSEL,#$80
    }													  
} 



void main(void) 
{
  DDRB=0X00;
  DDRA=0X00;
  DisableInterrupts;
  Start_PLL();
  PWME=0x00;      //禁止所有PWM輸出
  PWMCTL=0x30;     //通道0.1級聯 通道2。3級聯
  PWMPRCLK=0x04;   //ClockA=Bus/4=2MHZ;ClockB=Bus/1=8MHZ
  PWMCLK=0x00;     //時鐘來源選擇ClockSA，ClockSB
  PWMSCLA=0x7D;    //PWMSCLA=125  ClockSA=8kHZ
  PWMSCLB=0X01;    //PWMSCLB=1    ClockSB=2MHZ
  PWMPOL=0XFF;     //輸出為高電平
  PWMCAE=0X00;     //左對齊方式                                  
  PWMPER01=39999;    //39輸出舵機周期=(1/8kHZ)*(159+1)=20ms
  PWMDTY01=2980;    //占空比=[(PWMDTY0+1)/(PWMPER0+1)]=3/40,占空1.5ms
  PWMPER23=199;    //49輸出電機周期=8M/(199+1)=40KHZ
  PWMDTY23=199;    //占空比=[(159+1)/(199+1)]=0.8    
  PWME=0x0a;       //開啟PWM通道0.1和通道2 
  EnableInterrupts;
  
  for(;;)
   {
     jishu=PORTB;
     //A方案          
     switch(jishu) 
    {
      case 0x80:PWMDTY01=direction-600;PWMDTY23=speed-21;break;    // c00   
      case 0xC0:PWMDTY01=direction-560;PWMDTY23=speed-18;break;    // 600   
      case 0x40:PWMDTY01=direction-495;PWMDTY23=speed-24;break;     // 400   
      case 0x60:PWMDTY01=direction-395;PWMDTY23=speed-16;break;  // 200   
      case 0x20:PWMDTY01=direction-285;PWMDTY23=speed-19;break; // 300   
      case 0x30:PWMDTY01=direction-120;PWMDTY23=speed-25;break;  // 100   
      case 0x10:PWMDTY01=direction-50;PWMDTY23=speed-17;break;    
      case 0x18:PWMDTY01=direction+0;PWMDTY23=speed-18;break;     // 180  0
      case 0x08:PWMDTY01=direction+50;PWMDTY23=speed-17;break;    // 080
      case 0x0c:PWMDTY01=direction+120;PWMDTY23=speed-25;break;    // 0c0
      case 0x04:PWMDTY01=direction+280;PWMDTY23=speed-19;break;    // 040
      case 0x06:PWMDTY01=direction+380;PWMDTY23=speed-16;break;    // 060
      case 0x02:PWMDTY01=direction+480;PWMDTY23=speed-24;break;    // 020
      case 0x03:PWMDTY01=direction+535;PWMDTY23=speed-18;break;    // 010  
      case 0x01:PWMDTY01=direction+585;PWMDTY23=speed-21;break;    
      
      default:break;
    } 
    
    
     //B方案
  /* switch(jishu) 
    {
      case 0x80:PWMDTY01=direction-600;PWMDTY23=speed-19;break;    // c00   
      case 0xC0:PWMDTY01=direction-560;PWMDTY23=speed-23;break;    // 600   
      case 0x40:PWMDTY01=direction-495;PWMDTY23=speed-18;break;     // 400   
      case 0x60:PWMDTY01=direction-395;PWMDTY23=speed-16;break;  // 200   
      case 0x20:PWMDTY01=direction-285;PWMDTY23=speed-25;break; // 300   
      case 0x30:PWMDTY01=direction-130;PWMDTY23=speed-20;break;  // 100   
      case 0x10:PWMDTY01=direction-70;PWMDTY23=speed-16;break;    
      case 0x18:PWMDTY01=direction+0;PWMDTY23=speed-20;break;     // 180  0
      case 0x08:PWMDTY01=direction+70;PWMDTY23=speed-16;break;    // 080
      case 0x0c:PWMDTY01=direction+130;PWMDTY23=speed-20;break;    // 0c0
      case 0x04:PWMDTY01=direction+280;PWMDTY23=speed-24;break;    // 040
      case 0x06:PWMDTY01=direction+380;PWMDTY23=speed-16;break;    // 060
      case 0x02:PWMDTY01=direction+480;PWMDTY23=speed-18;break;    // 020
      case 0x03:PWMDTY01=direction+535;PWMDTY23=speed-23;break;    // 010  
      case 0x01:PWMDTY01=direction+585;PWMDTY23=speed-19;break;     
      default:break;
    }*/     
    
    
    
    
   } 
}