Rob Andon
/
EECKTclassicalcontrol
Classical Control
main.cpp@0:031c505c5d69, 2013-11-20 (annotated)
- Committer:
- RANDON
- Date:
- Wed Nov 20 16:40:47 2013 +0000
- Revision:
- 0:031c505c5d69
Classic
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RANDON | 0:031c505c5d69 | 1 | #include "mbed.h" |
RANDON | 0:031c505c5d69 | 2 | |
RANDON | 0:031c505c5d69 | 3 | Timer t; |
RANDON | 0:031c505c5d69 | 4 | |
RANDON | 0:031c505c5d69 | 5 | AnalogIn ain_v1(p20); |
RANDON | 0:031c505c5d69 | 6 | AnalogIn ain_v2(p19); |
RANDON | 0:031c505c5d69 | 7 | AnalogOut aout(p18); |
RANDON | 0:031c505c5d69 | 8 | |
RANDON | 0:031c505c5d69 | 9 | int main() |
RANDON | 0:031c505c5d69 | 10 | { |
RANDON | 0:031c505c5d69 | 11 | float t1, t2; |
RANDON | 0:031c505c5d69 | 12 | float V1, V1scaled; |
RANDON | 0:031c505c5d69 | 13 | float V2, V2scaled; |
RANDON | 0:031c505c5d69 | 14 | float Cmbed; |
RANDON | 0:031c505c5d69 | 15 | |
RANDON | 0:031c505c5d69 | 16 | float Cout, Cout1, Cout2; |
RANDON | 0:031c505c5d69 | 17 | float e, e1, e2; |
RANDON | 0:031c505c5d69 | 18 | float V2Ref; |
RANDON | 0:031c505c5d69 | 19 | float controlfreq, delaytime; |
RANDON | 0:031c505c5d69 | 20 | |
RANDON | 0:031c505c5d69 | 21 | // Initialize variables |
RANDON | 0:031c505c5d69 | 22 | Cout1 = 0.0; |
RANDON | 0:031c505c5d69 | 23 | Cout2 = 0.0; |
RANDON | 0:031c505c5d69 | 24 | e1 = 0.0; |
RANDON | 0:031c505c5d69 | 25 | e2 = 0.0; |
RANDON | 0:031c505c5d69 | 26 | |
RANDON | 0:031c505c5d69 | 27 | // Starts timer |
RANDON | 0:031c505c5d69 | 28 | t.start(); |
RANDON | 0:031c505c5d69 | 29 | while(t.read()<10.0) |
RANDON | 0:031c505c5d69 | 30 | { |
RANDON | 0:031c505c5d69 | 31 | |
RANDON | 0:031c505c5d69 | 32 | t1 = t.read(); // Gets elapsed time from when t.start() was done. |
RANDON | 0:031c505c5d69 | 33 | |
RANDON | 0:031c505c5d69 | 34 | // Sense |
RANDON | 0:031c505c5d69 | 35 | V1scaled = ain_v1.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. / Multiple by 3.3 to get voltage. |
RANDON | 0:031c505c5d69 | 36 | V1 = (V1scaled-1.65)*6.06; // Re-calculate actual V1(t) voltage. |
RANDON | 0:031c505c5d69 | 37 | |
RANDON | 0:031c505c5d69 | 38 | V2scaled = ain_v2.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. Multiple by 3.3 to get voltage. |
RANDON | 0:031c505c5d69 | 39 | V2 = (V2scaled-1.65)*6.06; // Re-calculate actual V1(t) voltage. |
RANDON | 0:031c505c5d69 | 40 | |
RANDON | 0:031c505c5d69 | 41 | // Decide |
RANDON | 0:031c505c5d69 | 42 | |
RANDON | 0:031c505c5d69 | 43 | // Reference voltage |
RANDON | 0:031c505c5d69 | 44 | V2Ref = 2.0; // (V) |
RANDON | 0:031c505c5d69 | 45 | |
RANDON | 0:031c505c5d69 | 46 | // Error signal |
RANDON | 0:031c505c5d69 | 47 | e = V2Ref-V2; // (V) |
RANDON | 0:031c505c5d69 | 48 | |
RANDON | 0:031c505c5d69 | 49 | // Commanded voltage (V) |
RANDON | 0:031c505c5d69 | 50 | Cout = (1.948051948051948)*Cout1+(-0.948051948051948)*Cout2+(0.00746223086813)*e+(-0.008291367631262)*e1+(0.002211031368337)*e2; |
RANDON | 0:031c505c5d69 | 51 | //Cout =(1.96078)*Cout1+(-0.96078)*Cout2+(0.01027)*e+(-0.01539)*e1+(0.00571)*e2; |
RANDON | 0:031c505c5d69 | 52 | |
RANDON | 0:031c505c5d69 | 53 | // Age variables |
RANDON | 0:031c505c5d69 | 54 | Cout2 = Cout1; |
RANDON | 0:031c505c5d69 | 55 | Cout1 = Cout; |
RANDON | 0:031c505c5d69 | 56 | e2 = e1; |
RANDON | 0:031c505c5d69 | 57 | e1 = e; |
RANDON | 0:031c505c5d69 | 58 | |
RANDON | 0:031c505c5d69 | 59 | // Actuate |
RANDON | 0:031c505c5d69 | 60 | Cmbed = Cout/6.06+1.65; // Scale to 0<Cmbed<3.3 |
RANDON | 0:031c505c5d69 | 61 | |
RANDON | 0:031c505c5d69 | 62 | // Saturate Cmbed for safety |
RANDON | 0:031c505c5d69 | 63 | if(Cmbed>3.3) |
RANDON | 0:031c505c5d69 | 64 | Cmbed = 3.3; |
RANDON | 0:031c505c5d69 | 65 | if(Cmbed<0.0) |
RANDON | 0:031c505c5d69 | 66 | Cmbed = 0.0; |
RANDON | 0:031c505c5d69 | 67 | |
RANDON | 0:031c505c5d69 | 68 | // Cmbed = 1.65; // Must be between 0<Cmbed<3.3 |
RANDON | 0:031c505c5d69 | 69 | aout.write(Cmbed/3.3); // 0<aout<1. Normalized voltage |
RANDON | 0:031c505c5d69 | 70 | t2 = t.read(); |
RANDON | 0:031c505c5d69 | 71 | |
RANDON | 0:031c505c5d69 | 72 | controlfreq = 100; |
RANDON | 0:031c505c5d69 | 73 | |
RANDON | 0:031c505c5d69 | 74 | //log variable for plotting |
RANDON | 0:031c505c5d69 | 75 | |
RANDON | 0:031c505c5d69 | 76 | printf("v1 = %.4f, v2 = %.4f, Cmbed = %.4f, Cout = %f\n\r",V1,V2,Cmbed,Cout); |
RANDON | 0:031c505c5d69 | 77 | delaytime =(1/(controlfreq)-(t2-t1)); // Make routine run at 0.01 (sec) |
RANDON | 0:031c505c5d69 | 78 | wait(delaytime); |
RANDON | 0:031c505c5d69 | 79 | } |
RANDON | 0:031c505c5d69 | 80 | } |