File content as of revision 0:1891695993b8:

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