【正文】 lude const float tancode[181]= { , , , , , , , , , , , , , , , }。 float K=650。 //int g_iZOffset。 float PositionControlOut,PositionControlNew,PositionControlOld,SpeedControlOut。 float RightMotorSpeedErr0,RightMotorSpeedErr1,RightMotorSpeedErr2。 float AngleSpeed。 TIM_init()。 //初始化Z信號尚未到來 EnableInterrupts。 GetMotorPulse()。 PositionControl()。 //PositionControl()。 EnableInterrupts。amp。 RightMotorSpeedGoal1 = 20。 } //pragma CODE_SEG DEFAULT //后續(xù)代碼置于默認區(qū)域內(nèi)。 g_ucLeft = 0。 SPEED_CONTROL_I = 。 LeftMotorPulse = 360。 if((PORTB_PB1 == 0) || (PORTB_PB3 == 0) || (PORTB_PB4 == 0)) MotorOutput()。 } //SpeedSetOut()。 if(PositionControlCount == POSITION_CONTROL_COUNT) { if(RightMotorSpeedGoal RightMotorSpeedGoal1) RightMotorSpeedGoal += 。) { //if(PORTB_PB1 == 1) // EnableInterrupts。 XINT_init()。 PIT_init()。 //float fI1 = 0。 float LeftMotorSpeed,LeftMotorSpeedOld。 //long g_lPositionGoal。 unsigned char PositionControlCount。 float SPEED_CONTROL_P,SPEED_CONTROL_I,SPEED_CONTROL_D。 fvalue = PositionControlNew PositionControlOld。 for(delay_c=0。 GravityAngle = ((int)(GRAVITY_ANGLE_RATIO*g_lPosition))%360。 SpeedControlOut += fP + fI + fD。 //} fP = SPEED_CONTROL_P*(RightMotorSpeedErr2 RightMotorSpeedErr1)。 SPEED_CONTROL_I = 1。amp。 RightMotorSpeedErr0 = RightMotorSpeedErr1。//ＰＩＤ得到的是角度控制量，加減速由車身的前傾角度決定 } void SpeedControl(void) { float fP,fI,fD。 } fD = POSITION_CONTROL_D*AngleSpeed。 } //else if(GravityAngle 180) { //if(GravityAngle 315) // fP = 15。 POSITION_CONTROL_D = 。(GravityAngle = 180))) LeftMotorOutput = LeftMotorOutput。 } if ((g_ucLeft == 1)amp。 if ((LeftMotorSpeed = 0)amp。amp。amp。((GravityAngle 0)amp。(LeftMotorSpeedOld 0)) { g_ucLeft = 1。 //SPEED_CONTROL_P = 0。amp。amp。 //if (PORTB_PB3 == 1) // K = 350。 } else { PWMDTY45 = nLeftVal + 120。 } void SetMotorVoltage(int nLeftVal) { if(nLeftValMOTOR_OUT_MAX) nLeftVal=MOTOR_OUT_MAX。 } if(rightdir == 0) RightMotorSpeed = (int)nRightPulse。 //計數(shù)器清零 if(leftdir == 0) { LeftMotorPulse = (int)nLeftPulse。 PTJ=PTJ|0XFF。 0x04。 //unsigned char ii。195。GNDVR?VRIN15IN27IN310IN412ENA6ENB11VSS9VS4OUT12OUT23OUT31314OUT4ISEN A1ISEN B15U3L298ND1D2D3D4D5D6D7D8+12VGNDGNDGND+C3100UFC4C6+C5100UF+5V+5V1234J1CON41IRd/21D31CP4ISd51Q61Q/7GND14VSS132Rd/122D112CP102Sd92Q82Q/*Component_1P1P51NC2VCC3GND4NC5A/6A7B/8B9I/10I11892+5v3GND419251936P771958P6919710P4U521B31Y42A52B62Y7GND11A14VCC134B124A114Y103B93A83YU674LS86210。+5V202。202。12V214。202。U2+C1680UFD1IN58