Rob Andon / Mbed 2 deprecated Wizardsneverdie

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?

UserRevisionLine numberNew 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