/**********************************************************************

 * 文 件 名：pwm.c
 
 * 功    能：脈沖輸出模塊，控制電機速度和舵機轉向
 
 * 日    期：2007年4月26日
 
 **********************************************************************/
#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */ 
#define Mid_PWM 2932
#define Right_Max 3465
#define Left_Max 2399
//PWM初始化
//設置舵機頻率為50HZ,PWM7輸出；設置電機頻率為1kHZ，PWM0 1輸出
void InitPWM(void)
{  
    PWME = 0; //關閉所有通道
    PWMCTL_CON01 = 1;//0,1通道級聯組合成一個16bit通道
    PWMPOL = 0xff;//設置0 1 67通道為正極性方波
    PWMCAE = 0;//設置0 1 67通道為左對齊方式
    PWMCLK = 0xc2;//SA作為0 1通道的時鐘源，SB作為67通道的時鐘源
    
    //PWMSCLA = 80;//CLOCK SA = CLOCK A /(2*80)=200K   32M
    PWMSCLB = 100;//CLOCK SA = CLOCK A /(2*100)=200K    40M
    PWMPER6 = 200;//6通道周期計數器設置         周期1ms
    PWMPER7= 200;//7通道周期計數器設置         周期1ms    
    
    //PWMSCLB = 8;//CLOCK SB = CLOCK B /(2*8)=2M           32M
    PWMSCLA = 10;//CLOCK SB = CLOCK B /(2*10)=2M            40M
    //40000
    PWMPER0 = 0x9C;
    PWMPER1 = 0x40;//01通道周期計數器設置       周期20ms
   
    PWMDTY6 = 0;//6通道占空比初始化，速度為0     低電平
    PWMDTY7 = 0;//7通道占空比初始化，速度為0     低電平
    
    // 3000
    PWMDTY0 = 0x0B;
    PWMDTY1 = 0xB8;//0 1通道占空比初始化，角度為正前方90度    1.5ms高電平
    
    PWME = 0xc2;//打開0 1 67通道，驅動電機和舵機使能  
}

//設置舵機，a為0到180度的角度，小車正前方為90度
void SetAngle(signed char a)
{
    word Dutycycle,Highbyte;
    if(a >= 30) a = 30;
    if(a <= -30) a = -30;
    Dutycycle = a*800/45+2932;//-30度-30度
    Highbyte = Dutycycle & 0xFF00;
    PWMDTY0 = Highbyte>>8;
    PWMDTY1 = Dutycycle&0x00FF;
}

void SetDutycycle(word temp_Duty)
  {    word High;
       if(temp_Duty>Right_Max) temp_Duty=Right_Max;
       if(temp_Duty<Left_Max) temp_Duty=Left_Max;
       High = temp_Duty & 0xFF00;
       PWMDTY0 = High>>8;
       PWMDTY1 = temp_Duty&0x00FF;
  }
//設置電機速度
//1通道正轉，0通道反轉，v為0-200的整數，v/200為1通道的占空比
void SetSpeed(byte v)
{    
    static byte tempspeed = 0;     //靜態局部變量
    PWMDTY7 = 0;
    if (tempspeed < v)
    {
        for (;tempspeed <= v; tempspeed+=5)
            PWMDTY6 = tempspeed;
    }
    else
    {
        tempspeed = v;
        PWMDTY6 = tempspeed;
    }
    return ;
}

//反轉，0通道反轉
void SetRevspeed(byte v)
{
    PWMDTY7 = v;
    PWMDTY6 = 0;
}