Diff: Wizadryfinal.cpp

Revision:

0:1891695993b8

--- /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====================================================================
+  
+   }
+}
+
+