program for an optical modulator bias controller. Input signal is converted by ADC via fastanalogin, then filtered using FIR filters. The signal is processed and the DAC has an output signal.
Dependencies: FastAnalogIn mbed-dsp mbed
main.cpp
00001 #include "mbed.h" 00002 #include "FastAnalogIn.h" 00003 00004 00005 // Tickers, timers - used for ensuring fixed times: 00006 Ticker ADCperiod; //Creates ticker class "Sampletime" used for ADC conversion 00007 Ticker DACperiod; //Creates ticker class "DACperiod" used for DAC conversion 00008 Timer DSPtimer; //Creates timer class "DSPperiod" used for DSP time. Time is needed for derivative and integral components of PID controller. 00009 00010 00011 // Serial communication: 00012 Serial pc(USBTX, USBRX); //tx,rx 00013 00014 //*************************************************************************Pin designations ************************************************************************************************* 00015 // ADC pin designation. FastAnalogIn runs ADC in "burst" mode (DMA): 00016 FastAnalogIn adc(PTE20); //float. 0 <=adc <= 1; 0 = 0V, 1.0f = 3.3V 00017 // Other analog inputs are set as digital outputs for better ADC performance: 00018 DigitalOut dig1(PTE22) = 0; 00019 DigitalOut dig2(PTE21); 00020 DigitalOut dig3(PTE29); 00021 DigitalOut dig4(PTE30); 00022 DigitalOut dig5(PTE23); 00023 DigitalOut dig6(PTB0); 00024 DigitalOut dig7(PTB1); 00025 DigitalOut dig8(PTB2); 00026 DigitalOut dig9(PTB3); 00027 DigitalOut dig10(PTC0); 00028 DigitalOut dig11(PTC1); 00029 DigitalOut dig12(PTC2); 00030 DigitalOut dig13(PTD1); 00031 DigitalOut dig14(PTD5); 00032 DigitalOut dig15(PTD6); 00033 //DAC pin designation. 00034 AnalogOut dac(PTE30); //float. 0 <=dac <= 1; 0 = 0V, 1.0f = 3.3V. Also used for debugging DSP_control_cycle 00035 //******************************************************************************************************************************************************************************************* 00036 00037 00038 //**********************************************************************Constants and variables********************************************************************************************** 00039 //Frequencies and periods: 00040 float sampletime = 0.0001; //ADC sample time [s]. Should be 1/Fs. 00041 double dactime = 5e-05; //DAC time. Should be 1/(freq1 * sinepoints) 00042 00043 //ADC parameters: 00044 float analogValuein; //defines variable where ADC conversion will be stored 00045 float shiftArray[1200]; //serves as a shift register for the ADC conversions. It stores the necessary taps for the highest order filter. MinSize = nmax + RMS cycles - 1 00046 00047 //RMS detector (envelope detection): 00048 int RMS_cycles = 100; //the amount of RMS detector cycles to be carried out for half of the fundamental sine wave. Must equal Fs/freq1; 00049 double FIRfundArray[100]; //array that stores several values of FIRfundoutput in order to find RMS. MinSize = RMS_cycles 00050 double FIR2ndArray[100]; //a rray that stores several values of FIR2ndoutput in order to find RMS. MinSize = RMS_cycles 00051 double rms1; //root mean square of fundamental frequency FIR filter output 00052 double rms2; //root mean square of 2nd harmonic FIR filter output 00053 double sum_squares_1; //sum of squares of fundamental 00054 double sum_squares_2; //sum of squares of 2nd harmonic 00055 double ratio; //ratio of rms1 and rms2 00056 00057 //PID controller: 00058 double dspsampletime; //Stores PID sample size 00059 double setpoint = 0; //PID controller set point. 0 for quadrature for MZM transfer function. Goes to infinity at 0, 180, .. 00060 double Kp = 0.001; //PID proportional coefficient 00061 double Ki= 0.0005; //PID integral coefficient 00062 double Kd= 0.01; //PID derivative coefficient 00063 double Pout; //PID proportional output 00064 double Iout; //PID integral output 00065 double Dout; //PID derivative output 00066 double loop_error; //PID control loop error 00067 double pre_error; //PID control error from previous cycle, used for the derivative output 00068 double integral; //PID integral of error 00069 double derivative; //PID derivative of error 00070 double PIDoutput; //PID output. This is then taken as the DC bias voltage. 00071 double max = 1; //PID output maximum value. 00072 double min = 0; //PID output minimum value. 00073 double errormin = 3e-2; //PID minimum error. Taken from sine experiment. This is the minimum value above noise. 00074 00075 //DAC parameters: 00076 float analogValueout; //defines variable where DAC output value will be stored 00077 //stores normalized sine wave coefficients (amplitude of 1.0f = 3.3V): 00078 double sinecoeff[] = {0,0.309016989,0.587785244,0.809016985,0.95105651,1,0.951056526,0.809017016,0.587785287,0.30901704,5.35898E-08,-0.309016938,-0.5877852,-0.809016953,-0.951056493,-1.00E+00,-0.951056543,-0.809017048,-0.58778533,-0.309017091}; 00079 float Vp = 0.5; //dither sinewave peak voltage [V]. This is the magnitude before OP AMP voltage shifter. 00080 double sineout[20]; //will contain sinewave with bias offset 00081 double biasdc = 0.5; //biasdc voltage [V]. V = biasdc * 3.3V 00082 00083 //FIR filter coefficients from matlab FDtool for fundamental: 00084 float FIR_coefficients_1[] = {0.00069406,-0.00043799,-0.0003647,-0.0003106,-0.0002486,-0.00016867,-7.6568e-05,8.821e-06,6.62e-05,7.977e-05,4.8598e-05,-1.3432e-05,-7.9988e-05,-0.00012357,-0.0001241,-7.8861e-05,-2.2895e-06,7.7063e-05,0.00012915,0.00013215,8.35e-05,-4.393e-07,-8.7681e-05,-0.00014486,-0.00014767,-9.3276e-05,-4.2932e-07,9.7182e-05,0.00015944,0.00016317,0.0001033,5.8883e-08,-0.0001071,-0.00017638,-0.00017945,-0.000113,4.7382e-09,0.00011721,0.00019335,0.00019693,0.00012405,3.3079e-08,-0.00012839,-0.0002116,-0.0002153,-0.00013547,1.6038e-08,0.00014015,0.00023075,0.00023467,0.00014755,-1.9713e-08,-0.00015245,-0.00025107,-0.00025497,-0.00016045,2.2135e-08,0.00016559,0.00027231,0.0002766,0.00017387,1.3997e-08,-0.00017931,-0.00029483,-0.00029928,-0.00018789,2.6618e-08,0.00019359,0.00031822,0.00032303,0.00020281,5.068e-08,-0.00020865,-0.00034284,-0.00034783,-0.00021826,-3.1088e-08,0.00022437,0.00036847,0.00037373,0.00023432,7.7341e-08,-0.00024085,-0.00039529,-0.00040071,-0.00025119,-8.3687e-09,0.00025804,0.0004233,0.00042893,0.00026872,-3.3565e-08,-0.00027594,-0.00045249,-0.00045836,-0.00028702,5.0392e-08,0.00029449,0.00048272,0.00048889,0.00030609,2.783e-08,-0.00031376,-0.00051416,-0.00052055,-0.00032577,3.5442e-08,0.00033375,0.00054678,0.00055335,0.00034618,-4.0657e-08,-0.00035455,-0.0005806,-0.0005874,-0.00036736,5.674e-08,0.00037605,0.00061561,0.00062266,0.0003893,-1.8637e-08,-0.00039819,-0.00065171,-0.00065902,-0.00041194,5.232e-09,0.00042104,0.00068892,0.00069646,0.00043523,-2.3433e-08,-0.0004447,-0.00072737,-0.00073516,-0.00045922,5.2582e-08,0.00046903,0.00076697,0.00077496,0.000484,1.8285e-08,-0.00049398,-0.00080759,-0.00081582,-0.00050939,-1.5414e-08,0.00051964,0.00084932,0.00085776,0.0005354,-7.2665e-08,-0.00054608,-0.00089223,-0.00090089,-0.00056217,6.6357e-08,0.00057312,0.00093619,0.00094506,0.00058966,-5.4152e-09,-0.00060071,-0.00098111,-0.00099019,-0.00061763,6.1874e-08,0.00062906,0.0010271,0.0010364,0.00064629,-7.5303e-08,-0.00065801,-0.0010742,-0.0010836,-0.00067564,1.6037e-08,0.00068748,0.0011221,0.0011317,0.00070547,-6.0194e-08,-0.0007176,-0.001171,-0.0011808,-0.0007359,7.551e-08,0.00074832,0.0012208,0.0012308,0.00076697,1.656e-08,-0.00077941,-0.0012714,-0.0012816,-0.00079841,3.0772e-08,0.00081114,0.0013228,0.0013332,0.00083039,-4.3455e-08,-0.00084335,-0.0013751,-0.0013856,-0.0008629,-1.7346e-08,0.00087593,0.001428,0.0014386,0.00089571,-6.2806e-08,-0.00090907,-0.0014817,-0.0014924,-0.00092907,2.0491e-08,0.00094249,0.0015359,0.0015468,0.00096273,-2.3141e-08,-0.00097629,-0.0015907,-0.0016017,-0.00099668,1.0569e-07,0.0010105,0.0016461,0.0016572,0.0010311,-1.1722e-08,-0.0010449,-0.0017019,-0.001713,-0.0010656,1.2409e-07,0.0010797,0.0017582,0.0017695,0.0011006,-1.9281e-08,-0.0011146,-0.0018147,-0.001826,-0.0011355,1.1686e-07,0.0011498,0.0018717,0.0018831,0.0011708,-5.0168e-08,-0.001185,-0.0019288,-0.0019402,-0.0012062,8.6518e-08,0.0012204,0.0019861,0.0019976,0.0012416,-3.6495e-08,-0.0012559,-0.0020435,-0.0020549,-0.0012771,6.5803e-08,0.0012914,0.002101,0.0021124,0.0013126,-5.6544e-08,-0.0013269,-0.0021584,-0.0021698,-0.0013481,5.324e-08,0.0013623,0.0022157,0.0022271,0.0013835,-5.2398e-08,-0.0013976,-0.0022728,-0.0022841,-0.0014187,6.1306e-08,0.0014328,0.0023297,0.002341,0.0014538,-7.253e-08,-0.0014678,-0.0023863,-0.0023975,-0.0014887,5.8142e-08,0.0015026,0.0024424,0.0024536,0.0015233,-6.9425e-08,-0.0015371,-0.0024982,-0.0025092,-0.0015576,8.3336e-08,0.0015713,0.0025534,0.0025643,0.0015916,-6.5189e-08,-0.0016051,-0.002608,-0.0026188,-0.0016252,6.4387e-08,0.0016386,0.0026619,0.0026726,0.0016583,-7.2277e-08,-0.0016715,-0.0027151,-0.0027256,-0.001691,5.6327e-08,0.001704,0.0027675,0.0027778,0.0017231,-4.4875e-08,-0.0017359,-0.0028189,-0.0028291,-0.0017547,5.1378e-08,0.0017672,0.0028695,0.0028794,0.0017857,-5.6993e-08,-0.001798,-0.002919,-0.0029287,-0.001816,3.4083e-08,0.001828,0.0029673,0.0029768,0.0018456,-4.638e-08,-0.0018573,-0.0030146,-0.0030238,-0.0018745,4.9635e-08,0.0018859,0.0030605,0.0030695,0.0019026,-3.7663e-08,-0.0019137,-0.0031052,-0.0031139,-0.0019299,5.3228e-08,0.0019406,0.0031485,0.003157,0.0019563,-5.4482e-08,-0.0019667,-0.0031904,-0.0031985,-0.0019818,3.7409e-08,0.0019918,0.0032307,0.0032386,0.0020064,-5.654e-08,-0.002016,-0.0032696,-0.0032771,-0.00203,5.3288e-08,0.0020392,0.0033068,0.003314,0.0020526,-3.8837e-08,-0.0020613,-0.0033423,-0.0033491,-0.0020741,5.3321e-08,0.0020825,0.0033761,0.0033826,0.0020946,-4.4528e-08,-0.0021025,-0.0034081,-0.0034143,-0.0021139,4.0049e-08,0.0021214,0.0034384,0.0034441,0.0021321,-5.3345e-08,-0.0021392,-0.0034667,-0.0034722,-0.0021492,3.0329e-08,0.0021557,0.0034932,0.0034982,0.0021651,-3.9513e-08,-0.0021711,-0.0035177,-0.0035223,-0.0021798,3.6678e-08,0.0021853,0.0035402,0.0035445,0.0021932,-2.2074e-08,-0.0021982,-0.0035607,-0.0035646,-0.0022054,2.7665e-08,0.0022099,0.0035792,0.0035827,0.0022163,-1.5345e-08,-0.0022203,-0.0035956,-0.0035986,-0.0022259,1.4945e-08,0.0022294,0.0036099,0.0036125,0.0022342,-3.7453e-09,-0.0022372,-0.0036221,-0.0036242,-0.0022412,-2.7894e-09,0.0022436,0.0036321,0.0036338,0.0022468,-1.7627e-08,-0.0022488,-0.00364,-0.0036413,-0.0022512,-2.5432e-08,0.0022525,0.0036456,0.0036465,0.0022541,-2.0976e-08,-0.002255,-0.0036491,-0.0036496,-0.0022558,-1.5581e-08,0.002256,0.0036504,0.0036504,0.002256,-1.5581e-08,-0.0022558,-0.0036496,-0.0036491,-0.002255,-2.0976e-08,0.0022541,0.0036465,0.0036456,0.0022525,-2.5432e-08,-0.0022512,-0.0036413,-0.00364,-0.0022488,-1.7627e-08,0.0022468,0.0036338,0.0036321,0.0022436,-2.7894e-09,-0.0022412,-0.0036242,-0.0036221,-0.0022372,-3.7453e-09,0.0022342,0.0036125,0.0036099,0.0022294,1.4945e-08,-0.0022259,-0.0035986,-0.0035956,-0.0022203,-1.5345e-08,0.0022163,0.0035827,0.0035792,0.0022099,2.7665e-08,-0.0022054,-0.0035646,-0.0035607,-0.0021982,-2.2074e-08,0.0021932,0.0035445,0.0035402,0.0021853,3.6678e-08,-0.0021798,-0.0035223,-0.0035177,-0.0021711,-3.9513e-08,0.0021651,0.0034982,0.0034932,0.0021557,3.0329e-08,-0.0021492,-0.0034722,-0.0034667,-0.0021392,-5.3345e-08,0.0021321,0.0034441,0.0034384,0.0021214,4.0049e-08,-0.0021139,-0.0034143,-0.0034081,-0.0021025,-4.4528e-08,0.0020946,0.0033826,0.0033761,0.0020825,5.3321e-08,-0.0020741,-0.0033491,-0.0033423,-0.0020613,-3.8837e-08,0.0020526,0.003314,0.0033068,0.0020392,5.3288e-08,-0.00203,-0.0032771,-0.0032696,-0.002016,-5.654e-08,0.0020064,0.0032386,0.0032307,0.0019918,3.7409e-08,-0.0019818,-0.0031985,-0.0031904,-0.0019667,-5.4482e-08,0.0019563,0.003157,0.0031485,0.0019406,5.3228e-08,-0.0019299,-0.0031139,-0.0031052,-0.0019137,-3.7663e-08,0.0019026,0.0030695,0.0030605,0.0018859,4.9635e-08,-0.0018745,-0.0030238,-0.0030146,-0.0018573,-4.638e-08,0.0018456,0.0029768,0.0029673,0.001828,3.4083e-08,-0.001816,-0.0029287,-0.002919,-0.001798,-5.6993e-08,0.0017857,0.0028794,0.0028695,0.0017672,5.1378e-08,-0.0017547,-0.0028291,-0.0028189,-0.0017359,-4.4875e-08,0.0017231,0.0027778,0.0027675,0.001704,5.6327e-08,-0.001691,-0.0027256,-0.0027151,-0.0016715,-7.2277e-08,0.0016583,0.0026726,0.0026619,0.0016386,6.4387e-08,-0.0016252,-0.0026188,-0.002608,-0.0016051,-6.5189e-08,0.0015916,0.0025643,0.0025534,0.0015713,8.3336e-08,-0.0015576,-0.0025092,-0.0024982,-0.0015371,-6.9425e-08,0.0015233,0.0024536,0.0024424,0.0015026,5.8142e-08,-0.0014887,-0.0023975,-0.0023863,-0.0014678,-7.253e-08,0.0014538,0.002341,0.0023297,0.0014328,6.1306e-08,-0.0014187,-0.0022841,-0.0022728,-0.0013976,-5.2398e-08,0.0013835,0.0022271,0.0022157,0.0013623,5.324e-08,-0.0013481,-0.0021698,-0.0021584,-0.0013269,-5.6544e-08,0.0013126,0.0021124,0.002101,0.0012914,6.5803e-08,-0.0012771,-0.0020549,-0.0020435,-0.0012559,-3.6495e-08,0.0012416,0.0019976,0.0019861,0.0012204,8.6518e-08,-0.0012062,-0.0019402,-0.0019288,-0.001185,-5.0168e-08,0.0011708,0.0018831,0.0018717,0.0011498,1.1686e-07,-0.0011355,-0.001826,-0.0018147,-0.0011146,-1.9281e-08,0.0011006,0.0017695,0.0017582,0.0010797,1.2409e-07,-0.0010656,-0.001713,-0.0017019,-0.0010449,-1.1722e-08,0.0010311,0.0016572,0.0016461,0.0010105,1.0569e-07,-0.00099668,-0.0016017,-0.0015907,-0.00097629,-2.3141e-08,0.00096273,0.0015468,0.0015359,0.00094249,2.0491e-08,-0.00092907,-0.0014924,-0.0014817,-0.00090907,-6.2806e-08,0.00089571,0.0014386,0.001428,0.00087593,-1.7346e-08,-0.0008629,-0.0013856,-0.0013751,-0.00084335,-4.3455e-08,0.00083039,0.0013332,0.0013228,0.00081114,3.0772e-08,-0.00079841,-0.0012816,-0.0012714,-0.00077941,1.656e-08,0.00076697,0.0012308,0.0012208,0.00074832,7.551e-08,-0.0007359,-0.0011808,-0.001171,-0.0007176,-6.0194e-08,0.00070547,0.0011317,0.0011221,0.00068748,1.6037e-08,-0.00067564,-0.0010836,-0.0010742,-0.00065801,-7.5303e-08,0.00064629,0.0010364,0.0010271,0.00062906,6.1874e-08,-0.00061763,-0.00099019,-0.00098111,-0.00060071,-5.4152e-09,0.00058966,0.00094506,0.00093619,0.00057312,6.6357e-08,-0.00056217,-0.00090089,-0.00089223,-0.00054608,-7.2665e-08,0.0005354,0.00085776,0.00084932,0.00051964,-1.5414e-08,-0.00050939,-0.00081582,-0.00080759,-0.00049398,1.8285e-08,0.000484,0.00077496,0.00076697,0.00046903,5.2582e-08,-0.00045922,-0.00073516,-0.00072737,-0.0004447,-2.3433e-08,0.00043523,0.00069646,0.00068892,0.00042104,5.232e-09,-0.00041194,-0.00065902,-0.00065171,-0.00039819,-1.8637e-08,0.0003893,0.00062266,0.00061561,0.00037605,5.674e-08,-0.00036736,-0.0005874,-0.0005806,-0.00035455,-4.0657e-08,0.00034618,0.00055335,0.00054678,0.00033375,3.5442e-08,-0.00032577,-0.00052055,-0.00051416,-0.00031376,2.783e-08,0.00030609,0.00048889,0.00048272,0.00029449,5.0392e-08,-0.00028702,-0.00045836,-0.00045249,-0.00027594,-3.3565e-08,0.00026872,0.00042893,0.0004233,0.00025804,-8.3687e-09,-0.00025119,-0.00040071,-0.00039529,-0.00024085,7.7341e-08,0.00023432,0.00037373,0.00036847,0.00022437,-3.1088e-08,-0.00021826,-0.00034783,-0.00034284,-0.00020865,5.068e-08,0.00020281,0.00032303,0.00031822,0.00019359,2.6618e-08,-0.00018789,-0.00029928,-0.00029483,-0.00017931,1.3997e-08,0.00017387,0.0002766,0.00027231,0.00016559,2.2135e-08,-0.00016045,-0.00025497,-0.00025107,-0.00015245,-1.9713e-08,0.00014755,0.00023467,0.00023075,0.00014015,1.6038e-08,-0.00013547,-0.0002153,-0.0002116,-0.00012839,3.3079e-08,0.00012405,0.00019693,0.00019335,0.00011721,4.7382e-09,-0.000113,-0.00017945,-0.00017638,-0.0001071,5.8883e-08,0.0001033,0.00016317,0.00015944,9.7182e-05,-4.2932e-07,-9.3276e-05,-0.00014767,-0.00014486,-8.7681e-05,-4.393e-07,8.35e-05,0.00013215,0.00012915,7.7063e-05,-2.2895e-06,-7.8861e-05,-0.0001241,-0.00012357,-7.9988e-05,-1.3432e-05,4.8598e-05,7.977e-05,6.62e-05,8.821e-06,-7.6568e-05,-0.00016867,-0.0002486,-0.0003106,-0.0003647,-0.00043799,0.00069406}; 00085 //FIR filter coefficients from matlab FDtool for 2nd harmonic: 00086 float FIR_coefficients_2[] = {-4.2759e-06,-0.00097887,-2.5561e-05,7.9182e-05,8.1224e-05,-3.1833e-05,-0.00010518,-3.3321e-05,8.9269e-05,9.1204e-05,-3.5625e-05,-0.00011807,-3.7218e-05,9.9772e-05,0.00010207,-3.9889e-05,-0.00013163,-4.1613e-05,0.00011126,0.00011351,-4.4265e-05,-0.00014645,-4.611e-05,0.00012342,0.00012606,-4.9221e-05,-0.00016218,-5.1153e-05,0.00013672,0.00013927,-5.4296e-05,-0.00017917,-5.6301e-05,0.00015056,0.00015345,-5.9818e-05,-0.00019677,-6.1956e-05,0.00016521,0.00016806,-6.5322e-05,-0.00021539,-6.7684e-05,0.00018041,0.00018374,-7.1457e-05,-0.0002351,-7.398e-05,0.00019711,0.00020037,-7.7919e-05,-0.00025643,-8.039e-05,0.00021427,0.00021774,-8.4539e-05,-0.00027745,-8.7161e-05,0.00023246,0.00023649,-9.198e-05,-0.00030094,-9.4488e-05,0.00025145,0.00025524,-9.9141e-05,-0.00032537,-0.00010202,0.00027143,0.00027538,-0.00010686,-0.00035082,-0.00010991,0.00029231,0.00029658,-0.00011506,-0.00037738,-0.00011826,0.00031431,0.00031864,-0.00012355,-0.00040528,-0.00012685,0.00033715,0.00034176,-0.0001325,-0.00043418,-0.00013594,0.00036101,0.00036569,-0.00014167,-0.00046437,-0.00014526,0.00038573,0.00039074,-0.00015138,-0.00049572,-0.00015508,0.00041162,0.00041667,-0.00016133,-0.00052835,-0.00016516,0.00043825,0.00044361,-0.00017172,-0.00056205,-0.00017571,0.00046614,0.00047159,-0.00018254,-0.00059729,-0.00018658,0.00049478,0.00050042,-0.00019364,-0.0006335,-0.00019795,0.00052469,0.00053036,-0.00020507,-0.00067104,-0.00020949,0.00055529,0.00056146,-0.00021709,-0.00070968,-0.00022163,0.00058712,0.00059331,-0.0002293,-0.0007497,-0.00023398,0.00061976,0.00062626,-0.000242,-0.00079077,-0.00024681,0.00065347,0.00066005,-0.00025496,-0.00083315,-0.00025989,0.00068806,0.00069491,-0.0002684,-0.0008766,-0.00027345,0.00072367,0.0007306,-0.00028205,-0.00092125,-0.00028727,0.00076007,0.0007673,-0.00029618,-0.00096698,-0.00030152,0.00079755,0.00080484,-0.00031058,-0.0010139,-0.00031602,0.00083573,0.00084328,-0.00032534,-0.0010618,-0.00033094,0.00087497,0.00088259,-0.00034039,-0.0011108,-0.00034607,0.00091485,0.00092278,-0.00035586,-0.0011608,-0.00036162,0.00095569,0.00096368,-0.00037154,-0.0012119,-0.00037745,0.00099728,0.0010054,-0.00038755,-0.0012638,-0.00039357,0.0010397,0.0010479,-0.00040384,-0.0013168,-0.00040994,0.0010827,0.0010911,-0.00042043,-0.0013705,-0.00042663,0.0011265,0.001135,-0.00043722,-0.0014251,-0.00044353,0.0011709,0.0011796,-0.00045432,-0.0014805,-0.00046071,0.001216,0.0012248,-0.0004716,-0.0015366,-0.00047808,0.0012616,0.0012705,-0.00048912,-0.0015934,-0.00049568,0.0013078,0.0013168,-0.00050683,-0.0016508,-0.00051345,0.0013544,0.0013635,-0.00052474,-0.0017089,-0.00053141,0.0014015,0.0014107,-0.00054278,-0.0017674,-0.00054953,0.001449,0.0014582,-0.00056098,-0.0018263,-0.00056777,0.0014969,0.0015061,-0.0005793,-0.0018857,-0.00058611,0.001545,0.0015544,-0.00059775,-0.0019454,-0.0006046,0.0015934,0.0016028,-0.00061624,-0.0020054,-0.00062312,0.001642,0.0016514,-0.00063486,-0.0020655,-0.00064173,0.0016907,0.0017001,-0.00065346,-0.0021258,-0.00066036,0.0017395,0.001749,-0.00067214,-0.0021862,-0.00067903,0.0017884,0.0017978,-0.00069078,-0.0022466,-0.00069768,0.0018372,0.0018467,-0.00070944,-0.0023069,-0.00071632,0.001886,0.0018954,-0.00072801,-0.002367,-0.00073488,0.0019346,0.0019439,-0.00074656,-0.002427,-0.00075339,0.001983,0.0019923,-0.00076498,-0.0024866,-0.00077178,0.0020311,0.0020403,-0.00078333,-0.0025458,-0.0007901,0.0020789,0.0020881,-0.00080152,-0.0026046,-0.00080822,0.0021263,0.0021354,-0.00081956,-0.0026629,-0.00082621,0.0021732,0.0021822,-0.0008374,-0.0027206,-0.00084397,0.0022196,0.0022286,-0.00085507,-0.0027775,-0.00086155,0.0022655,0.0022743,-0.00087247,-0.0028338,-0.00087887,0.0023107,0.0023194,-0.00088966,-0.0028892,-0.00089595,0.0023553,0.0023638,-0.00090653,-0.0029437,-0.00091272,0.002399,0.0024074,-0.00092315,-0.0029972,-0.00092921,0.002442,0.0024502,-0.0009394,-0.0030496,-0.00094535,0.0024841,0.0024921,-0.00095535,-0.003101,-0.00096116,0.0025252,0.002533,-0.00097091,-0.0031511,-0.00097657,0.0025654,0.002573,-0.00098608,-0.0032,-0.00099161,0.0026045,0.0026119,-0.0010008,-0.0032475,-0.0010062,0.0026424,0.0026497,-0.0010152,-0.0032936,-0.0010204,0.0026793,0.0026862,-0.001029,-0.0033382,-0.0010341,0.0027149,0.0027216,-0.0010424,-0.0033813,-0.0010473,0.0027492,0.0027557,-0.0010553,-0.0034227,-0.00106,0.0027822,0.0027884,-0.0010677,-0.0034625,-0.0010722,0.0028138,0.0028198,-0.0010796,-0.0035006,-0.0010839,0.002844,0.0028497,-0.0010909,-0.0035368,-0.001095,0.0028728,0.0028782,-0.0011016,-0.0035713,-0.0011055,0.0029,0.0029051,-0.0011118,-0.0036038,-0.0011154,0.0029257,0.0029305,-0.0011214,-0.0036344,-0.0011248,0.0029498,0.0029543,-0.0011303,-0.003663,-0.0011335,0.0029723,0.0029765,-0.0011387,-0.0036896,-0.0011416,0.0029932,0.002997,-0.0011464,-0.0037141,-0.0011491,0.0030123,0.0030158,-0.0011534,-0.0037366,-0.0011559,0.0030298,0.0030329,-0.0011598,-0.0037568,-0.001162,0.0030455,0.0030483,-0.0011655,-0.0037749,-0.0011675,0.0030594,0.0030619,-0.0011706,-0.0037908,-0.0011723,0.0030715,0.0030737,-0.0011749,-0.0038045,-0.0011764,0.0030819,0.0030837,-0.0011786,-0.003816,-0.0011798,0.0030904,0.0030918,-0.0011816,-0.0038251,-0.0011825,0.0030971,0.0030981,-0.0011838,-0.003832,-0.0011845,0.0031019,0.0031026,-0.0011854,-0.0038366,-0.0011858,0.0031049,0.0031052,-0.0011862,-0.0038389,-0.0011863,0.003106,0.003106,-0.0011863,-0.0038389,-0.0011862,0.0031052,0.0031049,-0.0011858,-0.0038366,-0.0011854,0.0031026,0.0031019,-0.0011845,-0.003832,-0.0011838,0.0030981,0.0030971,-0.0011825,-0.0038251,-0.0011816,0.0030918,0.0030904,-0.0011798,-0.003816,-0.0011786,0.0030837,0.0030819,-0.0011764,-0.0038045,-0.0011749,0.0030737,0.0030715,-0.0011723,-0.0037908,-0.0011706,0.0030619,0.0030594,-0.0011675,-0.0037749,-0.0011655,0.0030483,0.0030455,-0.001162,-0.0037568,-0.0011598,0.0030329,0.0030298,-0.0011559,-0.0037366,-0.0011534,0.0030158,0.0030123,-0.0011491,-0.0037141,-0.0011464,0.002997,0.0029932,-0.0011416,-0.0036896,-0.0011387,0.0029765,0.0029723,-0.0011335,-0.003663,-0.0011303,0.0029543,0.0029498,-0.0011248,-0.0036344,-0.0011214,0.0029305,0.0029257,-0.0011154,-0.0036038,-0.0011118,0.0029051,0.0029,-0.0011055,-0.0035713,-0.0011016,0.0028782,0.0028728,-0.001095,-0.0035368,-0.0010909,0.0028497,0.002844,-0.0010839,-0.0035006,-0.0010796,0.0028198,0.0028138,-0.0010722,-0.0034625,-0.0010677,0.0027884,0.0027822,-0.00106,-0.0034227,-0.0010553,0.0027557,0.0027492,-0.0010473,-0.0033813,-0.0010424,0.0027216,0.0027149,-0.0010341,-0.0033382,-0.001029,0.0026862,0.0026793,-0.0010204,-0.0032936,-0.0010152,0.0026497,0.0026424,-0.0010062,-0.0032475,-0.0010008,0.0026119,0.0026045,-0.00099161,-0.0032,-0.00098608,0.002573,0.0025654,-0.00097657,-0.0031511,-0.00097091,0.002533,0.0025252,-0.00096116,-0.003101,-0.00095535,0.0024921,0.0024841,-0.00094535,-0.0030496,-0.0009394,0.0024502,0.002442,-0.00092921,-0.0029972,-0.00092315,0.0024074,0.002399,-0.00091272,-0.0029437,-0.00090653,0.0023638,0.0023553,-0.00089595,-0.0028892,-0.00088966,0.0023194,0.0023107,-0.00087887,-0.0028338,-0.00087247,0.0022743,0.0022655,-0.00086155,-0.0027775,-0.00085507,0.0022286,0.0022196,-0.00084397,-0.0027206,-0.0008374,0.0021822,0.0021732,-0.00082621,-0.0026629,-0.00081956,0.0021354,0.0021263,-0.00080822,-0.0026046,-0.00080152,0.0020881,0.0020789,-0.0007901,-0.0025458,-0.00078333,0.0020403,0.0020311,-0.00077178,-0.0024866,-0.00076498,0.0019923,0.001983,-0.00075339,-0.002427,-0.00074656,0.0019439,0.0019346,-0.00073488,-0.002367,-0.00072801,0.0018954,0.001886,-0.00071632,-0.0023069,-0.00070944,0.0018467,0.0018372,-0.00069768,-0.0022466,-0.00069078,0.0017978,0.0017884,-0.00067903,-0.0021862,-0.00067214,0.001749,0.0017395,-0.00066036,-0.0021258,-0.00065346,0.0017001,0.0016907,-0.00064173,-0.0020655,-0.00063486,0.0016514,0.001642,-0.00062312,-0.0020054,-0.00061624,0.0016028,0.0015934,-0.0006046,-0.0019454,-0.00059775,0.0015544,0.001545,-0.00058611,-0.0018857,-0.0005793,0.0015061,0.0014969,-0.00056777,-0.0018263,-0.00056098,0.0014582,0.001449,-0.00054953,-0.0017674,-0.00054278,0.0014107,0.0014015,-0.00053141,-0.0017089,-0.00052474,0.0013635,0.0013544,-0.00051345,-0.0016508,-0.00050683,0.0013168,0.0013078,-0.00049568,-0.0015934,-0.00048912,0.0012705,0.0012616,-0.00047808,-0.0015366,-0.0004716,0.0012248,0.001216,-0.00046071,-0.0014805,-0.00045432,0.0011796,0.0011709,-0.00044353,-0.0014251,-0.00043722,0.001135,0.0011265,-0.00042663,-0.0013705,-0.00042043,0.0010911,0.0010827,-0.00040994,-0.0013168,-0.00040384,0.0010479,0.0010397,-0.00039357,-0.0012638,-0.00038755,0.0010054,0.00099728,-0.00037745,-0.0012119,-0.00037154,0.00096368,0.00095569,-0.00036162,-0.0011608,-0.00035586,0.00092278,0.00091485,-0.00034607,-0.0011108,-0.00034039,0.00088259,0.00087497,-0.00033094,-0.0010618,-0.00032534,0.00084328,0.00083573,-0.00031602,-0.0010139,-0.00031058,0.00080484,0.00079755,-0.00030152,-0.00096698,-0.00029618,0.0007673,0.00076007,-0.00028727,-0.00092125,-0.00028205,0.0007306,0.00072367,-0.00027345,-0.0008766,-0.0002684,0.00069491,0.00068806,-0.00025989,-0.00083315,-0.00025496,0.00066005,0.00065347,-0.00024681,-0.00079077,-0.000242,0.00062626,0.00061976,-0.00023398,-0.0007497,-0.0002293,0.00059331,0.00058712,-0.00022163,-0.00070968,-0.00021709,0.00056146,0.00055529,-0.00020949,-0.00067104,-0.00020507,0.00053036,0.00052469,-0.00019795,-0.0006335,-0.00019364,0.00050042,0.00049478,-0.00018658,-0.00059729,-0.00018254,0.00047159,0.00046614,-0.00017571,-0.00056205,-0.00017172,0.00044361,0.00043825,-0.00016516,-0.00052835,-0.00016133,0.00041667,0.00041162,-0.00015508,-0.00049572,-0.00015138,0.00039074,0.00038573,-0.00014526,-0.00046437,-0.00014167,0.00036569,0.00036101,-0.00013594,-0.00043418,-0.0001325,0.00034176,0.00033715,-0.00012685,-0.00040528,-0.00012355,0.00031864,0.00031431,-0.00011826,-0.00037738,-0.00011506,0.00029658,0.00029231,-0.00010991,-0.00035082,-0.00010686,0.00027538,0.00027143,-0.00010202,-0.00032537,-9.9141e-05,0.00025524,0.00025145,-9.4488e-05,-0.00030094,-9.198e-05,0.00023649,0.00023246,-8.7161e-05,-0.00027745,-8.4539e-05,0.00021774,0.00021427,-8.039e-05,-0.00025643,-7.7919e-05,0.00020037,0.00019711,-7.398e-05,-0.0002351,-7.1457e-05,0.00018374,0.00018041,-6.7684e-05,-0.00021539,-6.5322e-05,0.00016806,0.00016521,-6.1956e-05,-0.00019677,-5.9818e-05,0.00015345,0.00015056,-5.6301e-05,-0.00017917,-5.4296e-05,0.00013927,0.00013672,-5.1153e-05,-0.00016218,-4.9221e-05,0.00012606,0.00012342,-4.611e-05,-0.00014645,-4.4265e-05,0.00011351,0.00011126,-4.1613e-05,-0.00013163,-3.9889e-05,0.00010207,9.9772e-05,-3.7218e-05,-0.00011807,-3.5625e-05,9.1204e-05,8.9269e-05,-3.3321e-05,-0.00010518,-3.1833e-05,8.1224e-05,7.9182e-05,-2.5561e-05,-0.00097887,-4.2759e-06}; 00087 //Input sizes of matrices (FIR filter order, number of coefficients): 00088 int n1 = 958; 00089 int n2 = 958; 00090 //Largest of the two FIR coefficient matrices: 00091 int nmax; 00092 00093 //Auxiliary constants 00094 int i, j, k, l, m, n, o, p, q; //used as a counters in for loops 00095 //*************************************************************************************************************************************************************************************************************************** 00096 00097 00098 //Size of largest of the two matrices: 00099 void Max_size(){ 00100 if( n1 > n2 ){ 00101 nmax = n1; 00102 } 00103 else{ 00104 nmax = n2; 00105 } 00106 } 00107 00108 00109 //Sinewave function. returns sine with desired dither amplitude. Used for DAC_BiasandDither 00110 void Sinewave(){ 00111 for( i = 0; i <= 19; i++ ) { 00112 sineout[i] = Vp * sinecoeff[i] / 3.3; 00113 } 00114 } 00115 00116 //Function that moves adc value to shift register. Must do this function every sample time: 00117 void ADC_register(){ 00118 if (j > 0){ 00119 analogValuein = adc; 00120 //pc.printf("%f\n", analogValuein); 00121 shiftArray[j-1] = analogValuein; 00122 //pc.printf("shiftArray[%i]: %f\n", j-1, shiftArray[j-1]); 00123 j--; 00124 } 00125 } 00126 00127 00128 //void Print_shift_register(){ 00129 // for( int z = 0; z < 1000; z++ ) { 00130 //pc.printf("shiftArray[%i]:%E\n", z, shiftArray[z]); 00131 // } 00132 //} 00133 00134 00135 //FIR fundamental. Multiply taps by coefficients and then stores them in array: 00136 void FIR_fundamental(){ 00137 for( k = 0; k < RMS_cycles; k++ ){ 00138 FIRfundArray[k] = 0; 00139 for( l = 0; l < n1; l++ ) { 00140 FIRfundArray[k] += shiftArray[l + k] * FIR_coefficients_1[l]; 00141 //pc.printf("k:%i l:%i %E %E %E\n", k, l, FIRfundArray[k], shiftArray[l + k], FIR_coefficients_1[l]); 00142 } 00143 //pc.printf("FIRfundArray[%i]: %E\n", k, FIRfundArray[k]); 00144 } 00145 } 00146 00147 00148 //FIR 2nd harmonic. Multiplies taps by coefficients and then stores them in array: 00149 void FIR_2nd(){ 00150 for( m = 0; m < RMS_cycles; m++ ){ 00151 FIR2ndArray[m] = 0; 00152 for( n = 0; n < n2; n++ ) { 00153 FIR2ndArray[m] += shiftArray[n + m] * FIR_coefficients_2[n]; 00154 //pc.printf("m:%i n:%i %E %E %E\n", m, n, FIR2ndArray[m], shiftArray[n + m], FIR_coefficients_2[n]); 00155 } 00156 //pc.printf("FIR2ndArray[%i]: %E\n", m, FIR2ndArray[m]); 00157 } 00158 } 00159 00160 00161 //Calculates ratio of RMS of 2nd harmonic and fundamental and finds ratio: 00162 void Ratio_21(){ 00163 sum_squares_1 = 0; 00164 sum_squares_2 = 0; 00165 for( o = 0; o < RMS_cycles; o++ ) { 00166 sum_squares_1 += FIRfundArray[o] * FIRfundArray[o]; 00167 //pc.printf("sum_squares_1:%E FIRfundArray[%i]:%E\n", sum_squares_1, o, FIRfundArray[o]); 00168 sum_squares_2 += FIR2ndArray[o] * FIR2ndArray[o]; 00169 //pc.printf("sum_squares_2:%E FIR2ndArray[%i]:%E\n", sum_squares_2, o, FIR2ndArray[o]); 00170 } 00171 rms1 = sqrt(sum_squares_1/RMS_cycles); 00172 rms2 = sqrt(sum_squares_2/RMS_cycles); 00173 ratio = 1.2735*rms2 / rms1; // 1.2735 added to compensate for LPF fc=1872Hz 00174 pc.printf("rms1:%E rms2:%E ratio:%E\n", rms1, rms2, ratio); 00175 } 00176 00177 00178 //PID controller: 00179 void PID_control(){ 00180 loop_error = ratio - setpoint; // Calculate error 00181 if (loop_error < errormin){ 00182 loop_error = 0; 00183 } 00184 //pc.printf("loop error:%E setpoint:%E ratio:%E\n", loop_error, setpoint, ratio); 00185 Pout = Kp * loop_error; // Proportional term 00186 integral += loop_error * dspsampletime; // Integral term 00187 Iout = Ki * integral; 00188 derivative = (loop_error - pre_error) / dspsampletime; // Derivative term 00189 Dout = Kd * derivative; 00190 PIDoutput = Pout + Iout + Dout; // Calculate total output 00191 //pc.printf("Pout:%E Iout:%E Dout:%E PIDoutput:%E\n", Pout, Iout, Dout, PIDoutput); 00192 if( PIDoutput > max ){ // Restrict to max/min 00193 PIDoutput = max; 00194 } 00195 else if( PIDoutput < min ){ 00196 PIDoutput = min; 00197 } 00198 pre_error = loop_error; // Save error to previous error 00199 } 00200 00201 00202 //DSP cycle: 00203 void DSPandControl_cycle(){ 00204 FIR_fundamental(); 00205 FIR_2nd(); 00206 Ratio_21(); 00207 PID_control(); 00208 if (q == 1){ 00209 biasdc = PIDoutput; //0 <= biasdc <= 1; 0.0 = 0V, 1.0f = 3.3V 00210 } 00211 } 00212 00213 00214 //DAC conversion function. Outputs sine-wave with DC bias: 00215 void DAC_BiasandDither(){ 00216 00217 if( p == 20 ){ 00218 p = 0; 00219 } 00220 dac = sineout[p] + biasdc; //instead of constant, this should be ...+ biasdc 00221 p++; 00222 } 00223 00224 00225 int main() { 00226 00227 Max_size(); 00228 00229 Sinewave(); 00230 00231 DACperiod.attach(&DAC_BiasandDither, dactime ); //Attaches ticker to ADC_read function with a fixed sample time "sampletime" 00232 00233 ADCperiod.attach(&ADC_register, sampletime ); //Attaches ticker to ADC_read function with a fixed sample time "sampletime" 00234 00235 j = nmax + RMS_cycles - 1; 00236 00237 q = 0; 00238 00239 //pc.printf("ratio,loop_error,Pout,Iout,Dout,PIDoutput,biasdc\n"); 00240 00241 while(1) { 00242 00243 wait(0.1); 00244 00245 if(j == 0){ 00246 00247 //After ADC values are obtained, the dsptime is calculated for the PID controller: 00248 dspsampletime = DSPtimer.read(); 00249 //pc.printf("dspsampletime:%f\n", dspsampletime); //prints analog value to serial port 00250 DSPtimer.reset(); 00251 DSPtimer.start(); 00252 00253 DSPandControl_cycle(); 00254 //pc.printf("%E,%E,%E,%E,%E,%E,%f\n",ratio,loop_error,Pout,Iout,Dout,PIDoutput,biasdc); //prints analog value to serial port 00255 j = nmax + RMS_cycles - 1; 00256 00257 q = 1; 00258 00259 } 00260 00261 } 00262 00263 }
Generated on Mon Jul 25 2022 06:52:56 by 1.7.2