【正文】 el)。 k = ADCvalue(channel)。 if (i j) { mid = i。 i = j。 j = mid。 } if (k = j) { mid = j。 } else { if (k = i) mid = k。 else mid = i。 } return mid。} unsigned int ADCave(uchar n,uchar channel){ uchar i。45 uint ave=0。 for (i=0。 in。 i++) ave += ADCmid(channel)。 ave /= n。 return ave。}/*************************** 車(chē)體及賽道信息采集和處理 ****************************/void GetSensor1(void) { Sensor[1]=ADCave(5,1)。 Sensor[0]=ADCave(5,0)。 }void GetInputVoltageAverage(void){ GetSensor[1]=Sensor[1]。 GetSensor[0]=Sensor[0]。}void AngleAndAngleDotCalculate(void){ float GravityVoltOut。 float SinZ。 float GyroVoltOut。 GetSensor[1]=GetSensor[1]ZeroData。 GetSensor[0]=GetSensor[0]353。 GravityVoltOut=5000*(GetSensor[1]/)。 SinZ=GravityVoltOut/。 if( SinZ1) SinZ=1。46 else if( SinZ1) SinZ=1。 PreAngle=asin(SinZ)*。 GyroVoltOut=5000*(GetSensor[0]/)。 PreAngleDot=GyroVoltOut/。 Kalman_Filter(PreAngle,PreAngleDot)。 }void StandControl(void)//直立控制：角度 PD，角速度 P{ g_fAngleControlOut=125+KP_ANGLE*angle+*KP_ANGLEDOT*angle_dot。 }void MotorOutput(void){ int fLeft, fRight。 fLeft =g_fAngleControlOut+g_fSpeedControlOut+g_fDirectionControlOut。 fRight=g_fAngleControlOut+g_fSpeedControlOutg_fDirectionControlOut。 if(fLeft250) fLeft= if(fLeft0) fLeft=0。 if(fRight250) fRight= if(fRight0) fRight=0。 PWMDTY4=fRight。 PWMDTY5=fLeft。 }void GetMotorPulse(void) { 47 g_nLeftMotorPulseSigma+=PORTA。 PORTB_PB7=1。 _asm(nop)。 _asm(nop)。 PORTB_PB7=0。 _asm(nop)。 _asm(nop)。 g_nRightMotorPulseSigma+=PACNT。 PACNT=0。 }void SpeedControl(void) { float g_fSpeedControlIntegral。 float fDelta。 float fP, fI。 g_fCarSpeed=(g_nLeftMotorPulseSigma + g_nRightMotorPulseSigma) / 。 g_nLeftMotorPulseSigma = g_nRightMotorPulseSigma = 0。 fDelta = CAR_SPEED_SETfDelta g_fCarSpeed。 fP = fDelta 。 fI = fDelta 。 g_fSpeedControlIntegral += fI。 g_fSpeedControlOutOld = g_fSpeedControlOutNew。 g_fSpeedControlOutNew = fP**SPEED_CONTROL_P + g_fSpeedControlIntegral**SPEED_CONTROL_I 。}void SpeedControlOutput(void) { float fValue。 fValue = g_fSpeedControlOutNew g_fSpeedControlOutOld。 g_fSpeedControlOut = fValue * (g_nSpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD +g_fSpeedControlOutOld。48 }void GetSensor2(void){ Sensor[2]=ADCave(5,2)。 Sensor[4]=ADCave(5,4)。 Sensor[6]=ADCave(5,6)275。 Sensor[3]=ADCave(5,3)。 Sensor[5]=ADCave(5,5)。}void DirectionControl(void) //車(chē)模方向控制函數(shù){ float fFrontLeftRightAdd, fFrontLeftRightSub,fFrontValue 。 float fBackLeftRightAdd, fBackLeftRightSub, fBackValue 。 float fValue,fDValue。 float TurnGyroVoltOut。 nFrontLeft = Sensor[2]。 nFrontRight= Sensor[4]。 fFrontLeftRightAdd = nFrontLeft + nFrontRight。 fFrontLeftRightSub = nFrontLeft nFrontRight。 fFrontValue = 1*fFrontLeftRightSub * DIR_CONTROL_FP / fFrontLeftRightAdd。 nBackLeft = Sensor[3]。 nBackRight= Sensor[5]。 fBackLeftRightAdd = nBackLeft + nBackRight。 fBackLeftRightSub = nBackLeft nBackRight。 fBackValue = 1*fBackLeftRightSub * DIR_CONTROL_BP / fBackLeftRightAdd。 fFrontLeftRightSub = nFrontLeft nFrontRight。 fFrontValue = 1*fFrontLeftRightSub * DIR_CONTROL_FP / fFrontLeftRightAdd。 */49 fValue= fFrontValue 。//+ fBackValue。 TurnGyroVoltOut=5000*(Sensor[6]/)。 DIR_CONTROL_D_VALUE=TurnGyroVoltOut/。 fDValue =DIR_CONTROL_D_VALUE*DIR_CONTROL_D*。//DIR_CONTROL_D=???? fValue+=fDValue。 g_fDirectionControlOut = fValue。 } void main(void){ SetBusCLK_80M()。 IO_Init()。 LCD_Init()。 PWM_init()。 AD_Init()。 IOC_Init()。 RTI_Init()。 EnableInterrupts。 while(PTT_PTT3!=0) { display_dongtai_canshu()。 } LCD_Fill(0x00)。 para_delay()。 init_parameter()。 delay_nms(2600)。 KP_ANGLE =parameter[0]。 KP_ANGLEDOT =parameter[1]。 SPEED_CONTROL_P =parameter[2]。 SPEED_CONTROL_I =parameter[3]。 DIR_CONTROL_FP =parameter[4]。50 ZeroData =parameter[5]。 DIR_CONTROL_D =parameter[6]。 CAR_SPEED_SETfDelta=parameter[7]。 for(。) { display_dongtai_canshu()。 } }pragma CODE_SEG __NEAR_SEG NON_BANKEDvoid interrupt 7 RTI_ISR(void){ CRGFLG|=0X80。 TimeCnt1++。 if(TimeCnt1==10) { TimeCnt1=0。 keyscan_F=1。 } TimeCnt++。 g_nSpeedControlPeriod ++。 SpeedControlOutput()。 if(TimeCnt = 5) { TimeCnt = 0。 GetMotorPulse()。 } else if(TimeCnt == 1) { GetSensor1()。 } else if(TimeCnt == 2) {51 GetInputVoltageAverage()。 AngleAndAngleDotCalculate()。 StandControl()。 MotorOutput()。 } else if(TimeCnt == 3) { g_nSpeedControlCount ++。 if(g_nSpeedControlCount = SPEED_CONTROL_COUNT)//SPEED_CONTROL_COUNT=20, { SpeedControl()。 g_nSpeedControlCount = 0。 g_nSpeedControlPeriod = 0。 } } else if(TimeCnt == 4) { GetSensor2()。 DirectionControl()。 } }pragma CODE_SEG DEFAULT