第三届全国大学生智能汽车大赛
#define MAX_32 (signed long)0x7fffffffL #define MIN_32 (signed long)0x80000000L
#define MAX_16 ( signed int)0x7fff #define MIN_16 ( signed int)0x8000
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 DesiredValue, signed int MeasuredValue,sCaiXinBoPID *pParams);
static signed long L_sub(register signed long src_dst, register signed long src2)
15
第三届全国大学生智能汽车大赛
{ }
static signed long L_deposit_l(register signed int ssrc) { }
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) { }
16
return (src2-src_dst);
return (signed long)(ssrc);
return ( signed int)lsrc;
register signed long laccum; laccum=sinp1; laccum*=sinp2;
return laccum;
return (src_dst+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 */ /* 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 */
/*-------------------------------------------------------------------------------------------------*/
17
第三届全国大学生智能汽车大赛
/* proportional portion calculation */
ProportionalPortion=L_mult((pParams -> ProportionalGain), InputError) >> (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); else
if(IntegralPortion
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 */
18
第三届全国大学生智能汽车大赛
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=MIN_16; /* [e(k) - e(k-1)] = min. negative 16 bit signed value - 16 bit range */ else 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 */ /* controller output limitation */ if(PIDoutput>pParams->PositivePIDLimit) PIDoutput=pParams->PositivePIDLimit; else if(PIDoutput 19