File content as of revision 0:031c505c5d69:

#include "mbed.h"

Timer     t;
 
AnalogIn   ain_v1(p20);
AnalogIn   ain_v2(p19);
AnalogOut  aout(p18);

int main()
{
  float t1,   t2;
  float V1,   V1scaled;
  float V2,   V2scaled;
  float Cmbed;
  
  float Cout, Cout1, Cout2;
  float e, e1, e2;
  float V2Ref;
  float controlfreq, delaytime;
  
  // Initialize variables
  Cout1  = 0.0;
  Cout2  = 0.0;
  e1     = 0.0;
  e2     = 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
  
 // Reference voltage
 V2Ref = 2.0; // (V)

 // Error signal
 e = V2Ref-V2; // (V)

 // Commanded voltage (V)
 Cout = (1.948051948051948)*Cout1+(-0.948051948051948)*Cout2+(0.00746223086813)*e+(-0.008291367631262)*e1+(0.002211031368337)*e2;
 //Cout =(1.96078)*Cout1+(-0.96078)*Cout2+(0.01027)*e+(-0.01539)*e1+(0.00571)*e2;

 // Age variables
 Cout2 = Cout1;
 Cout1 = Cout;
 e2    = e1;
 e1    = e;
 
  // Actuate
  Cmbed = Cout/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();
  
  controlfreq = 100;
    
  //log variable for plotting
  
   printf("v1 = %.4f, v2 = %.4f, Cmbed = %.4f, Cout = %f\n\r",V1,V2,Cmbed,Cout);
   delaytime =(1/(controlfreq)-(t2-t1)); // Make routine run at 0.01 (sec)
   wait(delaytime);
   }
}