Zeljko Tokalic / Mbed 2 deprecated PID_DIPLOMSKI_ZeljkoTokalic2015

PI regulator

Dependencies:   PID QEI TextLCD mbed

Fork of PID_VelocityExample by Aaron Berk

main.cpp@2:12efc0d812e1, 2015-11-25 (annotated)

Committer:
Tokalic
Date:
Wed Nov 25 10:51:33 2015 +0000
Revision:
2:12efc0d812e1
Parent:
1:ac598811dd00 
Mbed PID regulator with 1x16 lcd display

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:9bca35ae9c6b 1 #include "PID.h"
aberk 0:9bca35ae9c6b 2 #include "QEI.h"
Tokalic 2:12efc0d812e1 3 #include "TextLCD.h"
aberk 0:9bca35ae9c6b 4
Tokalic 2:12efc0d812e1 5
Tokalic 2:12efc0d812e1 6
aberk 0:9bca35ae9c6b 7 #define RATE 0.01
Tokalic 2:12efc0d812e1 8 #define Kc 0.473
Tokalic 2:12efc0d812e1 9 #define Ti 0.025
aberk 0:9bca35ae9c6b 10 #define Td 0.0
aberk 0:9bca35ae9c6b 11
Tokalic 2:12efc0d812e1 12 TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2); // rs, e, d4-d7
Tokalic 2:12efc0d812e1 13 // Motor
Tokalic 2:12efc0d812e1 14 PwmOut Motor(p21);
Tokalic 2:12efc0d812e1 15 QEI Qei(p29, p30, NC, 504);
Tokalic 2:12efc0d812e1 16 PID Controller(Kc, Ti, Td, RATE);
Tokalic 2:12efc0d812e1 17
Tokalic 2:12efc0d812e1 18 // Definiranje konstanti
Tokalic 2:12efc0d812e1 19
Tokalic 2:12efc0d812e1 20
Tokalic 2:12efc0d812e1 21 Timer t;
Tokalic 2:12efc0d812e1 22 int startTimeUs, endTimeUs;
Tokalic 2:12efc0d812e1 23 int deltaUs = 0;
Tokalic 2:12efc0d812e1 24 int startPulse, endPulse;
Tokalic 2:12efc0d812e1 25 int deltaPulse = 0;
Tokalic 2:12efc0d812e1 26 int i = 0;
Tokalic 2:12efc0d812e1 27
Tokalic 2:12efc0d812e1 28
Tokalic 2:12efc0d812e1 29
Tokalic 2:12efc0d812e1 30 int Pulses = 0;
Tokalic 2:12efc0d812e1 31 int PrevPulses = 0;
Tokalic 2:12efc0d812e1 32 float PwmDuty = 1.0;
Tokalic 2:12efc0d812e1 33 float Velocity = 0.0;
aberk 0:9bca35ae9c6b 34 //Velocity to reach.
Tokalic 2:12efc0d812e1 35 int goal = 21000;
Tokalic 2:12efc0d812e1 36
Tokalic 2:12efc0d812e1 37
aberk 0:9bca35ae9c6b 38
Tokalic 2:12efc0d812e1 39 // Deklaracija funkcija
Tokalic 2:12efc0d812e1 40
Tokalic 2:12efc0d812e1 41 //Set motors to go "forward".
aberk 0:9bca35ae9c6b 42 void initializeMotors(void);
aberk 0:9bca35ae9c6b 43 //Set up PID controllers with appropriate limits and biases.
aberk 0:9bca35ae9c6b 44 void initializePidControllers(void);
aberk 0:9bca35ae9c6b 45
Tokalic 2:12efc0d812e1 46 float calcRpm(float deltaTimeSec, int deltaPulse);
Tokalic 2:12efc0d812e1 47
Tokalic 2:12efc0d812e1 48 float calcRpm(float deltaTimeSec, int deltaPulse)
Tokalic 2:12efc0d812e1 49 {
Tokalic 2:12efc0d812e1 50 return 60.0 * ((deltaPulse / deltaTimeSec) / 1008 );
Tokalic 2:12efc0d812e1 51 }
aberk 0:9bca35ae9c6b 52
Tokalic 2:12efc0d812e1 53
Tokalic 2:12efc0d812e1 54 void initializeMotors(void)
Tokalic 2:12efc0d812e1 55 {
Tokalic 2:12efc0d812e1 56
Tokalic 2:12efc0d812e1 57 Motor.period_us(50);
Tokalic 2:12efc0d812e1 58 Motor = 1.0;
aberk 0:9bca35ae9c6b 59
aberk 0:9bca35ae9c6b 60 }
aberk 0:9bca35ae9c6b 61
Tokalic 2:12efc0d812e1 62 void initializePidControllers(void)
Tokalic 2:12efc0d812e1 63 {
aberk 0:9bca35ae9c6b 64
Tokalic 2:12efc0d812e1 65 Controller.setInputLimits(0.0, 54500.0);
Tokalic 2:12efc0d812e1 66 Controller.setOutputLimits(0.0, 1.0);
Tokalic 2:12efc0d812e1 67 Controller.setBias(1.0);
Tokalic 2:12efc0d812e1 68 Controller.setMode(AUTO_MODE);
aberk 0:9bca35ae9c6b 69
aberk 0:9bca35ae9c6b 70 }
aberk 0:9bca35ae9c6b 71
Tokalic 2:12efc0d812e1 72 int main()
Tokalic 2:12efc0d812e1 73 {
aberk 0:9bca35ae9c6b 74
aberk 0:9bca35ae9c6b 75 //Initialization.
aberk 0:9bca35ae9c6b 76 initializeMotors();
aberk 0:9bca35ae9c6b 77 initializePidControllers();
Tokalic 2:12efc0d812e1 78 //Set velocity set point.
Tokalic 2:12efc0d812e1 79 Controller.setSetPoint(goal);
aberk 0:9bca35ae9c6b 80
Tokalic 2:12efc0d812e1 81 startTimeUs = t.read_us();
Tokalic 2:12efc0d812e1 82 startPulse = Qei.getPulses();
Tokalic 2:12efc0d812e1 83 t.start();
Tokalic 2:12efc0d812e1 84
Tokalic 2:12efc0d812e1 85
Tokalic 2:12efc0d812e1 86 while (1) {
Tokalic 2:12efc0d812e1 87
Tokalic 2:12efc0d812e1 88
Tokalic 2:12efc0d812e1 89 Pulses = Qei.getPulses();
Tokalic 2:12efc0d812e1 90 Velocity = (Pulses - PrevPulses) / RATE;
Tokalic 2:12efc0d812e1 91 PrevPulses = Pulses;
Tokalic 2:12efc0d812e1 92 Controller.setProcessValue(Velocity);
Tokalic 2:12efc0d812e1 93 PwmDuty = Controller.compute();
Tokalic 2:12efc0d812e1 94 Motor = PwmDuty;
Tokalic 2:12efc0d812e1 95
Tokalic 2:12efc0d812e1 96 //Ispis brzine na TeraTerm
Tokalic 2:12efc0d812e1 97 if(i > 30) {
Tokalic 2:12efc0d812e1 98 endTimeUs = t.read_us();
Tokalic 2:12efc0d812e1 99 // endPulse = Qei.getPulses();
Tokalic 2:12efc0d812e1 100 deltaUs = endTimeUs - startTimeUs;
Tokalic 2:12efc0d812e1 101 deltaPulse =Pulses - startPulse;
Tokalic 2:12efc0d812e1 102 lcd.locate(0,0);
Tokalic 2:12efc0d812e1 103 lcd.printf("N = %.0f\n",calcRpm(deltaUs/1000000.0, deltaPulse));
Tokalic 2:12efc0d812e1 104 lcd.printf(" okr/min");
Tokalic 2:12efc0d812e1 105 startPulse = Qei.getPulses();
Tokalic 2:12efc0d812e1 106 startTimeUs = t.read_us();
Tokalic 2:12efc0d812e1 107 i = 0;
Tokalic 2:12efc0d812e1 108 }
Tokalic 2:12efc0d812e1 109 i = i+1;
aberk 0:9bca35ae9c6b 110 wait(RATE);
Tokalic 2:12efc0d812e1 111
aberk 0:9bca35ae9c6b 112 }
Tokalic 2:12efc0d812e1 113 }