PID算法应用程序

附件1：系统电路原理图

IRd/1D1CPISd1Q1Q/GND *1234567U2141312111098 VSS2Rd/2D2CP2Sd2Q2Q/ +12V?±á÷ê?è? +VIN+ U213Component_1 13-12V?±á÷ê?è? C1680UF5LM2596?a1?μ??1μ÷?ú?￡?é 42FEEDBACKOUTPUT GNDON/OFF LVR?VR33uD1IN5824+5Vê?3? + C2220UF+5V+12141511152461097 X0X1X2X3XY0Y1Y2Y3INHABY3VEECD4052+520k +12V 20k20k20k1234 S1 R4R3R2R12|???a1? 8765 NCVCCGNDNCA/AB/BI/IM7M6M5M4M3M2M1M0S7S6S5S4S3S2S1S0+5+5P7P6P5P4K7NCP0J7J6P0P1P1M7M6M5M4M3M2M1M0P7P6P2P5P4K7J7J6S7S6S5S4S3S2S1S0+5+5P2NCP3P3J0J1J0J1K0K0K1K1K2K2K3K3T0T0T1T1T2T2T3T3T4T4T5T5T6T7T6T7K4K4K5K5B0B0B1B1B2B2B3B3B4B49S12XS/E-P112B5B5B6B6B7B7GND U512345678910 GNDGNDAD15AD14GNDGNDAD13AD12AD11AD10AD9AD8AD7AD6AD5AD4AD3AD2AD1AD0A7A6A5A4A3A2A1A0VRHGNDGND+5AD15AD14AD13AD12AD11AD10AD9AD8AD7AD6AD5AD4AD3AD2AD1AD0A7A6A5A4A3A2A1A0VRHGNDGND+5P1P5GND C3100UFGNDU3C40.1UF 94 ++5V 571012611 IN1IN2IN3IN4L298NVSSVSOUT1OUT2ENAOUT3ENB14ISEN AISEN B 2313OUT4115 D1D2D3D4D5D6D7D8

C5100UFC60.1UFGNDJ11234CON4GND 12345678910 189+5vGND192193P7195P6197P4 1A1B1Y2A2B2YGND U61234567 141312111098 VCC4B4A4Y3B3A3YH7H6H5H4E7E6E5E4E7E6E5E4H7H6H5H4 E0E1E2E3H0H1H2H3E0E1E2E3H0H1H2H3 74LS86òì?ò??

14

附件2：电机控制源程序 子函数： #include \ #include #include \ void GetMotorPulse(void) {

unsigned int nLeftPulse,nRightPulse; unsigned char leftdir,rightdir; //unsigned char ii; leftdir = PORTB & 0x01; //读取转向 rightdir = PORTB & 0x04; nLeftPulse = PACNT; //left为内部计数器，right为外部计数器 nRightPulse = PORTA; //读取脉冲数

PACNT = 0;

PTJ=PTJ|0XFF; Delay(15);

PTJ=PTJ&0X00; //计数器清零 if(leftdir == 0) { LeftMotorPulse -= (int)nLeftPulse; LeftMotorPulseSigma -= (int)nLeftPulse; } else { LeftMotorPulse += (int)nLeftPulse; LeftMotorPulseSigma += (int)nLeftPulse; } if(rightdir == 0) RightMotorSpeed = -(int)nRightPulse; else RightMotorSpeed = (int)nRightPulse; //PACNT = 0; //if(leftdir == 0)

15

g_lPosition = LeftMotorPulse; } void SetMotorVoltage(int nLeftVal) { if(nLeftVal>MOTOR_OUT_MAX) nLeftVal=MOTOR_OUT_MAX; if(nLeftVal 0) { PWMDTY45 = 0; PWMDTY01 = nLeftVal + 120; } else { PWMDTY45 = -nLeftVal + 120; PWMDTY01 = 0; } } void MotorOutput(void) { //计算最终给定的PWM占空比 //int LeftMotorOutput; //float K,P;

//if (PORTB_PB3 == 1) // K = 350; //else K = 700;

LeftMotorOutput = (int)K*PositionControlOut + SpeedControlOut;

if (PORTB_PB4 ==0) { if ((LeftMotorSpeed <= 0)&&(LeftMotorSpeedOld > 0)) { g_ucRight = 1; g_ucLeft = 0; } if ((g_ucRight == 1)&&((GravityAngle > 0)&&(GravityAngle < 60)))

16

{ LeftMotorOutput = 0 ; //SPEED_CONTROL_P = 0; //SPEED_CONTROL_D = 0; } if ((LeftMotorSpeed >= 0)&&(LeftMotorSpeedOld < 0)) { g_ucLeft = 1; g_ucRight = 0; } if ((g_ucLeft == 1)&&((GravityAngle < 0)&&(GravityAngle > -60))) { LeftMotorOutput = 0; } } if (PORTB_PB3 == 0) { if ((LeftMotorSpeed <= 0)&&(LeftMotorSpeedOld > 0)) { g_ucRight = 1; g_ucLeft = 0; } if ((g_ucRight == 1)&&((GravityAngle >=0)&&(GravityAngle <= 180))) LeftMotorOutput = -LeftMotorOutput; if ((LeftMotorSpeed >= 0)&&(LeftMotorSpeedOld < 0)) { g_ucLeft = 1; g_ucRight = 0; } if ((g_ucLeft == 1)&&((GravityAngle <= 0)&&(GravityAngle >= -180))) LeftMotorOutput = -LeftMotorOutput; }

SetMotorVoltage(LeftMotorOutput); }

17