Classical Control
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 21:11:38 by
1.7.2