PI regulator
Dependencies: PID QEI TextLCD mbed
Fork of PID_VelocityExample by
main.cpp
00001 #include "PID.h" 00002 #include "QEI.h" 00003 #include "TextLCD.h" 00004 00005 00006 00007 #define RATE 0.01 00008 #define Kc 0.473 00009 #define Ti 0.025 00010 #define Td 0.0 00011 00012 TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2); // rs, e, d4-d7 00013 // Motor 00014 PwmOut Motor(p21); 00015 QEI Qei(p29, p30, NC, 504); 00016 PID Controller(Kc, Ti, Td, RATE); 00017 00018 // Definiranje konstanti 00019 00020 00021 Timer t; 00022 int startTimeUs, endTimeUs; 00023 int deltaUs = 0; 00024 int startPulse, endPulse; 00025 int deltaPulse = 0; 00026 int i = 0; 00027 00028 00029 00030 int Pulses = 0; 00031 int PrevPulses = 0; 00032 float PwmDuty = 1.0; 00033 float Velocity = 0.0; 00034 //Velocity to reach. 00035 int goal = 21000; 00036 00037 00038 00039 // Deklaracija funkcija 00040 00041 //Set motors to go "forward". 00042 void initializeMotors(void); 00043 //Set up PID controllers with appropriate limits and biases. 00044 void initializePidControllers(void); 00045 00046 float calcRpm(float deltaTimeSec, int deltaPulse); 00047 00048 float calcRpm(float deltaTimeSec, int deltaPulse) 00049 { 00050 return 60.0 * ((deltaPulse / deltaTimeSec) / 1008 ); 00051 } 00052 00053 00054 void initializeMotors(void) 00055 { 00056 00057 Motor.period_us(50); 00058 Motor = 1.0; 00059 00060 } 00061 00062 void initializePidControllers(void) 00063 { 00064 00065 Controller.setInputLimits(0.0, 54500.0); 00066 Controller.setOutputLimits(0.0, 1.0); 00067 Controller.setBias(1.0); 00068 Controller.setMode(AUTO_MODE); 00069 00070 } 00071 00072 int main() 00073 { 00074 00075 //Initialization. 00076 initializeMotors(); 00077 initializePidControllers(); 00078 //Set velocity set point. 00079 Controller.setSetPoint(goal); 00080 00081 startTimeUs = t.read_us(); 00082 startPulse = Qei.getPulses(); 00083 t.start(); 00084 00085 00086 while (1) { 00087 00088 00089 Pulses = Qei.getPulses(); 00090 Velocity = (Pulses - PrevPulses) / RATE; 00091 PrevPulses = Pulses; 00092 Controller.setProcessValue(Velocity); 00093 PwmDuty = Controller.compute(); 00094 Motor = PwmDuty; 00095 00096 //Ispis brzine na TeraTerm 00097 if(i > 30) { 00098 endTimeUs = t.read_us(); 00099 // endPulse = Qei.getPulses(); 00100 deltaUs = endTimeUs - startTimeUs; 00101 deltaPulse =Pulses - startPulse; 00102 lcd.locate(0,0); 00103 lcd.printf("N = %.0f\n",calcRpm(deltaUs/1000000.0, deltaPulse)); 00104 lcd.printf(" okr/min"); 00105 startPulse = Qei.getPulses(); 00106 startTimeUs = t.read_us(); 00107 i = 0; 00108 } 00109 i = i+1; 00110 wait(RATE); 00111 00112 } 00113 }
Generated on Thu Jul 14 2022 00:36:47 by 1.7.2