官方代碼拆解

//==============================================================================
//				INPUT GRAVITY ACCELERATION AND GYROSCOPE 
//------------------------------------------------------------------------------
void AngleCalculate(void) {
	float fDeltaValue;
	
	//--------------------------------------------------------------------------
	// Define Angle 
	g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO;
	g_fGyroscopeAngleSpeed = (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;
	
	g_fCarAngle = g_fGyroscopeAngleIntegral;
	fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
	
#if SPEED_CONTROL_METHOD == 0
	g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
#else if SPEED_CONTROL_METHOD == 1
//	g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + g_fAngleControlSpeedFeedback) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
#endif // SPEED_CONTROL_METHOD
}
	
//------------------------------------------------------------------------------
void AngleControl(void) {
	float fValue;

	if(g_nAngleControlFlag == 0) {
		g_fAngleControlOut = 0;
		return;
	}
	
	//--------------------------------------------------------------------------
	fValue = (CAR_ANGLE_SET - g_fCarAngle) * ANGLE_CONTROL_P +
	         (CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D;
	         
	if(fValue > ANGLE_CONTROL_OUT_MAX)			fValue = ANGLE_CONTROL_OUT_MAX;
	else if(fValue < ANGLE_CONTROL_OUT_MIN) 	fValue = ANGLE_CONTROL_OUT_MIN;
	g_fAngleControlOut = fValue;
	
}

void SpeedControl(void) {
	float fP, fDelta;
	float fI;

	
	//--------------------------------------------------------------------------
	g_fCarSpeed = (g_nLeftMotorPulseSigma + g_nRightMotorPulseSigma) / 2;
	g_nLeftMotorPulseSigma = g_nRightMotorPulseSigma = 0;
	g_fCarSpeed *= CAR_SPEED_CONSTANT;

	//--------------------------------------------------------------------------	
	if(g_nSpeedControlFlag == 0) {
		g_fSpeedControlOutOld = g_fSpeedControlOutNew = g_fSpeedControlOut = 0;
		g_fSpeedControlIntegral = 0;
		return;
	}
		
	//--------------------------------------------------------------------------
	fDelta = CAR_SPEED_SET;
	fDelta -= g_fCarSpeed;
	
	fP = fDelta * SPEED_CONTROL_P;
	fI = fDelta * SPEED_CONTROL_I;
	g_fSpeedControlIntegral += fI;
	
	
		
	//--------------------------------------------------------------------------
	
#if SPEED_CONTROL_METHOD == 0
	if(g_fSpeedControlIntegral > SPEED_CONTROL_OUT_MAX)	
		g_fSpeedControlIntegral = SPEED_CONTROL_OUT_MAX;
	if(g_fSpeedControlIntegral < SPEED_CONTROL_OUT_MIN)  	
		g_fSpeedControlIntegral = SPEED_CONTROL_OUT_MIN;
	g_fSpeedControlOutOld = g_fSpeedControlOutNew;

	g_fSpeedControlOutNew = fP + g_fSpeedControlIntegral;
	
#else if SPEED_CONTROL_METHOD == 1
//	g_fSpeedControlOutOld = g_fSpeedControlOutNew;
	
//	g_fAngleControlSpeedFeedbackOld = g_fAngleControlSpeedFeedbackNew;
//	g_fAngleControlSpeedFeedbackNew = g_fSpeedControlIntegral + fP;
#endif // SPEED_CONTROL_METHOD
	
}

//------------------------------------------------------------------------------
//   
void SpeedControlOutput(void) {
	float fValue;
	fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld;
	g_fSpeedControlOut = fValue * (g_nSpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld;
	
#if SPEED_CONTROL_METHOD == 1
//	fValue = g_fAngleControlSpeedFeedbackNew - g_fAngleControlSpeedFeedbackOld;
//	g_fAngleControlSpeedFeedback = fValue * (g_nSpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + 
//					g_fAngleControlSpeedFeedbackOld;
#endif // SPEED_CONTROL_METHOD
}

//------------------------------------------------------------------------------
//						MOTOR SPEED CONTROL
//
// 
void GetMotorPulse(void) {
	unsigned int nLeftPulse, nRightPulse;
	
	COUNTER1_GetNumEvents(&nLeftPulse);
	COUNTER2_GetNumEvents(&nRightPulse);
	COUNTER1_Reset();
	COUNTER2_Reset();
	
	g_nLeftMotorPulse = (int)nLeftPulse;
	g_nRightMotorPulse = (int)nRightPulse;
	if(!MOTOR_LEFT_SPEED_POSITIVE)		g_nLeftMotorPulse = -g_nLeftMotorPulse;
	if(!MOTOR_RIGHT_SPEED_POSITIVE)		g_nRightMotorPulse = -g_nRightMotorPulse;
		
	g_nLeftMotorPulseSigma += g_nLeftMotorPulse;
	g_nRightMotorPulseSigma += g_nRightMotorPulse;
}

//------------------------------------------------------------------------------
void SetMotorVoltage(float fLeftVoltage, float fRightVoltage) {
                                                // Voltage : > 0 : Move forward;
                                                //           < 0 : Move backward
	short nPeriod;
	int nOut;
	
	nPeriod = (short)getReg(PWM_PWMCM);
	
	//--------------------------------------------------------------------------
	if(fLeftVoltage > 1.0) 			fLeftVoltage = 1.0;
	else if(fLeftVoltage < -1.0) 	fLeftVoltage = -1.0;
	
	if(fRightVoltage > 1.0) 		fRightVoltage = 1.0;
	else if(fRightVoltage < -1.0)	fRightVoltage = -1.0;
                                              
	//--------------------------------------------------------------------------	                                            	                                              
	if(fLeftVoltage > 0) {
		setReg(PWM_PWMVAL1, 0);
		nOut = (int)(fLeftVoltage * nPeriod);
		setReg(PWM_PWMVAL0, nOut);
	} else {
		setReg(PWM_PWMVAL0, 0);
		fLeftVoltage = -fLeftVoltage;
		nOut = (int)(fLeftVoltage * nPeriod);
		setReg(PWM_PWMVAL1, nOut);
	}                                     

	//--------------------------------------------------------------------------
	if(fRightVoltage > 0) {
		setReg(PWM_PWMVAL2, 0);
		nOut = (int)(fRightVoltage * nPeriod);
		setReg(PWM_PWMVAL3, nOut);
	} else {
		setReg(PWM_PWMVAL3, 0);
		fRightVoltage = -fRightVoltage;
		nOut = (int)(fRightVoltage * nPeriod);
		setReg(PWM_PWMVAL2, nOut);
	}

	MOTOR_SETLOAD;                              // Reload the PWM value
}                                            


//------------------------------------------------------------------------------
//						MOTOR SPEED CONTROL OUTPUT
// 
void MotorSpeedOut(void) {
	float fLeftVal, fRightVal;
	
	fLeftVal = g_fLeftMotorOut;
	fRightVal = g_fRightMotorOut;
	if(fLeftVal > 0) 			fLeftVal += MOTOR_OUT_DEAD_VAL;
	else if(fLeftVal < 0) 		fLeftVal -= MOTOR_OUT_DEAD_VAL;
	
	if(fRightVal > 0)			fRightVal += MOTOR_OUT_DEAD_VAL;
	else if(fRightVal < 0)		fRightVal -= MOTOR_OUT_DEAD_VAL;
		
	if(fLeftVal > MOTOR_OUT_MAX)	fLeftVal = MOTOR_OUT_MAX;
	if(fLeftVal < MOTOR_OUT_MIN)	fLeftVal = MOTOR_OUT_MIN;
	if(fRightVal > MOTOR_OUT_MAX)	fRightVal = MOTOR_OUT_MAX;
	if(fRightVal < MOTOR_OUT_MIN)	fRightVal = MOTOR_OUT_MIN;
			
	SetMotorVoltage(fLeftVal, fRightVal);
}
		
//------------------------------------------------------------------------------
void MotorOutput(void) {
	float fLeft, fRight;

	fLeft = g_fAngleControlOut - g_fSpeedControlOut - g_fDirectionControlOut;
	fRight = g_fAngleControlOut - g_fSpeedControlOut + g_fDirectionControlOut;
	
	if(fLeft > MOTOR_OUT_MAX)		fLeft = MOTOR_OUT_MAX;
	if(fLeft < MOTOR_OUT_MIN)		fLeft = MOTOR_OUT_MIN;
	if(fRight > MOTOR_OUT_MAX)		fRight = MOTOR_OUT_MAX;
	if(fRight < MOTOR_OUT_MIN)		fRight = MOTOR_OUT_MIN;
		
	g_fLeftMotorOut = fLeft;
	g_fRightMotorOut = fRight;
	MotorSpeedOut();
}

//------------------------------------------------------------------------------
//						GET INPUT VOLTAGE
void GetInputVoltage(void) {
	int i;
	
	AD1_GetValue16((unsigned int *)g_nInputVoltage);
	for(i = 0; i < 6; i ++)
		g_nInputVoltage[i] = (int)(((unsigned int)g_nInputVoltage[i]) >> 4);
}

void SampleInputVoltage(void) {
	unsigned int nInputVoltage[6];
	
	AD1_Measure(0);
	Delay10US(2);
	AD1_GetValue16(nInputVoltage);
	g_lnInputVoltageSigma[0] += nInputVoltage[0];
	g_lnInputVoltageSigma[1] += nInputVoltage[1];
	g_lnInputVoltageSigma[2] += nInputVoltage[2];
	g_lnInputVoltageSigma[3] += nInputVoltage[3];
	g_lnInputVoltageSigma[4] += nInputVoltage[4];
	g_lnInputVoltageSigma[5] += nInputVoltage[5];
	g_nInputVoltageCount ++;	
}

void GetInputVoltageAverage(void) {
	int i;
	
	if(g_nInputVoltageCount == 0) {
		for(i = 0; i < 6; i ++) 
			g_lnInputVoltageSigma[i] = 0;
		return;
	}
	
	g_nInputVoltageCount <<= 4;
	for(i = 0; i < 6; i ++) {
		g_nInputVoltage[i] = (int)(g_lnInputVoltageSigma[i] / g_nInputVoltageCount);
		g_lnInputVoltageSigma[i] = 0;
	}
	
	g_nInputVoltageCount = 0;
}

void DirectionVoltageSigma(void) {
	int nLeft, nRight;
	if(VOLTAGE_LEFT > DIR_LEFT_OFFSET) 		nLeft = VOLTAGE_LEFT - DIR_LEFT_OFFSET;
	else nLeft = 0;
	if(VOLTAGE_RIGHT > DIR_RIGHT_OFFSET) 	nRight = VOLTAGE_RIGHT - DIR_RIGHT_OFFSET;
	else nRight = 0;

	g_fLeftVoltageSigma += nLeft;
	g_fRightVoltageSigma += nRight;
}

//------------------------------------------------------------------------------	
void DirectionControl(void) {
	float fLeftRightAdd, fLeftRightSub, fValue;
	int nLeft, nRight;

	if(g_nDirectionControlFlag == 0) {
		g_fDirectionControlOut = 0;
		g_fDirectionControlOutOld = g_fDirectionControlOutNew = 0;
		return;
	}

	//--------------------------------------------------------------------------
	nLeft = (int)(g_fLeftVoltageSigma /= DIRECTION_CONTROL_COUNT);
	nRight = (int)(g_fRightVoltageSigma /= DIRECTION_CONTROL_COUNT);
	g_fLeftVoltageSigma = 0;
	g_fRightVoltageSigma = 0;
	//--------------------------------------------------------------------------	
/*	if(VOLTAGE_LEFT > DIR_LEFT_OFFSET) 		nLeft = VOLTAGE_LEFT - DIR_LEFT_OFFSET;
	else nLeft = 0;
	if(VOLTAGE_RIGHT > DIR_RIGHT_OFFSET) 	nRight = VOLTAGE_RIGHT - DIR_RIGHT_OFFSET;
	else nRight = 0;
*/	
	fLeftRightAdd = nLeft + nRight;
	fLeftRightSub = nLeft - nRight;
	
	g_fDirectionControlOutOld = g_fDirectionControlOutNew;
	
	if(fLeftRightAdd < LEFT_RIGHT_MINIMUM) {
		g_fDirectionControlOutNew = 0;
	} else {
		fValue = fLeftRightSub * DIR_CONTROL_P / fLeftRightAdd;
		
		if(fValue > 0) fValue += DIRECTION_CONTROL_DEADVALUE;
		if(fValue < 0) fValue -= DIRECTION_CONTROL_DEADVALUE;
		
		if(fValue > DIRECTION_CONTROL_OUT_MAX) fValue = DIRECTION_CONTROL_OUT_MAX;
		if(fValue < DIRECTION_CONTROL_OUT_MIN) fValue = DIRECTION_CONTROL_OUT_MIN;
			g_fDirectionControlOutNew = fValue;
	}
}

//------------------------------------------------------------------------------
void DirectionControlOutput(void) {
	float fValue;
	fValue = g_fDirectionControlOutNew - g_fDirectionControlOutOld;
	g_fDirectionControlOut = fValue * (g_nDirectionControlPeriod + 1) / DIRECTION_CONTROL_PERIOD + g_fDirectionControlOutOld;
	
}

#pragma interrupt called /* Comment this line if the appropriate 'Interrupt preserve registers' property */
                         /* is set to 'yes' (#pragma interrupt saveall is generated before the ISR)      */
void TI1_OnInterrupt(void)
{
	int i;
  /* Write your code here ... */
  	//--------------------------------------------------------------------------
  	WAITTIME_INC;                               // Increase WAITTIME function counter;
  	if(TIME1MS_INT_FLAG) 	return;
  	
  	//--------------------------------------------------------------------------
  	g_n1MSEventCount ++;
	if(g_nTimeTestFlag)		TIMETEST_ON;
		  	
  	//--------------------------------------------------------------------------
  	g_nSpeedControlPeriod ++;
  	SpeedControlOutput();
  	
  	g_nDirectionControlPeriod ++;
  	DirectionControlOutput();
 
 	//--------------------------------------------------------------------------
  	if(g_n1MSEventCount >= CONTROL_PERIOD) {    // Motor speed adjust
  		g_n1MSEventCount = 0;                   // Clear the event counter;
  		
  		GetMotorPulse();
  	} else if(g_n1MSEventCount == 1) {          // Start ADC convert and Car erect adjust
  		if(AD_FLAG) {
			for(i = 0; i < INPUT_VOLTAGE_AVERAGE; i ++) 
				SampleInputVoltage();
	  	}
  	} else if(g_n1MSEventCount == 2) {          // Get the voltage and start calculation
		if(AD_FLAG) GetInputVoltageAverage();

		//----------------------------------------------------------------------
		AngleCalculate();
		AngleControl();		

		//----------------------------------------------------------------------
		MotorOutput();                          // Output motor control voltage;  		
  	} else if(g_n1MSEventCount == 3) {          // Car speed adjust
  		g_nSpeedControlCount ++;
  		if(g_nSpeedControlCount >= SPEED_CONTROL_COUNT) {
  			SpeedControl();
  			g_nSpeedControlCount = 0;
  			g_nSpeedControlPeriod = 0;  			
  		}
  	} else if(g_n1MSEventCount == 4) {          // Car direction control
		g_nDirectionControlCount ++;
		DirectionVoltageSigma();
		if(g_nDirectionControlCount >= DIRECTION_CONTROL_COUNT) {
	  		DirectionControl();
	  		g_nDirectionControlCount = 0;
	  		g_nDirectionControlPeriod = 0;
	  	}
  	}
  	
 	//--------------------------------------------------------------------------
 	if(g_nTimeTestFlag)		TIMETEST_OFF;
}