SS implementation almost completed
Dependencies: Matrix MatrixMath mbed
Wizadryfinal.cpp@0:1891695993b8, 2013-11-21 (annotated)
- Committer:
- RANDON
- Date:
- Thu Nov 21 16:21:13 2013 +0000
- Revision:
- 0:1891695993b8
SS Implementation;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RANDON | 0:1891695993b8 | 1 | // State Space Implementation on MBED Processor |
RANDON | 0:1891695993b8 | 2 | // Created by Rob Andon and Caleb Gerad |
RANDON | 0:1891695993b8 | 3 | // Last updated 21 NOV 13 |
RANDON | 0:1891695993b8 | 4 | |
RANDON | 0:1891695993b8 | 5 | #include "mbed.h" |
RANDON | 0:1891695993b8 | 6 | #include "Matrix.h" |
RANDON | 0:1891695993b8 | 7 | #include "MatrixMath.h" |
RANDON | 0:1891695993b8 | 8 | |
RANDON | 0:1891695993b8 | 9 | Timer t; |
RANDON | 0:1891695993b8 | 10 | |
RANDON | 0:1891695993b8 | 11 | AnalogIn ain_v1(p20); |
RANDON | 0:1891695993b8 | 12 | AnalogIn ain_v2(p19); |
RANDON | 0:1891695993b8 | 13 | AnalogOut aout(p18); |
RANDON | 0:1891695993b8 | 14 | |
RANDON | 0:1891695993b8 | 15 | int main() |
RANDON | 0:1891695993b8 | 16 | { |
RANDON | 0:1891695993b8 | 17 | |
RANDON | 0:1891695993b8 | 18 | Matrix A(2,2); |
RANDON | 0:1891695993b8 | 19 | Matrix B(2,1); |
RANDON | 0:1891695993b8 | 20 | Matrix x(2,1); |
RANDON | 0:1891695993b8 | 21 | Matrix e(2,1); |
RANDON | 0:1891695993b8 | 22 | Matrix K(2,2); |
RANDON | 0:1891695993b8 | 23 | Matrix xRef(2,1); |
RANDON | 0:1891695993b8 | 24 | Matrix xRefdot(2,1); |
RANDON | 0:1891695993b8 | 25 | Matrix BpseudoInverse(1,2); |
RANDON | 0:1891695993b8 | 26 | Matrix BT(1,2); |
RANDON | 0:1891695993b8 | 27 | Matrix BW(1,1); |
RANDON | 0:1891695993b8 | 28 | //Matrix BI(1,1); |
RANDON | 0:1891695993b8 | 29 | Matrix Cout(1,1); |
RANDON | 0:1891695993b8 | 30 | |
RANDON | 0:1891695993b8 | 31 | |
RANDON | 0:1891695993b8 | 32 | float t1, t2; |
RANDON | 0:1891695993b8 | 33 | float V1, V1scaled; |
RANDON | 0:1891695993b8 | 34 | float V2, V2scaled; |
RANDON | 0:1891695993b8 | 35 | float Cmbed; |
RANDON | 0:1891695993b8 | 36 | float V2Ref; |
RANDON | 0:1891695993b8 | 37 | float controlfreq, delaytime; |
RANDON | 0:1891695993b8 | 38 | float rf1, rf2, ri1, ri2,cf1, cf2; |
RANDON | 0:1891695993b8 | 39 | float Ts, k1, k2; |
RANDON | 0:1891695993b8 | 40 | float Cout1; |
RANDON | 0:1891695993b8 | 41 | float BW1; |
RANDON | 0:1891695993b8 | 42 | float BI; |
RANDON | 0:1891695993b8 | 43 | |
RANDON | 0:1891695993b8 | 44 | |
RANDON | 0:1891695993b8 | 45 | //Define the model's variables |
RANDON | 0:1891695993b8 | 46 | rf1 = 510000.0; //(Ohms) |
RANDON | 0:1891695993b8 | 47 | rf2 = 130000.0; //(Ohms) |
RANDON | 0:1891695993b8 | 48 | ri1 = 180000.0; //(Ohms) |
RANDON | 0:1891695993b8 | 49 | ri2 = 180000.0; //(Ohms) |
RANDON | 0:1891695993b8 | 50 | cf1 = 0.0000001; //(F) |
RANDON | 0:1891695993b8 | 51 | cf2 = 0.0000001; //(F) |
RANDON | 0:1891695993b8 | 52 | |
RANDON | 0:1891695993b8 | 53 | // Matrix Declaration |
RANDON | 0:1891695993b8 | 54 | A << (-1/(rf1*cf1)) << 0 |
RANDON | 0:1891695993b8 | 55 | << (-1/(ri1*cf2)) << (-1/(rf2*cf2)); |
RANDON | 0:1891695993b8 | 56 | |
RANDON | 0:1891695993b8 | 57 | B << (-1/(ri1*cf1)) |
RANDON | 0:1891695993b8 | 58 | << 0 ; |
RANDON | 0:1891695993b8 | 59 | |
RANDON | 0:1891695993b8 | 60 | xRef << (-ri2/rf2)*2.0 |
RANDON | 0:1891695993b8 | 61 | << 2.0; |
RANDON | 0:1891695993b8 | 62 | |
RANDON | 0:1891695993b8 | 63 | xRefdot << 0.0 |
RANDON | 0:1891695993b8 | 64 | << 0.0; |
RANDON | 0:1891695993b8 | 65 | |
RANDON | 0:1891695993b8 | 66 | // Starts timer |
RANDON | 0:1891695993b8 | 67 | t.start(); |
RANDON | 0:1891695993b8 | 68 | while(t.read()<10.0) |
RANDON | 0:1891695993b8 | 69 | { |
RANDON | 0:1891695993b8 | 70 | |
RANDON | 0:1891695993b8 | 71 | t1 = t.read(); // Gets elapsed time from when t.start() was done. |
RANDON | 0:1891695993b8 | 72 | |
RANDON | 0:1891695993b8 | 73 | //==========================================Sense========================================================================= |
RANDON | 0:1891695993b8 | 74 | |
RANDON | 0:1891695993b8 | 75 | V1scaled = ain_v1.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. / Multiple by 3.3 to get voltage. |
RANDON | 0:1891695993b8 | 76 | V1 = (V1scaled-1.65)*6.06; // Re-calculate actual V1(t) voltage. |
RANDON | 0:1891695993b8 | 77 | |
RANDON | 0:1891695993b8 | 78 | V2scaled = ain_v2.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. Multiple by 3.3 to get voltage. |
RANDON | 0:1891695993b8 | 79 | V2 = (V2scaled-1.65)*6.06; // Re-calculate actual V1(t) voltage. |
RANDON | 0:1891695993b8 | 80 | |
RANDON | 0:1891695993b8 | 81 | //==========================================Decide========================================================================= |
RANDON | 0:1891695993b8 | 82 | |
RANDON | 0:1891695993b8 | 83 | //Error Calculations |
RANDON | 0:1891695993b8 | 84 | x << V1 |
RANDON | 0:1891695993b8 | 85 | << V2; |
RANDON | 0:1891695993b8 | 86 | |
RANDON | 0:1891695993b8 | 87 | e = xRef-x; |
RANDON | 0:1891695993b8 | 88 | Ts = 0.3; //(sec) settling time |
RANDON | 0:1891695993b8 | 89 | k1 = 4/Ts; |
RANDON | 0:1891695993b8 | 90 | k2 = 4/Ts; |
RANDON | 0:1891695993b8 | 91 | K <<k1 <<k2 |
RANDON | 0:1891695993b8 | 92 | << 0.0 << 0.0; |
RANDON | 0:1891695993b8 | 93 | |
RANDON | 0:1891695993b8 | 94 | // Wizardry Shiz Below........... |
RANDON | 0:1891695993b8 | 95 | |
RANDON | 0:1891695993b8 | 96 | BT << (-1/(ri1*cf1)) << 0 ; // 1x2 |
RANDON | 0:1891695993b8 | 97 | |
RANDON | 0:1891695993b8 | 98 | BW = BT*B; // 1x1 = 1x2*2x1 |
RANDON | 0:1891695993b8 | 99 | |
RANDON | 0:1891695993b8 | 100 | BW1 = BW(1,1); //scalar |
RANDON | 0:1891695993b8 | 101 | |
RANDON | 0:1891695993b8 | 102 | BI = 1/(BW1); |
RANDON | 0:1891695993b8 | 103 | |
RANDON | 0:1891695993b8 | 104 | |
RANDON | 0:1891695993b8 | 105 | BpseudoInverse = BT*BI; //1x2 * 1x1 |
RANDON | 0:1891695993b8 | 106 | |
RANDON | 0:1891695993b8 | 107 | Cout = BpseudoInverse*(xRefdot-A*x+K*e); |
RANDON | 0:1891695993b8 | 108 | |
RANDON | 0:1891695993b8 | 109 | // 1X2 * (2X1 - 2X1+2x1) = 1*1 |
RANDON | 0:1891695993b8 | 110 | Cout1 = Cout( 1, 1 ); |
RANDON | 0:1891695993b8 | 111 | |
RANDON | 0:1891695993b8 | 112 | //==========================================Actuate========================================================================= |
RANDON | 0:1891695993b8 | 113 | |
RANDON | 0:1891695993b8 | 114 | Cmbed = Cout1/6.06+1.65; // Scale to 0<Cmbed<3.3 |
RANDON | 0:1891695993b8 | 115 | |
RANDON | 0:1891695993b8 | 116 | // Saturate Cmbed for safety |
RANDON | 0:1891695993b8 | 117 | if(Cmbed>3.3) |
RANDON | 0:1891695993b8 | 118 | Cmbed = 3.3; |
RANDON | 0:1891695993b8 | 119 | if(Cmbed<0.0) |
RANDON | 0:1891695993b8 | 120 | Cmbed = 0.0; |
RANDON | 0:1891695993b8 | 121 | |
RANDON | 0:1891695993b8 | 122 | // Cmbed = 1.65; // Must be between 0<Cmbed<3.3 |
RANDON | 0:1891695993b8 | 123 | aout.write(Cmbed/3.3); // 0<aout<1. Normalized voltage |
RANDON | 0:1891695993b8 | 124 | t2 = t.read(); |
RANDON | 0:1891695993b8 | 125 | |
RANDON | 0:1891695993b8 | 126 | //log variable for plotting |
RANDON | 0:1891695993b8 | 127 | |
RANDON | 0:1891695993b8 | 128 | printf("v1 = %.4f, v2 = %.4f, Cmbed = %.4f, Cout = %f\n\r",V1,V2,Cmbed,Cout1); |
RANDON | 0:1891695993b8 | 129 | //======================================Logging Variable==================================================================== |
RANDON | 0:1891695993b8 | 130 | |
RANDON | 0:1891695993b8 | 131 | } |
RANDON | 0:1891695993b8 | 132 | } |
RANDON | 0:1891695993b8 | 133 | |
RANDON | 0:1891695993b8 | 134 |