Rob Andon
SS implementation almost completed
Dependencies: Matrix MatrixMath mbed
Revision 0:1891695993b8, committed 2013-11-21
- Comitter:
- RANDON
- Date:
- Thu Nov 21 16:21:13 2013 +0000
- Commit message:
- SS Implementation;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.lib Thu Nov 21 16:21:13 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MatrixMath.lib Thu Nov 21 16:21:13 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Yo_Robot/code/MatrixMath/#93948a9bbde2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Wizadryfinal.cpp Thu Nov 21 16:21:13 2013 +0000
@@ -0,0 +1,134 @@
+// State Space Implementation on MBED Processor
+// Created by Rob Andon and Caleb Gerad
+// Last updated 21 NOV 13
+
+#include "mbed.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
+
+Timer t;
+
+AnalogIn ain_v1(p20);
+AnalogIn ain_v2(p19);
+AnalogOut aout(p18);
+
+int main()
+{
+
+Matrix A(2,2);
+Matrix B(2,1);
+Matrix x(2,1);
+Matrix e(2,1);
+Matrix K(2,2);
+Matrix xRef(2,1);
+Matrix xRefdot(2,1);
+Matrix BpseudoInverse(1,2);
+Matrix BT(1,2);
+Matrix BW(1,1);
+//Matrix BI(1,1);
+Matrix Cout(1,1);
+
+
+ float t1, t2;
+ float V1, V1scaled;
+ float V2, V2scaled;
+ float Cmbed;
+ float V2Ref;
+ float controlfreq, delaytime;
+ float rf1, rf2, ri1, ri2,cf1, cf2;
+ float Ts, k1, k2;
+ float Cout1;
+ float BW1;
+ float BI;
+
+
+ //Define the model's variables
+ rf1 = 510000.0; //(Ohms)
+ rf2 = 130000.0; //(Ohms)
+ ri1 = 180000.0; //(Ohms)
+ ri2 = 180000.0; //(Ohms)
+ cf1 = 0.0000001; //(F)
+ cf2 = 0.0000001; //(F)
+
+ // Matrix Declaration
+ A << (-1/(rf1*cf1)) << 0
+ << (-1/(ri1*cf2)) << (-1/(rf2*cf2));
+
+ B << (-1/(ri1*cf1))
+ << 0 ;
+
+ xRef << (-ri2/rf2)*2.0
+ << 2.0;
+
+ xRefdot << 0.0
+ << 0.0;
+
+ // Starts timer
+ t.start();
+ while(t.read()<10.0)
+ {
+
+ t1 = t.read(); // Gets elapsed time from when t.start() was done.
+
+ //==========================================Sense=========================================================================
+
+ V1scaled = ain_v1.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. / Multiple by 3.3 to get voltage.
+ V1 = (V1scaled-1.65)*6.06; // Re-calculate actual V1(t) voltage.
+
+ V2scaled = ain_v2.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. Multiple by 3.3 to get voltage.
+ V2 = (V2scaled-1.65)*6.06; // Re-calculate actual V1(t) voltage.
+
+//==========================================Decide=========================================================================
+
+ //Error Calculations
+ x << V1
+ << V2;
+
+ e = xRef-x;
+ Ts = 0.3; //(sec) settling time
+ k1 = 4/Ts;
+ k2 = 4/Ts;
+ K <<k1 <<k2
+ << 0.0 << 0.0;
+
+ // Wizardry Shiz Below...........
+
+ BT << (-1/(ri1*cf1)) << 0 ; // 1x2
+
+ BW = BT*B; // 1x1 = 1x2*2x1
+
+ BW1 = BW(1,1); //scalar
+
+ BI = 1/(BW1);
+
+
+ BpseudoInverse = BT*BI; //1x2 * 1x1
+
+ Cout = BpseudoInverse*(xRefdot-A*x+K*e);
+
+ // 1X2 * (2X1 - 2X1+2x1) = 1*1
+ Cout1 = Cout( 1, 1 );
+
+//==========================================Actuate=========================================================================
+
+ Cmbed = Cout1/6.06+1.65; // Scale to 0<Cmbed<3.3
+
+ // Saturate Cmbed for safety
+ if(Cmbed>3.3)
+ Cmbed = 3.3;
+ if(Cmbed<0.0)
+ Cmbed = 0.0;
+
+ // Cmbed = 1.65; // Must be between 0<Cmbed<3.3
+ aout.write(Cmbed/3.3); // 0<aout<1. Normalized voltage
+ t2 = t.read();
+
+ //log variable for plotting
+
+ printf("v1 = %.4f, v2 = %.4f, Cmbed = %.4f, Cout = %f\n\r",V1,V2,Cmbed,Cout1);
+ //======================================Logging Variable====================================================================
+
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Nov 21 16:21:13 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file