Classical Control

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 
00003 Timer     t;
00004  
00005 AnalogIn   ain_v1(p20);
00006 AnalogIn   ain_v2(p19);
00007 AnalogOut  aout(p18);
00008 
00009 int main()
00010 {
00011   float t1,   t2;
00012   float V1,   V1scaled;
00013   float V2,   V2scaled;
00014   float Cmbed;
00015   
00016   float Cout, Cout1, Cout2;
00017   float e, e1, e2;
00018   float V2Ref;
00019   float controlfreq, delaytime;
00020   
00021   // Initialize variables
00022   Cout1  = 0.0;
00023   Cout2  = 0.0;
00024   e1     = 0.0;
00025   e2     = 0.0;
00026   
00027   // Starts timer
00028   t.start();
00029   while(t.read()<10.0)
00030   {
00031   
00032   t1 = t.read();                // Gets elapsed time from when t.start() was done. 
00033  
00034   // Sense
00035   V1scaled = ain_v1.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. / Multiple by 3.3 to get voltage.   
00036   V1 = (V1scaled-1.65)*6.06;    // Re-calculate actual V1(t) voltage.
00037       
00038   V2scaled = ain_v2.read()*3.3; // 0.0<ain.read()<1.0. Normalized voltage. Multiple by 3.3 to get voltage.   
00039   V2 = (V2scaled-1.65)*6.06;    // Re-calculate actual V1(t) voltage.
00040   
00041   // Decide
00042   
00043  // Reference voltage
00044  V2Ref = 2.0; // (V)
00045 
00046  // Error signal
00047  e = V2Ref-V2; // (V)
00048 
00049  // Commanded voltage (V)
00050  Cout = (1.948051948051948)*Cout1+(-0.948051948051948)*Cout2+(0.00746223086813)*e+(-0.008291367631262)*e1+(0.002211031368337)*e2;
00051  //Cout =(1.96078)*Cout1+(-0.96078)*Cout2+(0.01027)*e+(-0.01539)*e1+(0.00571)*e2;
00052 
00053  // Age variables
00054  Cout2 = Cout1;
00055  Cout1 = Cout;
00056  e2    = e1;
00057  e1    = e;
00058  
00059   // Actuate
00060   Cmbed = Cout/6.06+1.65; // Scale to 0<Cmbed<3.3
00061   
00062   // Saturate Cmbed for safety
00063   if(Cmbed>3.3)
00064     Cmbed = 3.3;
00065   if(Cmbed<0.0)
00066     Cmbed = 0.0;
00067   
00068   // Cmbed = 1.65;        // Must be between 0<Cmbed<3.3
00069   aout.write(Cmbed/3.3);  // 0<aout<1. Normalized voltage
00070   t2 = t.read();
00071   
00072   controlfreq = 100;
00073     
00074   //log variable for plotting
00075   
00076    printf("v1 = %.4f, v2 = %.4f, Cmbed = %.4f, Cout = %f\n\r",V1,V2,Cmbed,Cout);
00077    delaytime =(1/(controlfreq)-(t2-t1)); // Make routine run at 0.01 (sec)
00078    wait(delaytime);
00079    }
00080 }