Hector Andrade / Mbed 2 deprecated MBC

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@0:0eea2c4741ec, 2017-02-20 (annotated)

Committer:
handrade88
Date:
Mon Feb 20 05:37:50 2017 +0000
Revision:
0:0eea2c4741ec
_

Who changed what in which revision?

UserRevisionLine numberNew contents of line
handrade88 0:0eea2c4741ec 1 #include "mbed.h"
handrade88 0:0eea2c4741ec 2 #include "FastAnalogIn.h"
handrade88 0:0eea2c4741ec 3
handrade88 0:0eea2c4741ec 4
handrade88 0:0eea2c4741ec 5 // Tickers, timers - used for ensuring fixed times:
handrade88 0:0eea2c4741ec 6 Ticker ADCperiod; //Creates ticker class "Sampletime" used for ADC conversion
handrade88 0:0eea2c4741ec 7 Ticker DACperiod; //Creates ticker class "DACperiod" used for DAC conversion
handrade88 0:0eea2c4741ec 8 Timer DSPtimer; //Creates timer class "DSPperiod" used for DSP time. Time is needed for derivative and integral components of PID controller.
handrade88 0:0eea2c4741ec 9
handrade88 0:0eea2c4741ec 10
handrade88 0:0eea2c4741ec 11 // Serial communication:
handrade88 0:0eea2c4741ec 12 Serial pc(USBTX, USBRX); //tx,rx
handrade88 0:0eea2c4741ec 13
handrade88 0:0eea2c4741ec 14 //*************************************************************************Pin designations *************************************************************************************************
handrade88 0:0eea2c4741ec 15 // ADC pin designation. FastAnalogIn runs ADC in "burst" mode (DMA):
handrade88 0:0eea2c4741ec 16 FastAnalogIn adc(PTE20); //float. 0 <=adc <= 1; 0 = 0V, 1.0f = 3.3V
handrade88 0:0eea2c4741ec 17 // Other analog inputs are set as digital outputs for better ADC performance:
handrade88 0:0eea2c4741ec 18 DigitalOut dig1(PTE22) = 0;
handrade88 0:0eea2c4741ec 19 DigitalOut dig2(PTE21);
handrade88 0:0eea2c4741ec 20 DigitalOut dig3(PTE29);
handrade88 0:0eea2c4741ec 21 DigitalOut dig4(PTE30);
handrade88 0:0eea2c4741ec 22 DigitalOut dig5(PTE23);
handrade88 0:0eea2c4741ec 23 DigitalOut dig6(PTB0);
handrade88 0:0eea2c4741ec 24 DigitalOut dig7(PTB1);
handrade88 0:0eea2c4741ec 25 DigitalOut dig8(PTB2);
handrade88 0:0eea2c4741ec 26 DigitalOut dig9(PTB3);
handrade88 0:0eea2c4741ec 27 DigitalOut dig10(PTC0);
handrade88 0:0eea2c4741ec 28 DigitalOut dig11(PTC1);
handrade88 0:0eea2c4741ec 29 DigitalOut dig12(PTC2);
handrade88 0:0eea2c4741ec 30 DigitalOut dig13(PTD1);
handrade88 0:0eea2c4741ec 31 DigitalOut dig14(PTD5);
handrade88 0:0eea2c4741ec 32 DigitalOut dig15(PTD6);
handrade88 0:0eea2c4741ec 33 //DAC pin designation.
handrade88 0:0eea2c4741ec 34 AnalogOut dac(PTE30); //float. 0 <=dac <= 1; 0 = 0V, 1.0f = 3.3V. Also used for debugging DSP_control_cycle
handrade88 0:0eea2c4741ec 35 //*******************************************************************************************************************************************************************************************
handrade88 0:0eea2c4741ec 36
handrade88 0:0eea2c4741ec 37
handrade88 0:0eea2c4741ec 38 //**********************************************************************Constants and variables**********************************************************************************************
handrade88 0:0eea2c4741ec 39 //Frequencies and periods:
handrade88 0:0eea2c4741ec 40 float sampletime = 0.0001; //ADC sample time [s]. Should be 1/Fs.
handrade88 0:0eea2c4741ec 41 double dactime = 5e-05; //DAC time. Should be 1/(freq1 * sinepoints)
handrade88 0:0eea2c4741ec 42
handrade88 0:0eea2c4741ec 43 //ADC parameters:
handrade88 0:0eea2c4741ec 44 float analogValuein; //defines variable where ADC conversion will be stored
handrade88 0:0eea2c4741ec 45 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
handrade88 0:0eea2c4741ec 46
handrade88 0:0eea2c4741ec 47 //RMS detector (envelope detection):
handrade88 0:0eea2c4741ec 48 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;
handrade88 0:0eea2c4741ec 49 double FIRfundArray[100]; //array that stores several values of FIRfundoutput in order to find RMS. MinSize = RMS_cycles
handrade88 0:0eea2c4741ec 50 double FIR2ndArray[100]; //a rray that stores several values of FIR2ndoutput in order to find RMS. MinSize = RMS_cycles
handrade88 0:0eea2c4741ec 51 double rms1; //root mean square of fundamental frequency FIR filter output
handrade88 0:0eea2c4741ec 52 double rms2; //root mean square of 2nd harmonic FIR filter output
handrade88 0:0eea2c4741ec 53 double sum_squares_1; //sum of squares of fundamental
handrade88 0:0eea2c4741ec 54 double sum_squares_2; //sum of squares of 2nd harmonic
handrade88 0:0eea2c4741ec 55 double ratio; //ratio of rms1 and rms2
handrade88 0:0eea2c4741ec 56
handrade88 0:0eea2c4741ec 57 //PID controller:
handrade88 0:0eea2c4741ec 58 double dspsampletime; //Stores PID sample size
handrade88 0:0eea2c4741ec 59 double setpoint = 0; //PID controller set point. 0 for quadrature for MZM transfer function. Goes to infinity at 0, 180, ..
handrade88 0:0eea2c4741ec 60 double Kp = 0.001; //PID proportional coefficient
handrade88 0:0eea2c4741ec 61 double Ki= 0.0005; //PID integral coefficient
handrade88 0:0eea2c4741ec 62 double Kd= 0.01; //PID derivative coefficient
handrade88 0:0eea2c4741ec 63 double Pout; //PID proportional output
handrade88 0:0eea2c4741ec 64 double Iout; //PID integral output
handrade88 0:0eea2c4741ec 65 double Dout; //PID derivative output
handrade88 0:0eea2c4741ec 66 double loop_error; //PID control loop error
handrade88 0:0eea2c4741ec 67 double pre_error; //PID control error from previous cycle, used for the derivative output
handrade88 0:0eea2c4741ec 68 double integral; //PID integral of error
handrade88 0:0eea2c4741ec 69 double derivative; //PID derivative of error
handrade88 0:0eea2c4741ec 70 double PIDoutput; //PID output. This is then taken as the DC bias voltage.
handrade88 0:0eea2c4741ec 71 double max = 1; //PID output maximum value.
handrade88 0:0eea2c4741ec 72 double min = 0; //PID output minimum value.
handrade88 0:0eea2c4741ec 73 double errormin = 3e-2; //PID minimum error. Taken from sine experiment. This is the minimum value above noise.
handrade88 0:0eea2c4741ec 74
handrade88 0:0eea2c4741ec 75 //DAC parameters:
handrade88 0:0eea2c4741ec 76 float analogValueout; //defines variable where DAC output value will be stored
handrade88 0:0eea2c4741ec 77 //stores normalized sine wave coefficients (amplitude of 1.0f = 3.3V):
handrade88 0:0eea2c4741ec 78 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};
handrade88 0:0eea2c4741ec 79 float Vp = 0.5; //dither sinewave peak voltage [V]. This is the magnitude before OP AMP voltage shifter.
handrade88 0:0eea2c4741ec 80 double sineout[20]; //will contain sinewave with bias offset
handrade88 0:0eea2c4741ec 81 double biasdc = 0.5; //biasdc voltage [V]. V = biasdc * 3.3V
handrade88 0:0eea2c4741ec 82
handrade88 0:0eea2c4741ec 83 //FIR filter coefficients from matlab FDtool for fundamental:
handrade88 0:0eea2c4741ec 84 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};
handrade88 0:0eea2c4741ec 85 //FIR filter coefficients from matlab FDtool for 2nd harmonic:
handrade88 0:0eea2c4741ec 86 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};
handrade88 0:0eea2c4741ec 87 //Input sizes of matrices (FIR filter order, number of coefficients):
handrade88 0:0eea2c4741ec 88 int n1 = 958;
handrade88 0:0eea2c4741ec 89 int n2 = 958;
handrade88 0:0eea2c4741ec 90 //Largest of the two FIR coefficient matrices:
handrade88 0:0eea2c4741ec 91 int nmax;
handrade88 0:0eea2c4741ec 92
handrade88 0:0eea2c4741ec 93 //Auxiliary constants
handrade88 0:0eea2c4741ec 94 int i, j, k, l, m, n, o, p, q; //used as a counters in for loops
handrade88 0:0eea2c4741ec 95 //***************************************************************************************************************************************************************************************************************************
handrade88 0:0eea2c4741ec 96
handrade88 0:0eea2c4741ec 97
handrade88 0:0eea2c4741ec 98 //Size of largest of the two matrices:
handrade88 0:0eea2c4741ec 99 void Max_size(){
handrade88 0:0eea2c4741ec 100 if( n1 > n2 ){
handrade88 0:0eea2c4741ec 101 nmax = n1;
handrade88 0:0eea2c4741ec 102 }
handrade88 0:0eea2c4741ec 103 else{
handrade88 0:0eea2c4741ec 104 nmax = n2;
handrade88 0:0eea2c4741ec 105 }
handrade88 0:0eea2c4741ec 106 }
handrade88 0:0eea2c4741ec 107
handrade88 0:0eea2c4741ec 108
handrade88 0:0eea2c4741ec 109 //Sinewave function. returns sine with desired dither amplitude. Used for DAC_BiasandDither
handrade88 0:0eea2c4741ec 110 void Sinewave(){
handrade88 0:0eea2c4741ec 111 for( i = 0; i <= 19; i++ ) {
handrade88 0:0eea2c4741ec 112 sineout[i] = Vp * sinecoeff[i] / 3.3;
handrade88 0:0eea2c4741ec 113 }
handrade88 0:0eea2c4741ec 114 }
handrade88 0:0eea2c4741ec 115
handrade88 0:0eea2c4741ec 116 //Function that moves adc value to shift register. Must do this function every sample time:
handrade88 0:0eea2c4741ec 117 void ADC_register(){
handrade88 0:0eea2c4741ec 118 if (j > 0){
handrade88 0:0eea2c4741ec 119 analogValuein = adc;
handrade88 0:0eea2c4741ec 120 //pc.printf("%f\n", analogValuein);
handrade88 0:0eea2c4741ec 121 shiftArray[j-1] = analogValuein;
handrade88 0:0eea2c4741ec 122 //pc.printf("shiftArray[%i]: %f\n", j-1, shiftArray[j-1]);
handrade88 0:0eea2c4741ec 123 j--;
handrade88 0:0eea2c4741ec 124 }
handrade88 0:0eea2c4741ec 125 }
handrade88 0:0eea2c4741ec 126
handrade88 0:0eea2c4741ec 127
handrade88 0:0eea2c4741ec 128 //void Print_shift_register(){
handrade88 0:0eea2c4741ec 129 // for( int z = 0; z < 1000; z++ ) {
handrade88 0:0eea2c4741ec 130 //pc.printf("shiftArray[%i]:%E\n", z, shiftArray[z]);
handrade88 0:0eea2c4741ec 131 // }
handrade88 0:0eea2c4741ec 132 //}
handrade88 0:0eea2c4741ec 133
handrade88 0:0eea2c4741ec 134
handrade88 0:0eea2c4741ec 135 //FIR fundamental. Multiply taps by coefficients and then stores them in array:
handrade88 0:0eea2c4741ec 136 void FIR_fundamental(){
handrade88 0:0eea2c4741ec 137 for( k = 0; k < RMS_cycles; k++ ){
handrade88 0:0eea2c4741ec 138 FIRfundArray[k] = 0;
handrade88 0:0eea2c4741ec 139 for( l = 0; l < n1; l++ ) {
handrade88 0:0eea2c4741ec 140 FIRfundArray[k] += shiftArray[l + k] * FIR_coefficients_1[l];
handrade88 0:0eea2c4741ec 141 //pc.printf("k:%i l:%i %E %E %E\n", k, l, FIRfundArray[k], shiftArray[l + k], FIR_coefficients_1[l]);
handrade88 0:0eea2c4741ec 142 }
handrade88 0:0eea2c4741ec 143 //pc.printf("FIRfundArray[%i]: %E\n", k, FIRfundArray[k]);
handrade88 0:0eea2c4741ec 144 }
handrade88 0:0eea2c4741ec 145 }
handrade88 0:0eea2c4741ec 146
handrade88 0:0eea2c4741ec 147
handrade88 0:0eea2c4741ec 148 //FIR 2nd harmonic. Multiplies taps by coefficients and then stores them in array:
handrade88 0:0eea2c4741ec 149 void FIR_2nd(){
handrade88 0:0eea2c4741ec 150 for( m = 0; m < RMS_cycles; m++ ){
handrade88 0:0eea2c4741ec 151 FIR2ndArray[m] = 0;
handrade88 0:0eea2c4741ec 152 for( n = 0; n < n2; n++ ) {
handrade88 0:0eea2c4741ec 153 FIR2ndArray[m] += shiftArray[n + m] * FIR_coefficients_2[n];
handrade88 0:0eea2c4741ec 154 //pc.printf("m:%i n:%i %E %E %E\n", m, n, FIR2ndArray[m], shiftArray[n + m], FIR_coefficients_2[n]);
handrade88 0:0eea2c4741ec 155 }
handrade88 0:0eea2c4741ec 156 //pc.printf("FIR2ndArray[%i]: %E\n", m, FIR2ndArray[m]);
handrade88 0:0eea2c4741ec 157 }
handrade88 0:0eea2c4741ec 158 }
handrade88 0:0eea2c4741ec 159
handrade88 0:0eea2c4741ec 160
handrade88 0:0eea2c4741ec 161 //Calculates ratio of RMS of 2nd harmonic and fundamental and finds ratio:
handrade88 0:0eea2c4741ec 162 void Ratio_21(){
handrade88 0:0eea2c4741ec 163 sum_squares_1 = 0;
handrade88 0:0eea2c4741ec 164 sum_squares_2 = 0;
handrade88 0:0eea2c4741ec 165 for( o = 0; o < RMS_cycles; o++ ) {
handrade88 0:0eea2c4741ec 166 sum_squares_1 += FIRfundArray[o] * FIRfundArray[o];
handrade88 0:0eea2c4741ec 167 //pc.printf("sum_squares_1:%E FIRfundArray[%i]:%E\n", sum_squares_1, o, FIRfundArray[o]);
handrade88 0:0eea2c4741ec 168 sum_squares_2 += FIR2ndArray[o] * FIR2ndArray[o];
handrade88 0:0eea2c4741ec 169 //pc.printf("sum_squares_2:%E FIR2ndArray[%i]:%E\n", sum_squares_2, o, FIR2ndArray[o]);
handrade88 0:0eea2c4741ec 170 }
handrade88 0:0eea2c4741ec 171 rms1 = sqrt(sum_squares_1/RMS_cycles);
handrade88 0:0eea2c4741ec 172 rms2 = sqrt(sum_squares_2/RMS_cycles);
handrade88 0:0eea2c4741ec 173 ratio = 1.2735*rms2 / rms1; // 1.2735 added to compensate for LPF fc=1872Hz
handrade88 0:0eea2c4741ec 174 pc.printf("rms1:%E rms2:%E ratio:%E\n", rms1, rms2, ratio);
handrade88 0:0eea2c4741ec 175 }
handrade88 0:0eea2c4741ec 176
handrade88 0:0eea2c4741ec 177
handrade88 0:0eea2c4741ec 178 //PID controller:
handrade88 0:0eea2c4741ec 179 void PID_control(){
handrade88 0:0eea2c4741ec 180 loop_error = ratio - setpoint; // Calculate error
handrade88 0:0eea2c4741ec 181 if (loop_error < errormin){
handrade88 0:0eea2c4741ec 182 loop_error = 0;
handrade88 0:0eea2c4741ec 183 }
handrade88 0:0eea2c4741ec 184 //pc.printf("loop error:%E setpoint:%E ratio:%E\n", loop_error, setpoint, ratio);
handrade88 0:0eea2c4741ec 185 Pout = Kp * loop_error; // Proportional term
handrade88 0:0eea2c4741ec 186 integral += loop_error * dspsampletime; // Integral term
handrade88 0:0eea2c4741ec 187 Iout = Ki * integral;
handrade88 0:0eea2c4741ec 188 derivative = (loop_error - pre_error) / dspsampletime; // Derivative term
handrade88 0:0eea2c4741ec 189 Dout = Kd * derivative;
handrade88 0:0eea2c4741ec 190 PIDoutput = Pout + Iout + Dout; // Calculate total output
handrade88 0:0eea2c4741ec 191 //pc.printf("Pout:%E Iout:%E Dout:%E PIDoutput:%E\n", Pout, Iout, Dout, PIDoutput);
handrade88 0:0eea2c4741ec 192 if( PIDoutput > max ){ // Restrict to max/min
handrade88 0:0eea2c4741ec 193 PIDoutput = max;
handrade88 0:0eea2c4741ec 194 }
handrade88 0:0eea2c4741ec 195 else if( PIDoutput < min ){
handrade88 0:0eea2c4741ec 196 PIDoutput = min;
handrade88 0:0eea2c4741ec 197 }
handrade88 0:0eea2c4741ec 198 pre_error = loop_error; // Save error to previous error
handrade88 0:0eea2c4741ec 199 }
handrade88 0:0eea2c4741ec 200
handrade88 0:0eea2c4741ec 201
handrade88 0:0eea2c4741ec 202 //DSP cycle:
handrade88 0:0eea2c4741ec 203 void DSPandControl_cycle(){
handrade88 0:0eea2c4741ec 204 FIR_fundamental();
handrade88 0:0eea2c4741ec 205 FIR_2nd();
handrade88 0:0eea2c4741ec 206 Ratio_21();
handrade88 0:0eea2c4741ec 207 PID_control();
handrade88 0:0eea2c4741ec 208 if (q == 1){
handrade88 0:0eea2c4741ec 209 biasdc = PIDoutput; //0 <= biasdc <= 1; 0.0 = 0V, 1.0f = 3.3V
handrade88 0:0eea2c4741ec 210 }
handrade88 0:0eea2c4741ec 211 }
handrade88 0:0eea2c4741ec 212
handrade88 0:0eea2c4741ec 213
handrade88 0:0eea2c4741ec 214 //DAC conversion function. Outputs sine-wave with DC bias:
handrade88 0:0eea2c4741ec 215 void DAC_BiasandDither(){
handrade88 0:0eea2c4741ec 216
handrade88 0:0eea2c4741ec 217 if( p == 20 ){
handrade88 0:0eea2c4741ec 218 p = 0;
handrade88 0:0eea2c4741ec 219 }
handrade88 0:0eea2c4741ec 220 dac = sineout[p] + biasdc; //instead of constant, this should be ...+ biasdc
handrade88 0:0eea2c4741ec 221 p++;
handrade88 0:0eea2c4741ec 222 }
handrade88 0:0eea2c4741ec 223
handrade88 0:0eea2c4741ec 224
handrade88 0:0eea2c4741ec 225 int main() {
handrade88 0:0eea2c4741ec 226
handrade88 0:0eea2c4741ec 227 Max_size();
handrade88 0:0eea2c4741ec 228
handrade88 0:0eea2c4741ec 229 Sinewave();
handrade88 0:0eea2c4741ec 230
handrade88 0:0eea2c4741ec 231 DACperiod.attach(&DAC_BiasandDither, dactime ); //Attaches ticker to ADC_read function with a fixed sample time "sampletime"
handrade88 0:0eea2c4741ec 232
handrade88 0:0eea2c4741ec 233 ADCperiod.attach(&ADC_register, sampletime ); //Attaches ticker to ADC_read function with a fixed sample time "sampletime"
handrade88 0:0eea2c4741ec 234
handrade88 0:0eea2c4741ec 235 j = nmax + RMS_cycles - 1;
handrade88 0:0eea2c4741ec 236
handrade88 0:0eea2c4741ec 237 q = 0;
handrade88 0:0eea2c4741ec 238
handrade88 0:0eea2c4741ec 239 //pc.printf("ratio,loop_error,Pout,Iout,Dout,PIDoutput,biasdc\n");
handrade88 0:0eea2c4741ec 240
handrade88 0:0eea2c4741ec 241 while(1) {
handrade88 0:0eea2c4741ec 242
handrade88 0:0eea2c4741ec 243 wait(0.1);
handrade88 0:0eea2c4741ec 244
handrade88 0:0eea2c4741ec 245 if(j == 0){
handrade88 0:0eea2c4741ec 246
handrade88 0:0eea2c4741ec 247 //After ADC values are obtained, the dsptime is calculated for the PID controller:
handrade88 0:0eea2c4741ec 248 dspsampletime = DSPtimer.read();
handrade88 0:0eea2c4741ec 249 //pc.printf("dspsampletime:%f\n", dspsampletime); //prints analog value to serial port
handrade88 0:0eea2c4741ec 250 DSPtimer.reset();
handrade88 0:0eea2c4741ec 251 DSPtimer.start();
handrade88 0:0eea2c4741ec 252
handrade88 0:0eea2c4741ec 253 DSPandControl_cycle();
handrade88 0:0eea2c4741ec 254 //pc.printf("%E,%E,%E,%E,%E,%E,%f\n",ratio,loop_error,Pout,Iout,Dout,PIDoutput,biasdc); //prints analog value to serial port
handrade88 0:0eea2c4741ec 255 j = nmax + RMS_cycles - 1;
handrade88 0:0eea2c4741ec 256
handrade88 0:0eea2c4741ec 257 q = 1;
handrade88 0:0eea2c4741ec 258
handrade88 0:0eea2c4741ec 259 }
handrade88 0:0eea2c4741ec 260
handrade88 0:0eea2c4741ec 261 }
handrade88 0:0eea2c4741ec 262
handrade88 0:0eea2c4741ec 263 }