File content as of revision 0:0eea2c4741ec:

#include "mbed.h"
#include "FastAnalogIn.h"


// Tickers, timers - used for ensuring fixed times:
Ticker ADCperiod;                   //Creates ticker class "Sampletime" used for ADC conversion
Ticker DACperiod;                   //Creates ticker class "DACperiod" used for DAC conversion
Timer DSPtimer;                     //Creates timer class "DSPperiod" used for DSP time. Time is needed for derivative and integral components of PID controller.


// Serial communication:
Serial pc(USBTX, USBRX); //tx,rx

//*************************************************************************Pin designations *************************************************************************************************
// ADC pin designation. FastAnalogIn runs ADC in "burst" mode (DMA):
FastAnalogIn adc(PTE20); //float. 0 <=adc <= 1; 0 = 0V, 1.0f = 3.3V
// Other analog inputs are set as digital outputs for better ADC performance:
DigitalOut dig1(PTE22) = 0;
DigitalOut dig2(PTE21);
DigitalOut dig3(PTE29);
DigitalOut dig4(PTE30);
DigitalOut dig5(PTE23);
DigitalOut dig6(PTB0); 
DigitalOut dig7(PTB1); 
DigitalOut dig8(PTB2);
DigitalOut dig9(PTB3);  
DigitalOut dig10(PTC0); 
DigitalOut dig11(PTC1); 
DigitalOut dig12(PTC2);  
DigitalOut dig13(PTD1);  
DigitalOut dig14(PTD5); 
DigitalOut dig15(PTD6);  
//DAC pin designation. 
AnalogOut dac(PTE30);   //float. 0 <=dac <= 1; 0 = 0V, 1.0f = 3.3V. Also used for debugging DSP_control_cycle
//*******************************************************************************************************************************************************************************************


//**********************************************************************Constants and variables**********************************************************************************************
//Frequencies and periods:
float sampletime = 0.0001;      //ADC sample time [s]. Should be 1/Fs.
double dactime = 5e-05;      //DAC time. Should be 1/(freq1 * sinepoints)

//ADC parameters:
float analogValuein;            //defines variable where ADC conversion will be stored
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

//RMS detector (envelope detection):
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;
double FIRfundArray[100];         //array that stores several values of FIRfundoutput in order to find RMS. MinSize = RMS_cycles
double FIR2ndArray[100];          //a   rray that stores several values of FIR2ndoutput in order to find RMS. MinSize = RMS_cycles
double rms1;                     //root mean square of fundamental frequency FIR filter output
double rms2;                     //root mean square of 2nd harmonic FIR filter output
double sum_squares_1;            //sum of squares of fundamental
double sum_squares_2;            //sum of squares of 2nd harmonic
double ratio;                    //ratio of rms1 and rms2

//PID controller:
double dspsampletime;            //Stores PID sample size
double setpoint = 0;             //PID controller set point. 0 for quadrature for MZM transfer function. Goes to infinity at 0, 180, ..
double Kp = 0.001;                 //PID proportional coefficient
double Ki= 0.0005;                  //PID integral coefficient
double Kd= 0.01;                  //PID derivative coefficient
double Pout;                     //PID proportional output
double Iout;                     //PID integral output
double Dout;                     //PID derivative output
double loop_error;               //PID control loop error
double pre_error;                //PID control error from previous cycle, used for the derivative output
double integral;                 //PID integral of error
double derivative;               //PID derivative of error
double PIDoutput;                //PID output. This is then taken as the DC bias voltage.
double max = 1;                  //PID output maximum value. 
double min = 0;                  //PID output minimum value.
double errormin = 3e-2;          //PID minimum error. Taken from sine experiment. This is the minimum value above noise.

//DAC parameters:
float analogValueout;   //defines variable where DAC output value will be stored
//stores normalized sine wave coefficients (amplitude of 1.0f = 3.3V):
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};              
float Vp = 0.5;           //dither sinewave peak voltage [V]. This is the magnitude before OP AMP voltage shifter.
double sineout[20];      //will contain sinewave with bias offset
double biasdc = 0.5;           //biasdc voltage [V]. V = biasdc * 3.3V

//FIR filter coefficients from matlab FDtool for fundamental:
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};
//FIR filter coefficients from matlab FDtool for 2nd harmonic:
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};
//Input sizes of matrices (FIR filter order, number of coefficients):
int n1 = 958;
int n2 = 958;
//Largest of the two FIR coefficient matrices:
int nmax;

//Auxiliary constants
int i, j, k, l, m, n, o, p, q; //used as a counters in for loops                    
//***************************************************************************************************************************************************************************************************************************


//Size of largest of the two matrices:
void Max_size(){
    if( n1 > n2 ){
        nmax = n1;
        }
        else{
            nmax = n2;
            }
    }   


//Sinewave function. returns sine with desired dither amplitude. Used for DAC_BiasandDither
void Sinewave(){
    for( i = 0; i <= 19; i++ ) {
        sineout[i] = Vp * sinecoeff[i] / 3.3;
        }
    }

//Function that moves adc value to shift register. Must do this function every sample time:
void ADC_register(){
    if (j > 0){
        analogValuein = adc;
        //pc.printf("%f\n", analogValuein);
        shiftArray[j-1] = analogValuein;
        //pc.printf("shiftArray[%i]: %f\n", j-1, shiftArray[j-1]);
        j--;
        }
    }


//void Print_shift_register(){
       // for( int z = 0; z < 1000; z++ ) {
            //pc.printf("shiftArray[%i]:%E\n", z, shiftArray[z]);
           // } 
        //}


//FIR fundamental. Multiply taps by coefficients and then stores them in array:
void FIR_fundamental(){
    for( k = 0; k < RMS_cycles; k++ ){
        FIRfundArray[k] = 0;
        for( l = 0; l < n1; l++ ) {
            FIRfundArray[k] += shiftArray[l + k] * FIR_coefficients_1[l];
            //pc.printf("k:%i l:%i    %E   %E  %E\n", k, l, FIRfundArray[k], shiftArray[l + k], FIR_coefficients_1[l]);
            }
        //pc.printf("FIRfundArray[%i]: %E\n", k, FIRfundArray[k]);
        }
    }
        

//FIR 2nd harmonic. Multiplies taps by coefficients and then stores them in array:
void FIR_2nd(){
    for( m = 0; m < RMS_cycles; m++ ){
        FIR2ndArray[m] = 0;
        for( n = 0; n < n2; n++ ) {
            FIR2ndArray[m] += shiftArray[n + m] * FIR_coefficients_2[n];
            //pc.printf("m:%i n:%i    %E   %E  %E\n", m, n, FIR2ndArray[m], shiftArray[n + m], FIR_coefficients_2[n]);
            }
        //pc.printf("FIR2ndArray[%i]: %E\n", m, FIR2ndArray[m]);
        }
    }
    

//Calculates ratio of RMS of 2nd harmonic and fundamental and finds ratio:
void Ratio_21(){
    sum_squares_1 = 0;
    sum_squares_2 = 0;
    for( o = 0; o < RMS_cycles; o++ ) {
        sum_squares_1 += FIRfundArray[o] * FIRfundArray[o];
        //pc.printf("sum_squares_1:%E     FIRfundArray[%i]:%E\n", sum_squares_1, o, FIRfundArray[o]);
        sum_squares_2 += FIR2ndArray[o] * FIR2ndArray[o];
        //pc.printf("sum_squares_2:%E     FIR2ndArray[%i]:%E\n", sum_squares_2, o, FIR2ndArray[o]);
        }
    rms1 = sqrt(sum_squares_1/RMS_cycles);
    rms2 = sqrt(sum_squares_2/RMS_cycles);
    ratio = 1.2735*rms2 / rms1;   // 1.2735 added to compensate for LPF fc=1872Hz
    pc.printf("rms1:%E  rms2:%E ratio:%E\n", rms1, rms2, ratio);
    }


//PID controller:
void PID_control(){
    loop_error = ratio - setpoint;   // Calculate error
    if (loop_error < errormin){
        loop_error = 0;
        }
    //pc.printf("loop error:%E  setpoint:%E   ratio:%E\n", loop_error, setpoint, ratio);
    Pout = Kp * loop_error;  // Proportional term   
    integral += loop_error * dspsampletime;   // Integral term
    Iout = Ki * integral;
    derivative = (loop_error - pre_error) / dspsampletime; // Derivative term
    Dout = Kd * derivative;
    PIDoutput = Pout + Iout + Dout; // Calculate total output
    //pc.printf("Pout:%E     Iout:%E     Dout:%E    PIDoutput:%E\n", Pout, Iout, Dout, PIDoutput);
    if( PIDoutput > max ){ // Restrict to max/min
        PIDoutput = max;
        }
        else if( PIDoutput < min ){
            PIDoutput = min;
            }
    pre_error = loop_error; // Save error to previous error
    }
    

//DSP cycle:
void DSPandControl_cycle(){
    FIR_fundamental();
    FIR_2nd();
    Ratio_21();  
    PID_control();
    if (q == 1){
        biasdc = PIDoutput; //0 <= biasdc <= 1; 0.0 = 0V, 1.0f = 3.3V
        }
    }
    

//DAC conversion function. Outputs sine-wave with DC bias:
void DAC_BiasandDither(){
    
    if( p == 20 ){
        p = 0;
        }
    dac = sineout[p] + biasdc;   //instead of constant, this should be ...+ biasdc
    p++;
    }

    
int main() {
    
    Max_size();
    
    Sinewave();
    
    DACperiod.attach(&DAC_BiasandDither, dactime );  //Attaches ticker to ADC_read function with a fixed sample time "sampletime"
    
    ADCperiod.attach(&ADC_register, sampletime );  //Attaches ticker to ADC_read function with a fixed sample time "sampletime"
    
    j = nmax + RMS_cycles - 1;
    
    q = 0;
    
    //pc.printf("ratio,loop_error,Pout,Iout,Dout,PIDoutput,biasdc\n");
    
    while(1) {
            
            wait(0.1);
                
            if(j == 0){
                 
                //After ADC values are obtained, the dsptime is calculated for the PID controller: 
                dspsampletime = DSPtimer.read();
                //pc.printf("dspsampletime:%f\n", dspsampletime);   //prints analog value to serial port
                DSPtimer.reset();
                DSPtimer.start();
                
                DSPandControl_cycle();
                //pc.printf("%E,%E,%E,%E,%E,%E,%f\n",ratio,loop_error,Pout,Iout,Dout,PIDoutput,biasdc);   //prints analog value to serial port
                j = nmax + RMS_cycles - 1;
                
                q = 1;
                
                }
            
    }
    
}