【文章內(nèi)容簡介】 。 /* input error 32bit range */ if(PIDoutput MAX_16) /* inpur error is greater than 0x00007fff = 32767 32bit range */ InputError = MAX_16。 /* input error = max. positive 16 bit signed value */ else if(PIDoutput MIN_16) /* input error is less than 0xffff7fff = 32768 32bit range */ InputError = MIN_16。 /* input error = min. negative 16 bit signed value */ else InputError = extract_l(PIDoutput)。 /* input error 16bit range */ /**/ /* proportional portion calculation */ ProportionalPortion=L_mult((pParams ProportionalGain), InputError) (pParams ProportionalGainScale + 1)。 /**/ /* integral portion calculation */ IntegralPortion=L_mult((pParamsIntegralGain), InputError) (pParamsIntegralGainScale + 1)。 /* integral portion in step k + integral portion in step k1 */ IntegralPortion=L_add(IntegralPortion, L_deposit_l(pParamsIntegralPortionK_1))。 /* integral portion limitation */ if(IntegralPortion(pParamsPositivePIDLimit)) (pParamsIntegralPortionK_1)=(pParamsPositivePIDLimit)。 else if(IntegralPortionpParamsNegativePIDLimit) pParamsIntegralPortionK_1=pParamsNegativePIDLimit。 else pParamsIntegralPortionK_1=extract_l(IntegralPortion)。 /**/ /* derivative portion calculation */ PIDoutput=L_sub(L_deposit_l(InputError),L_deposit_l(pParamsInputErrorK_1))。 /* [e(k) e(k1)] 32bit range */ pParamsInputErrorK_1=InputError。 /* e(k1) = e(k) */ if(PIDoutputMAX_16) /* [e(k) e(k1)] is greater than 0x00007fff = 32767 32bit range */ InputError=MAX_16。 /* [e(k) e(k1)] = max. positive 16 bit signed value 16 bit range */ else if(PIDoutputMIN_16) /* [e(k) e(k1)] is less than 0xffff7fff = 32768 32bit range */ InputError=MIN_16。 /* [e(k) e(k1)] = min. negative 16 bit signed value 16 bit range */ else InputError=extract_l(PIDoutput)。 /* [e(k) e(k1)] 16bit range */ /* drivative portion in step k integer */ PIDoutput=L_mult((pParamsDerivativeGain),InputError)(pParamsDerivativeGainScale+1)。 /**/ /* controller output calculation */ PIDoutput=L_add(PIDoutput, ProportionalPortion)。 /* derivative portion + proportional portion */ PIDoutput=L_add(PIDoutput, L_deposit_l(pParamsIntegralPortionK_1))。 /* + integral portion = controller output */ /* controller output limitation */ if(PIDoutputpParamsPositivePIDLimit) PIDoutput=pParamsPositivePIDLimit。 else if(PIDoutputpParamsNegativePIDLimit)PIDoutput=pParamsNegativePIDLimit。、初始化程序void RTI_Init(void){ /* setup of the RTI interrupt frequency */ /* adjusted to get 1 millisecond ( ms) with 16 MHz oscillator */ RTICTL = 0x1f。//5*2^16: //0x1f // set RTI prescaler ::晶振16384分頻。(低四位+1)*(2^(高三位+9)) CRGINT = 0x80。 // enable RTI interrupts。 //低四位:17,不可為零,否則分頻器不工作}void SET_PLL(void) {CLKSEL=0x00。PLLCTL=0xe1。SYNR=2。REFDV=1。PLLCTL=0x60。asm NOP。asm NOP。asm NOP。while((CRGFLGamp。0x08)==0)。CLKSEL=0x80。} /*鎖相環(huán)程序*/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=0x90。 //右對齊，無符號數(shù)，單次轉換隊列模式，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)) ATD0CTL5=0x90。 //右對齊，無符號數(shù)，單次轉換隊列模式，ch 1 begin,6 channels。多通道8路，從通道0開始 } /*ATD初始化程序*/ void Servo_Init(void) //舵機 16位PWM控制{ PWME_PWME1=0。 //disable PWM1 servo PWMPRCLK|= 0x03。 //CLKA:8分頻 PWMCLK|=0x00 。 //這就是默認值//通道1用clock A時鐘源