【正文】 if(fLeft MOTOR_OUT_MIN) fLeft = MOTOR_OUT_MIN。// 左電機(jī)電壓值 成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 30 fRight =g_fAngleControlOut g_fSpeedControlOut+g_fDirectionControlOut。 } /*******************************************************************************/ //電機(jī)輸出值控制 // void MotorOutput(void) //電機(jī)輸出電壓值計(jì)算函數(shù) 5ms { float fLeft, fRight。 if(fRightVal MOTOR_OUT_MAX) fRightVal = MOTOR_OUT_MAX。 if(fLeftVal MOTOR_OUT_MAX) fLeftVal = MOTOR_OUT_MAX。 //MOTOR_OUT_DEAD_VAL。 else if(fLeftVal 0) fLeftVal = MOTOR_OUT_DEAD_VAL。 fRightVal = g_fRightMotorOut。//左正 } else { MotorRun_Behind_Left(Value_fLeftVoltage)//左反 MotorRun_Behind_Right(Value_fRightVoltage)。//左正 MotorRun_Front_Left(Value_fLeftVoltage)。//左正 MotorRun_Front_Left(Value_fLeftVoltage)。//左正 MotorRun_Front_Left(Value_fLeftVoltage)。//左正 MotorRun_Front_Left(Value_fLeftVoltage)。//左正 MotorRun_Front_Left(Value_fLeftVoltage)。//左正 MotorRun_Front_Left(Value_fLeftVoltage)。//左正 MotorRun_Front_Left(Value_fLeftVoltage)。 MotorRun_Front_Left(Value_fLeftVoltage)。 //右正 MotorRun_Front_Right(Value_fRightVoltage)。 //右正 成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 28 MotorRun_Front_Right(Value_fRightVoltage)。 //右正 MotorRun_Front_Right(Value_fRightVoltage)。 //右正 MotorRun_Front_Right(Value_fRightVoltage)。 //右正 MotorRun_Front_Right(Value_fRightVoltage)。 //右正 MotorRun_Front_Right(Value_fRightVoltage)。 //右正 MotorRun_Front_Right(Value_fRightVoltage)。 //右正 MotorRun_Front_Right(Value_fRightVoltage)。 if( fLeftVoltage=0) { // MotorRun_Disable_Left。 } void SetMotorVoltage(float fLeftVoltage, float fRightVoltage) //電機(jī)轉(zhuǎn)向方向判斷并賦值 { Value_fLeftVoltage = fabs(fLeftVoltage)。 g_nLeftMotorPulseSigma += g_nLeftMotorPulse。 if(!MOTOR_LEFT_SPEED_POSITIVE) g_nLeftMotorPulse = g_nLeftMotorPulse。 g_nLeftMotorPulse = (unsigned int)nLeftPulse。 //脈沖值清零 成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 27 nRightPulse=Input_Num。 // void GetMotorPulse(void) //采集脈沖信號(hào) 5ms { nLeftPulse=PACNT。 extern float fLeftVal=0, fRightVal=0。 } 電機(jī) 控制程序： //電機(jī)計(jì)算 MotorControl float g_fLeftMotorOut=0, g_fRightMotorOut=0。 // g_fSpeedControlOut = fValue * (g_nSpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld。 } void SpeedControlOutput(void) //5ms { float fValue。 g_fSpeedControlOutOld = g_fSpeedControlOutNew。 if(g_fSpeedControlIntegral SPEED_CONTROL_OUT_MAX) //判斷積分是否達(dá)到設(shè)定最大最小值 g_fSpeedControlIntegral = SPEED_CONTROL_OUT_MAX。 成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 26 fD=xiangshijian*dd_error* 。 fP = error * sudup。 //fP = fDelta * sudup。 //fP = fDelta * SPEED_CONTROL_P。 pre_error=error。 d_error=errorpre_error。 //CAR_SPEED_SET 速度設(shè)定值 10 //fDelta = g_fCarSpeed。 return。//CAR_SPEED_CONSTANT= //速度歸一化處理 if(g_nSpeedControlFlag == 0) //判斷標(biāo)志位 { g_fSpeedControlOutOld = g_fSpeedControlOutNew = g_fSpeedControlOut = 0。 //電機(jī)速度平均值 g_nLeftMotorPulseSigma = g_nRightMotorPulseSigma = 0。 float error, d_error, dd_error。 float fI。 extern float pre_error, pre_d_error 。 float g_fSpeedControlOutOld=0, g_fSpeedControlOutNew=0,g_fSpeedControlIntegral=0。 g_fAngleControlOut = fValue。 if(fValue ANGLE_CONTROL_OUT_MAX) fValue = ANGLE_CONTROL_OUT_MAX。 } //fValue = (CAR_ANGLE_SET g_fCarAngle) * ANGLE_CONTROL_P +(CAR_ANGLE_SPEED_SET g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D。 if(g_nAngleControlFlag == 0) { g_fAngleControlOut = 0。 g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY。 fDeltaValue = (g_fGravityAngle g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT。 //加速度計(jì)歸一化為角度值 g_fGyroscopeAngleSpeed = (Gyro_Data GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO。 void AngleCalculate(void) //周期為 5ms { double fDeltaValue。 } PITTF_PTF3=1。 } if(cout==3500){ CAR_SPEED_Set1=CAR_SPEED_Set。amp。(cout3000)) { CAR_SPEED_Set1=CAR_SPEED_Set*3/5。 } else if((cout2500)amp。amp。(cout=2020)) { CAR_SPEED_Set1=CAR_SPEED_Set/5。 } else if((cout1500)amp。 cout++。 PITTF_PTF0=1。 成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 22 } else if (jieshou==21|jieshou==5|jieshou==87|jieshou==43) { nMidPosition+=。 } else if (jieshou==17|jieshou==69|jieshou==34|jieshou==68) { nMidPosition=。 } else if (jieshou==103) { CAR_SPEED_Set1=。 } else if(jieshou==51) { CAR_SPEED_Set1+=。 LED1 = ~LED1。 } PITTF_PTF2=1。 // 曝光開(kāi)始 } if(TimerCnt20ms = 20) { TimerCnt20ms = 0。 //根據(jù)曝光時(shí)間計(jì)算 20ms 周期內(nèi)的曝光點(diǎn) integration_piont = 20 IntegrationTime。 unsigned char integration_piont。 if(Input_Num=500) 成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 20 { Input_Num=0。 //清中斷標(biāo)志位 Input_Num++。 //等待站立 CheckCarStand()。 //波形觀察 OutPut_Data()。 //VOLTAGE_LEFT ,VOLTAGE_RIGHT g_fCarAngle OutData[3] = g_fSpeedControlIntegral。 // g_fDirectionControlOut Acc_Data Acc_z_OFF_SET Gyro_DataGyro_OFF_SET OutData[2] =g_fCarSpeed。 OutData[0] =g_fGravityAngle 。 LCD_Print(90,6,右 )。 //加速度 LCD_Print(52,6,中 )。