大學生電子設(shè)計大賽論文簡易旋轉(zhuǎn)倒立擺及控制裝置(c題)(參考版)
2025-06-21 12:58本頁面
【正文】 } //pragma CODE_SEG DEFAULT //后續(xù)代碼置于默認區(qū)域內(nèi)。 g_ucLeft = 0。 RightMotorSpeedGoal1 = 20。 SPEED_CONTROL_I = 。amp。 LeftMotorPulse = 360。 EnableInterrupts。 if((PORTB_PB1 == 0) || (PORTB_PB3 == 0) || (PORTB_PB4 == 0)) MotorOutput()。 //PositionControl()。 } //SpeedSetOut()。 PositionControl()。 if(PositionControlCount == POSITION_CONTROL_COUNT) { if(RightMotorSpeedGoal RightMotorSpeedGoal1) RightMotorSpeedGoal += 。 GetMotorPulse()。) { //if(PORTB_PB1 == 1) // EnableInterrupts。 //初始化Z信號尚未到來 EnableInterrupts。 XINT_init()。 TIM_init()。 PIT_init()。 float AngleSpeed。 //float fI1 = 0。 float RightMotorSpeedErr0,RightMotorSpeedErr1,RightMotorSpeedErr2。 float LeftMotorSpeed,LeftMotorSpeedOld。 float PositionControlOut,PositionControlNew,PositionControlOld,SpeedControlOut。 //long g_lPositionGoal。 //int g_iZOffset。 unsigned char PositionControlCount。 float K=650。 float SPEED_CONTROL_P,SPEED_CONTROL_I,SPEED_CONTROL_D。 }主函數(shù)： include include include include const float tancode[181]= { , , , , , , , , , , , , , , , }。 fvalue = PositionControlNew PositionControlOld。delay_c++)。 for(delay_c=0。 AngleSpeed = ANGLE_SPEED_RATIO*LeftMotorSpeed。 GravityAngle = ((int)(GRAVITY_ANGLE_RATIO*g_lPosition))%360。 LeftMotorSpeed = LeftMotorPulseSigma。 SpeedControlOut += fP + fI + fD。 fD = SPEED_CONTROL_D*(RightMotorSpeedErr2 2*RightMotorSpeedErr1 + RightMotorSpeedErr0)。 //} fP = SPEED_CONTROL_P*(RightMotorSpeedErr2 RightMotorSpeedErr1)。 //SPEED_CONTROL_I = 0。 SPEED_CONTROL_I = 1。 SPEED_CONTROL_I = 0。amp。 RightMotorSpeedErr2 = RightMotorSpeedGoal RightMotorSpeed。 RightMotorSpeedErr0 = RightMotorSpeedErr1。 //RightMotorSpeed = RightMotorPulseSigma。//ＰＩＤ得到的是角度控制量，加減速由車身的前傾角度決定 } void SpeedControl(void) { float fP,fI,fD。 PositionControlOld = PositionControlNew。 } fD = POSITION_CONTROL_D*AngleSpeed。 // } else { //if(GravityAngle 45) // fP =15。 } //else if(GravityAngle 180) { //if(GravityAngle 315) // fP = 15。 //if(GravityAngle 180) fP = POSITION_CONTROL_P*tancode[GravityAngle 180]。 POSITION_CONTROL_D = 。 } void PositionControl(void) { float fP,fD。(GravityAngle = 180))) LeftMotorOutput = LeftMotorOutput。((GravityAngle = 0)amp。 } if ((g_ucLeft == 1)amp。(LeftMotorSpeedOld 0)) { g_ucLeft = 1。 if ((LeftMotorSpeed = 0)amp。amp。amp。 g_ucLeft = 0。amp。(GravityAngle 60))) { LeftMotorOutput = 0。((GravityAngle 0)amp。 } if ((g_ucLeft == 1)amp。(LeftMotorSpeedOld 0)) { g_ucLeft = 1。 } if ((LeftMotorSpeed = 0)amp。 //SPEED_CONTROL_P = 0。amp。amp。 g_ucLeft = 0。amp。 LeftMotorOutput = (int)K*PositionControlOut + SpeedControlOut。 //if (PORTB_PB3 == 1) // K = 350。 } } void MotorOutput(void) { //計算最終給定的PWM占空比 //int L
教學課件相關(guān)推薦
鄂ICP備17016276號-1