智能车程序(4)

第三届全国大学生智能汽车大赛

typedef struct {

signed int ProportionalGain; signed int ProportionalGainScale; signed int IntegralGain; signed int IntegralGainScale; signed int DerivativeGain; signed int DerivativeGainScale; signed int PositivePIDLimit; signed int NegativePIDLimit; signed int IntegralPortionK_1; signed int InputErrorK_1; }sCaiXinBoPID; sCaiXinBoPID SpdPID;

extern signed int CaiXinBoPIDController( signed

int

MeasuredValue,sCaiXinBoPID *pParams);

static signed long L_sub(register signed long src_dst, register signed { return (src2-src_dst);

}

static signed long L_deposit_l(register signed int ssrc) { return (signed long)(ssrc); }

15

DesiredValue,

long src2) signed

int

第三届全国大学生智能汽车大赛

static signed int extract_l(register signed long lsrc) { }

static signed long L_mult(register signed int sinp1, register signed int sinp2) { }

static signed long L_add(register signed long src_dst, register signed long src2) { }

signed int CaiXinBoPIDController( signed int DesiredValue, signed int MeasuredValue,sCaiXinBoPID *pParams) {

signed long ProportionalPortion, IntegralPortion, PIDoutput; signed int InputError;

/*-------------------------------------------------------------------------------------------------*/ /* Saturation mode must be set */

return (src_dst+src2); register signed long laccum; laccum=sinp1; laccum*=sinp2;

return laccum; return ( signed int)lsrc;

16

第三届全国大学生智能汽车大赛

/* InputError = sub(DesiredValue, MeasuredValue); */ /* input error */

/*-------------------------------------------------------------------------------------------------*/ /* input error calculation - 16bit range, with and without saturation mode */

PIDoutput = L_sub(L_deposit_l(DesiredValue),L_deposit_l(MeasuredValue)); /* 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 ProportionalGainScale + 1);

/*-------------------------------------------------------------------------------------------------*/ /* integral portion calculation */

IntegralPortion=L_mult((pParams->IntegralGain), InputError) >> (pParams->IntegralGainScale + 1);

/* integral portion in step k + integral portion in step k-1 */

IntegralPortion=L_add(IntegralPortion, L_deposit_l(pParams->IntegralPortionK_1));

/* integral portion limitation */

if(IntegralPortion>(pParams->PositivePIDLimit))

(pParams->IntegralPortionK_1)=(pParams->PositivePIDLimit);

->

ProportionalGain),

InputError)

>>

(pParams

->

17

第三届全国大学生智能汽车大赛

else

if(IntegralPortionNegativePIDLimit)

pParams->IntegralPortionK_1=pParams->NegativePIDLimit; else

pParams->IntegralPortionK_1=extract_l(IntegralPortion);

/*-------------------------------------------------------------------------------------------------*/ /* derivative portion calculation */

PIDoutput=L_sub(L_deposit_l(InputError),L_deposit_l(pParams->InputErrorK_1)); /* [e(k) - e(k-1)] - 32bit range */

pParams->InputErrorK_1=InputError; /* e(k-1) = e(k) */

if(PIDoutput>MAX_16) /* [e(k) - e(k-1)] is greater than 0x00007fff = 32767 - 32bit range */

InputError=MAX_16; /* [e(k) - e(k-1)] = max. positive 16 bit signed value - 16 bit range */ else

if(PIDoutput

InputError=extract_l(PIDoutput); /* [e(k) - e(k-1)] - 16bit range */

/* drivative portion in step k - integer */

PIDoutput=L_mult((pParams->DerivativeGain),InputError)>>(pParams->DerivativeGainScale+1);

/*-------------------------------------------------------------------------------------------------*/ /* controller output calculation */

PIDoutput=L_add(PIDoutput, ProportionalPortion); /* derivative portion + proportional portion */

PIDoutput=L_add(PIDoutput, L_deposit_l(pParams->IntegralPortionK_1)); /* + integral portion = controller output */

18

第三届全国大学生智能汽车大赛

/* controller output limitation */

if(PIDoutput>pParams->PositivePIDLimit) PIDoutput=pParams->PositivePIDLimit; else

if(PIDoutputNegativePIDLimit)

PIDoutput=pParams->NegativePIDLimit;、

初始化程序

void RTI_Init(void) {

/* setup of the RTI interrupt frequency */

/* adjusted to get 1 millisecond (1.024 ms) with 16 MHz oscillator */

RTICTL = 0x1f;//5*2^16:48.8hz //0x1f // set RTI prescaler ::晶振16384分频;(低四位+1)*(2^(高三位+9))

CRGINT = 0x80; // enable RTI interrupts; //低四位:1-7,不可为零,否则分频器不工作 }

void SET_PLL(void) {

CLKSEL=0x00; PLLCTL=0xe1; SYNR=2; REFDV=1; PLLCTL=0x60; asm NOP;

19

百度搜索“77cn”或“免费范文网”即可找到本站免费阅读全部范文。收藏本站方便下次阅读，免费范文网，提供经典小说综合文库智能车程序(4)在线全文阅读。