PI regulator
Dependencies: PID QEI TextLCD mbed
Fork of PID_VelocityExample by
Revision 2:12efc0d812e1, committed 2015-11-25
- Comitter:
- Tokalic
- Date:
- Wed Nov 25 10:51:33 2015 +0000
- Parent:
- 1:ac598811dd00
- Commit message:
- Mbed PID regulator with 1x16 lcd display
Changed in this revision
TextLCD.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r ac598811dd00 -r 12efc0d812e1 TextLCD.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Wed Nov 25 10:51:33 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/wim/code/TextLCD/#d3496c3ea301
diff -r ac598811dd00 -r 12efc0d812e1 main.cpp --- a/main.cpp Sat Nov 27 11:37:20 2010 +0000 +++ b/main.cpp Wed Nov 25 10:51:33 2015 +0000 @@ -1,104 +1,113 @@ -//****************************************************************************/ -// Includes -//****************************************************************************/ #include "PID.h" #include "QEI.h" +#include "TextLCD.h" -//****************************************************************************/ -// Defines -//****************************************************************************/ + + #define RATE 0.01 -#define Kc -2.6 -#define Ti 0.0 +#define Kc 0.473 +#define Ti 0.025 #define Td 0.0 -//****************************************************************************/ -// Globals -//****************************************************************************/ -//-------- -// Motors -//-------- -//Left motor. -PwmOut leftMotor(p23); -DigitalOut leftBrake(p19); -DigitalOut leftDirection(p28); -QEI leftQei(p29, p30, NC, 624); -PID leftController(Kc, Ti, Td, RATE); -//------- -// Files -//------- -LocalFileSystem local("local"); -FILE* fp; -//-------- -// Timers -//-------- -Timer endTimer; -//-------------------- -// Working variables. -//-------------------- -volatile int leftPulses = 0; -volatile int leftPrevPulses = 0; -volatile float leftPwmDuty = 1.0; -volatile float leftVelocity = 0.0; +TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2); // rs, e, d4-d7 +// Motor +PwmOut Motor(p21); +QEI Qei(p29, p30, NC, 504); +PID Controller(Kc, Ti, Td, RATE); + +// Definiranje konstanti + + +Timer t; +int startTimeUs, endTimeUs; +int deltaUs = 0; +int startPulse, endPulse; +int deltaPulse = 0; +int i = 0; + + + +int Pulses = 0; +int PrevPulses = 0; +float PwmDuty = 1.0; +float Velocity = 0.0; //Velocity to reach. -int goal = 3000; +int goal = 21000; + + -//****************************************************************************/ -// Prototypes -//****************************************************************************/ -//Set motors to go "forward", brake off, not moving. +// Deklaracija funkcija + +//Set motors to go "forward". void initializeMotors(void); //Set up PID controllers with appropriate limits and biases. void initializePidControllers(void); -void initializeMotors(void){ +float calcRpm(float deltaTimeSec, int deltaPulse); + +float calcRpm(float deltaTimeSec, int deltaPulse) +{ + return 60.0 * ((deltaPulse / deltaTimeSec) / 1008 ); +} - leftMotor.period_us(50); - leftMotor = 1.0; - leftBrake = 0.0; - leftDirection = 0; + +void initializeMotors(void) +{ + + Motor.period_us(50); + Motor = 1.0; } -void initializePidControllers(void){ +void initializePidControllers(void) +{ - leftController.setInputLimits(0.0, 10500.0); - leftController.setOutputLimits(0.0, 1.0); - leftController.setBias(1.0); - leftController.setMode(AUTO_MODE); + Controller.setInputLimits(0.0, 54500.0); + Controller.setOutputLimits(0.0, 1.0); + Controller.setBias(1.0); + Controller.setMode(AUTO_MODE); } -int main() { +int main() +{ //Initialization. initializeMotors(); initializePidControllers(); - - //Open results file. - fp = fopen("/local/pidtest.csv", "w"); - - endTimer.start(); - - //Set velocity set point. - leftController.setSetPoint(goal); +//Set velocity set point. + Controller.setSetPoint(goal); - //Run for 3 seconds. - while (endTimer.read() < 3.0){ - leftPulses = leftQei.getPulses(); - leftVelocity = (leftPulses - leftPrevPulses) / RATE; - leftPrevPulses = leftPulses; - leftController.setProcessValue(leftVelocity); - leftPwmDuty = leftController.compute(); - leftMotor = leftPwmDuty; - fprintf(fp, "%f,%f\n", leftVelocity, goal); + startTimeUs = t.read_us(); + startPulse = Qei.getPulses(); + t.start(); + + + while (1) { + + + Pulses = Qei.getPulses(); + Velocity = (Pulses - PrevPulses) / RATE; + PrevPulses = Pulses; + Controller.setProcessValue(Velocity); + PwmDuty = Controller.compute(); + Motor = PwmDuty; + + //Ispis brzine na TeraTerm + if(i > 30) { + endTimeUs = t.read_us(); + // endPulse = Qei.getPulses(); + deltaUs = endTimeUs - startTimeUs; + deltaPulse =Pulses - startPulse; + lcd.locate(0,0); + lcd.printf("N = %.0f\n",calcRpm(deltaUs/1000000.0, deltaPulse)); + lcd.printf(" okr/min"); + startPulse = Qei.getPulses(); + startTimeUs = t.read_us(); + i = 0; + } + i = i+1; wait(RATE); + } - - //Stop motors. - leftMotor = 1.0; - - //Close results file. - fclose(fp); - -} +} \ No newline at end of file