PI regulator
Dependencies: PID QEI TextLCD mbed
Fork of PID_VelocityExample by
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?
User | Revision | Line number | New 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 | } |