第九届飞思卡尔智能车竞赛电磁组程序

#include <hidef.h>      
#include <MC9S12XS128.h>
//#include "math.h"
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"

signed int PulseCount=0, IdeaSpeed=650;
signed int  AdGet[4]={0},AD[4]={0},adGet[4]={0},ADGet[4], Pre_AdGet[4][6]={0},admin[4]={35,35,35,35},AdGethe;
float ad03;
signed int  ADcha,AdGetcha;
char jia1=0,jiaa1=0,jian1=0,jiann1=0,jia3=0,jiaa3=0,jian3=0,jiann3=0;
unsigned char countPre_scha=1,sflag, bisaiflag=0,startflag=0,countflag=0,stopflag=0;
signed int  stime;
unsigned int startcount=0;
signed int  S3010MidPwm =1500, S3010MaxPwm=1900, S3010MinPwm=1100;  
float S3010Pwm=0;
float ErrorA, PreErrorA,dErrorA,KP_A,KD_A;

signed int  DianJiPwm, DianJiMaxPwm, DianJiMinPwm,DianJiPwmF;
signed int  ErrorM,PreErrorM,d_ErrorM,dd_ErrorM,Pre_d_ErrorM; 
float MotorP,MotorI,MotorD,TempMotor;
signed int HitNum;
//extern float OutData[4];
signed int  zwan;
void chaopin(void)//超频40MHZ
{  
    CLKSEL=0X00;   
    PLLCTL_PLLON=1;   
    SYNR =0xc0 | 0x04;                        
    REFDV=0x80 | 0x01; 
    POSTDIV=0x00;       

    _asm(nop);          
    _asm(nop);
    _asm(nop);
    _asm(nop);
    while(!(CRGFLG_LOCK==1));   
    CLKSEL_PLLSEL =1;                    
}
void PWM_int(void) 
{   
PWMPRCLK = 0X40;    //clockA 不分频,clockA=busclock=16MHz;
//CLK B 16分频:1Mhz  
  PWMSCLA = 0x08;    //对clock SA 16分频,pwm clock=clockA/16=1MHz;     
    PWMSCLB = 0x08; 
   PWMCTL_CON23=1;
   PWMCTL_CON01=1;

    PWMCTL_CON67=1;    //0和1联合成16位PWM；
    PWMCAE_CAE7=0;    //选择输出模式为左对齐输出模式
    PWMCAE_CAE3=0;
    PWMCAE_CAE1=0;
    PWMCNT67 = 0;     //计数器清零
    PWMCNT23 = 0;
    PWMCNT01 = 0;
    PWMPOL_PPOL7=1;    //先输出高电平，计数到DTY时，反转电平
    PWMPOL_PPOL3=1;
    PWMPOL_PPOL1=1;
    
    PWMCLK_PCLK7 = 1;   //选择clock SA做时钟源
    PWMCLK_PCLK3 = 1;
    PWMCLK_PCLK1 = 1;