Diff: main.cpp

Revision:

0:3927c6d8ad1f

Child:

1:f0a5690db73f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Sep 06 04:23:51 2015 +0000
@@ -0,0 +1,978 @@
+
+// TODO: 
+#include "mbed.h"
+#include <string>
+
+#include <stdio.h>
+#include "PeripheralNames.h"
+#include "PeripheralPins.h"
+#include "fsl_adc_hal.h"
+#include "fsl_clock_manager.h"
+#include "fsl_dspi_hal.h"
+
+
+
+Serial pc(USBTX, USBRX);
+
+/*
+#if DEBUG
+#define DEBUG_PRINT(x) pc.printf(x)
+#else
+#define DEBUG_PRINT(x) do {} while(0)
+#endif
+*/
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
+
+
+#define MAX_FADC 6000000
+
+#define TimerInterruptInMicroSeconds 25 // This is for 40 kHz
+
+
+#define CarrierFrequency 190
+
+#define ISR_counter_loop_reset 4 // 10 kHz sampling of the signal
+
+int ISR_counter = 1;
+
+#define MaxNumSamples 0  //set to 0 if want continuous sampling (i.e. run forever)
+#define NumSamplesAverageInADC 32 //hardware averaging default is 2. values 0..32. Higher values reduce noise and maximum sample rate
+#define NumADCbits 16
+
+int CyclesInState = 0;
+// While 32 is the minimum, it seems to take a few cycles to get things to work right. 32 + 18 + 18 + 18
+#define PreCalculateCycles 256
+#define NumCalculatedAverages 500
+
+AnalogOut WaveOut(DAC0_OUT);
+AnalogIn WaveIn1(A0);
+AnalogIn WaveIn2(A2);
+
+
+double previousTime = 0;
+
+
+volatile unsigned long counter = 0; //keep track of how many times the timer interrupt executes
+
+
+volatile int phase_counter = 0;
+float twopi = 3.14159265359 * 2;
+
+#define pre_compute_length 4000
+float i_mod_pre[pre_compute_length];
+float q_mod_pre[pre_compute_length];
+int out_val_pre[pre_compute_length]; 
+
+float current_i_mod;
+float current_q_mod;
+
+
+#define lp_fir_length 256
+#define bp_fir_length 256
+
+float x_0_i[lp_fir_length];
+float x_0_q[lp_fir_length];
+float x_1_i[lp_fir_length];
+float x_1_q[lp_fir_length];
+
+float real_ip = 0;
+float real_ip_total = 0;
+float imag_ip = 0;
+float imag_ip_total = 0;
+float y_0_squared = 0;
+float y_1_squared = 0;
+
+int max_0 = -1;
+int min_0 = 65536;
+int max_1 = -1;
+int min_1 = 65536;
+
+
+// Coefficients for FIR low-pass filter
+// Order: 127
+// Fs = 10000
+// Fpass = 5
+// Fstop = 50
+// Density Factor 20
+float lp_filter_coeff[lp_fir_length] = {
+0.0861805974737537,
+0.00194058236914933,
+0.00196838475539374,
+0.00198494885848217,
+0.00201327315570587,
+0.00202912020658817,
+0.00205837690374429,
+0.00207476466732652,
+0.00210551867296513,
+0.00212232982776793,
+0.00215469918380619,
+0.00217146660668663,
+0.00220558455176705,
+0.00222092552588268,
+0.00225605241370255,
+0.00226646783939495,
+0.00230092287432329,
+0.00229715073318613,
+0.00232623464517315,
+0.00225358157041652,
+0.00237497949175132,
+0.00240962754549243,
+0.00239489319998042,
+0.00243168327631056,
+0.00244169878557599,
+0.00247033152393311,
+0.00248484040428462,
+0.00250942587726843,
+0.00252568320446658,
+0.00254761155808375,
+0.00256464167177514,
+0.00258446154111681,
+0.00260244294098803,
+0.00262131473283837,
+0.00264164776893963,
+0.00266111980952651,
+0.00268646307379128,
+0.00270761719256829,
+0.00274205375475237,
+0.00274149898919641,
+0.00274945418608348,
+0.00279082346790514,
+0.00279778186887203,
+0.00282250662520772,
+0.00283603752336853,
+0.00285874465133605,
+0.00287407059058428,
+0.00289570516626933,
+0.00291199026392523,
+0.00293311428662787,
+0.00295009345796840,
+0.00297044279838009,
+0.00298714691522817,
+0.00300581268506110,
+0.00302120747953588,
+0.00303732312039544,
+0.00305157040082656,
+0.00306643736356742,
+0.00308869428115324,
+0.00311307778862543,
+0.00311424353468359,
+0.00313896920891965,
+0.00315257957066846,
+0.00317063676755790,
+0.00318404202174892,
+0.00320106537708682,
+0.00321431574769385,
+0.00323043068211672,
+0.00324309082121968,
+0.00325830171776266,
+0.00327070521989578,
+0.00328578417067009,
+0.00329857943310240,
+0.00331411607506732,
+0.00332746621803916,
+0.00334278731970893,
+0.00335490587728655,
+0.00336582204821038,
+0.00337340614667260,
+0.00339593158768486,
+0.00340201380261714,
+0.00341358117591052,
+0.00342527970126076,
+0.00343729081346655,
+0.00344788640610150,
+0.00345950626709297,
+0.00346977560789619,
+0.00348137861989829,
+0.00349145780595203,
+0.00350258155909523,
+0.00351179661900564,
+0.00352191431883533,
+0.00352996025917487,
+0.00353918710384162,
+0.00354686935571502,
+0.00355665353091388,
+0.00356587002173744,
+0.00357689031711855,
+0.00357801462512765,
+0.00359006856669697,
+0.00359950458629560,
+0.00360494297850754,
+0.00361233039031660,
+0.00361922292724758,
+0.00362591596791954,
+0.00363226321494882,
+0.00363786374022990,
+0.00364352288365460,
+0.00364862849480178,
+0.00365423915635453,
+0.00365927328685460,
+0.00366494409556548,
+0.00366963573779647,
+0.00367457381745451,
+0.00367783024950588,
+0.00368130382332767,
+0.00368329710162579,
+0.00369126347001870,
+0.00369087449038130,
+0.00369079790741907,
+0.00369643982204526,
+0.00369766453946281,
+0.00369941489929772,
+0.00370041177271180,
+0.00370189241602639,
+0.00370304548909528,
+0.00370413802604405,
+0.00370464038533780,
+0.00370464038533780,
+0.00370413802604405,
+0.00370304548909528,
+0.00370189241602639,
+0.00370041177271180,
+0.00369941489929772,
+0.00369766453946281,
+0.00369643982204526,
+0.00369079790741907,
+0.00369087449038130,
+0.00369126347001870,
+0.00368329710162579,
+0.00368130382332767,
+0.00367783024950588,
+0.00367457381745451,
+0.00366963573779647,
+0.00366494409556548,
+0.00365927328685460,
+0.00365423915635453,
+0.00364862849480178,
+0.00364352288365460,
+0.00363786374022990,
+0.00363226321494882,
+0.00362591596791954,
+0.00361922292724758,
+0.00361233039031660,
+0.00360494297850754,
+0.00359950458629560,
+0.00359006856669697,
+0.00357801462512765,
+0.00357689031711855,
+0.00356587002173744,
+0.00355665353091388,
+0.00354686935571502,
+0.00353918710384162,
+0.00352996025917487,
+0.00352191431883533,
+0.00351179661900564,
+0.00350258155909523,
+0.00349145780595203,
+0.00348137861989829,
+0.00346977560789619,
+0.00345950626709297,
+0.00344788640610150,
+0.00343729081346655,
+0.00342527970126076,
+0.00341358117591052,
+0.00340201380261714,
+0.00339593158768486,
+0.00337340614667260,
+0.00336582204821038,
+0.00335490587728655,
+0.00334278731970893,
+0.00332746621803916,
+0.00331411607506732,
+0.00329857943310240,
+0.00328578417067009,
+0.00327070521989578,
+0.00325830171776266,
+0.00324309082121968,
+0.00323043068211672,
+0.00321431574769385,
+0.00320106537708682,
+0.00318404202174892,
+0.00317063676755790,
+0.00315257957066846,
+0.00313896920891965,
+0.00311424353468359,
+0.00311307778862543,
+0.00308869428115324,
+0.00306643736356742,
+0.00305157040082656,
+0.00303732312039544,
+0.00302120747953588,
+0.00300581268506110,
+0.00298714691522817,
+0.00297044279838009,
+0.00295009345796840,
+0.00293311428662787,
+0.00291199026392523,
+0.00289570516626933,
+0.00287407059058428,
+0.00285874465133605,
+0.00283603752336853,
+0.00282250662520772,
+0.00279778186887203,
+0.00279082346790514,
+0.00274945418608348,
+0.00274149898919641,
+0.00274205375475237,
+0.00270761719256829,
+0.00268646307379128,
+0.00266111980952651,
+0.00264164776893963,
+0.00262131473283837,
+0.00260244294098803,
+0.00258446154111681,
+0.00256464167177514,
+0.00254761155808375,
+0.00252568320446658,
+0.00250942587726843,
+0.00248484040428462,
+0.00247033152393311,
+0.00244169878557599,
+0.00243168327631056,
+0.00239489319998042,
+0.00240962754549243,
+0.00237497949175132,
+0.00225358157041652,
+0.00232623464517315,
+0.00229715073318613,
+0.00230092287432329,
+0.00226646783939495,
+0.00225605241370255,
+0.00222092552588268,
+0.00220558455176705,
+0.00217146660668663,
+0.00215469918380619,
+0.00212232982776793,
+0.00210551867296513,
+0.00207476466732652,
+0.00205837690374429,
+0.00202912020658817,
+0.00201327315570587,
+0.00198494885848217,
+0.00196838475539374,
+0.00194058236914933,
+0.0861805974737537
+};
+
+
+// Coefficients for FIR band-pass filter (single-precision floating)
+// Order: 255
+// Fs = 10000
+// Fstop1 = 140
+// Fpass1 = 180
+// Fpass2 = 200
+// Fstop2 = 240  
+// Density Factor 20
+float bp_filter_coeff[bp_fir_length] = {
+-0.0222569832875342,
+-0.000668571820934435,
+-0.000617098880070943,
+-0.000526912832180624,
+-0.000394083108191473,
+-0.000224044823896433,
+-1.40519139447993e-05,
+0.000228753985399507,
+0.000506043646368515,
+0.000809081179935479,
+0.00113886884349691,
+0.00148482706293391,
+0.00184749122790831,
+0.00221412926605753,
+0.00258556196728641,
+0.00294623650522875,
+0.00329828480589904,
+0.00362103051636677,
+0.00392129596654753,
+0.00416037700790689,
+0.00440774110765089,
+0.00458742109894213,
+0.00470432883601479,
+0.00477604326343940,
+0.00477932011891816,
+0.00472369327143935,
+0.00459673449069964,
+0.00440550863345307,
+0.00414276809544121,
+0.00381610310764620,
+0.00342194240814885,
+0.00296924934443244,
+0.00245734168541197,
+0.00189704915840860,
+0.00128969301635370,
+0.000647875455120701,
+-2.63604406483334e-05,
+-0.000717680016887559,
+-0.00142424098193147,
+-0.00211871793213848,
+-0.00280466259597701,
+-0.00347302243192041,
+-0.00410155649476450,
+-0.00468849572012807,
+-0.00521831221732055,
+-0.00568473286208729,
+-0.00607617428224791,
+-0.00638642038264520,
+-0.00660654522935380,
+-0.00673201343716618,
+-0.00675686946570070,
+-0.00667909912380224,
+-0.00649590629000032,
+-0.00620853153050349,
+-0.00581778091366643,
+-0.00532861669082900,
+-0.00474475510188909,
+-0.00407398377208893,
+-0.00332036080041792,
+-0.00249639621821765,
+-0.00161692422174727,
+-0.000686114637835394,
+0.000276969889091618,
+0.00126158838172335,
+0.00225109525801695,
+0.00323185932244432,
+0.00418763727591780,
+0.00510440961298076,
+0.00596653561986331,
+0.00676059725642517,
+0.00747245231692098,
+0.00809037791844765,
+0.00860240336852779,
+0.00899909501543391,
+0.00927138366550150,
+0.00941337467689702,
+0.00942035086127182,
+0.00929102977283836,
+0.00902403254438426,
+0.00861862088073253,
+0.00808277337301294,
+0.00741973293721300,
+0.00663808854880170,
+0.00574764652047989,
+0.00475963542671197,
+0.00368791267821526,
+0.00254692028134653,
+0.00135318490817447,
+0.000123381816987046,
+-0.00112406829604574,
+-0.00237116204428360,
+-0.00359884130528209,
+-0.00478919843371992,
+-0.00592368092266592,
+-0.00698521179296005,
+-0.00795630134054653,
+-0.00882180545791639,
+-0.00956745282162880,
+-0.0101826820881787,
+-0.0106544412214730,
+-0.0109757370305086,
+-0.0111401872280476,
+-0.0111439989937533,
+-0.0109860306403343,
+-0.0106669796725910,
+-0.0101903953657629,
+-0.00956214881730735,
+-0.00879053537462268,
+-0.00788592056463026,
+-0.00686079789488663,
+-0.00572962138717207,
+-0.00450868146307788,
+-0.00321565789521111,
+-0.00186931333282453,
+-0.000489113370880662,
+0.000904730267919755,
+0.00229132886837385,
+0.00364982806960963,
+0.00496072866543581,
+0.00620453967164588,
+0.00736222523931905,
+0.00841655902006347,
+0.00935134258076706,
+0.0101530565640308,
+0.0108094111402764,
+0.0113105535457012,
+0.0116487913595070,
+0.0118191627771399,
+0.0118191627771399,
+0.0116487913595070,
+0.0113105535457012,
+0.0108094111402764,
+0.0101530565640308,
+0.00935134258076706,
+0.00841655902006347,
+0.00736222523931905,
+0.00620453967164588,
+0.00496072866543581,
+0.00364982806960963,
+0.00229132886837385,
+0.000904730267919755,
+-0.000489113370880662,
+-0.00186931333282453,
+-0.00321565789521111,
+-0.00450868146307788,
+-0.00572962138717207,
+-0.00686079789488663,
+-0.00788592056463026,
+-0.00879053537462268,
+-0.00956214881730735,
+-0.0101903953657629,
+-0.0106669796725910,
+-0.0109860306403343,
+-0.0111439989937533,
+-0.0111401872280476,
+-0.0109757370305086,
+-0.0106544412214730,
+-0.0101826820881787,
+-0.00956745282162880,
+-0.00882180545791639,
+-0.00795630134054653,
+-0.00698521179296005,
+-0.00592368092266592,
+-0.00478919843371992,
+-0.00359884130528209,
+-0.00237116204428360,
+-0.00112406829604574,
+0.000123381816987046,
+0.00135318490817447,
+0.00254692028134653,
+0.00368791267821526,
+0.00475963542671197,
+0.00574764652047989,
+0.00663808854880170,
+0.00741973293721300,
+0.00808277337301294,
+0.00861862088073253,
+0.00902403254438426,
+0.00929102977283836,
+0.00942035086127182,
+0.00941337467689702,
+0.00927138366550150,
+0.00899909501543391,
+0.00860240336852779,
+0.00809037791844765,
+0.00747245231692098,
+0.00676059725642517,
+0.00596653561986331,
+0.00510440961298076,
+0.00418763727591780,
+0.00323185932244432,
+0.00225109525801695,
+0.00126158838172335,
+0.000276969889091618,
+-0.000686114637835394,
+-0.00161692422174727,
+-0.00249639621821765,
+-0.00332036080041792,
+-0.00407398377208893,
+-0.00474475510188909,
+-0.00532861669082900,
+-0.00581778091366643,
+-0.00620853153050349,
+-0.00649590629000032,
+-0.00667909912380224,
+-0.00675686946570070,
+-0.00673201343716618,
+-0.00660654522935380,
+-0.00638642038264520,
+-0.00607617428224791,
+-0.00568473286208729,
+-0.00521831221732055,
+-0.00468849572012807,
+-0.00410155649476450,
+-0.00347302243192041,
+-0.00280466259597701,
+-0.00211871793213848,
+-0.00142424098193147,
+-0.000717680016887559,
+-2.63604406483334e-05,
+0.000647875455120701,
+0.00128969301635370,
+0.00189704915840860,
+0.00245734168541197,
+0.00296924934443244,
+0.00342194240814885,
+0.00381610310764620,
+0.00414276809544121,
+0.00440550863345307,
+0.00459673449069964,
+0.00472369327143935,
+0.00477932011891816,
+0.00477604326343940,
+0.00470432883601479,
+0.00458742109894213,
+0.00440774110765089,
+0.00416037700790689,
+0.00392129596654753,
+0.00362103051636677,
+0.00329828480589904,
+0.00294623650522875,
+0.00258556196728641,
+0.00221412926605753,
+0.00184749122790831,
+0.00148482706293391,
+0.00113886884349691,
+0.000809081179935479,
+0.000506043646368515,
+0.000228753985399507,
+-1.40519139447993e-05,
+-0.000224044823896433,
+-0.000394083108191473,
+-0.000526912832180624,
+-0.000617098880070943,
+-0.000668571820934435,
+-0.0222569832875342
+};
+
+
+int lp_fifo_num = 1;
+int bp_fifo_num = 1;
+
+float x_0[bp_fir_length];
+float x_1[bp_fir_length];
+
+int value[] = {0,0};
+
+float mag = 0;
+float calc_mag = 0;
+double mag_total = 0;
+float calc_phase = 0;
+int NumInBlock = 0;
+
+float mag_result = 0;
+float phase_result = 0;
+/*
+int diff_0[3] = {0, 0, 0};
+int diff_1[3] = {0, 0, 0};
+*/
+double final_mag = 0;
+float final_phase = 0;
+
+// Finite State Machine
+int current_state = 0;
+
+// Flag
+bool ADC_reading = false;
+bool isTransmitting = false;
+
+
+//IntervalTimer timer0;
+Ticker timer0;
+Timer t1;
+
+int adc_cal(void)
+{
+    ADC0_CFG1 |= (ADC_CFG1_MODE(3)  | // 16 bits mode
+                  ADC_CFG1_ADICLK(1)| // Input Bus Clock divided by 2 (20-25 MHz out of reset (FEI mode) / 2)
+                  ADC_CFG1_ADIV(2)) ; // Clock divide by 4 (2.5-3 MHz)
+    
+    ADC0_SC3 |= ADC_SC3_AVGE_MASK |   // Enable HW average
+                ADC_SC3_AVGS(3)   |   // Set HW average of 32 samples
+                ADC_SC3_CAL_MASK;     // Start calibration process
+    
+    while(ADC0_SC3 & ADC_SC3_CAL_MASK); // Wait for calibration to end
+    
+    if(ADC0_SC3 & ADC_SC3_CALF_MASK) return 1;  // Check for successful calibration
+    
+    uint16_t calib = 0; // calibration variable 
+    calib += ADC0->CLPS + ADC0_CLP4 + ADC0_CLP3 + ADC0_CLP2 + ADC0_CLP1 + ADC0_CLP0;
+    calib /= 2;
+    calib |= 0x8000;    // Set MSB 
+    ADC0_PG = calib;
+    calib = 0;
+    calib += ADC0_CLMS + ADC0_CLM4 + ADC0_CLM3 + ADC0_CLM2 + ADC0_CLM1 + ADC0_CLM0;
+    calib /= 2;
+    calib |= 0x8000;    // Set MSB
+    ADC0_MG = calib;
+    
+    return 0;
+}
+
+void adc_init()
+{
+    // red, indicating not ready
+
+    
+    // Turn on the ADC0 and ADC1 clocks
+    SIM_SCGC6 |= SIM_SCGC6_ADC0_MASK;
+    SIM_SCGC3 |= SIM_SCGC3_ADC1_MASK;
+    
+    
+     __disable_irq();
+     adc_cal();
+    //if(adc_cal()) {red = 0; green = 0;} // if calibration fails, display yellow
+    __enable_irq();
+
+
+    
+    // Setup Configuration Register 1 
+    ADC0_CFG1 = 0; // clear register
+    ADC0_CFG1 |= ADC_CFG1_ADICLK(0);    // select bus clock
+    ADC0_CFG1 |= ADC_CFG1_MODE(3);      // select 16-bit 2's complement output  
+    ADC0_CFG1 |= ADC_CFG1_ADIV(0);      // clock divisor = 0.
+    ADC0_CFG1 &= ~ADC_CFG1_ADLSMP_MASK; // select short sample time   //QUESTION
+    ADC0_CFG1 &= ~ADC_CFG1_ADLPC_MASK;  // select normal power configuration
+    ADC1_CFG1 = 0; // clear register
+    ADC1_CFG1 |= ADC_CFG1_ADICLK(0);    // select bus clock
+    ADC1_CFG1 |= ADC_CFG1_MODE(3);      // select 16-bit 2's complement output
+    ADC1_CFG1 |= ADC_CFG1_ADIV(0);      // clock divisor = 0.
+    ADC1_CFG1 &= ~ADC_CFG1_ADLSMP_MASK; // select short sample time
+    ADC1_CFG1 &= ~ADC_CFG1_ADLPC_MASK;  // select normal power configuration
+    
+    // Setup Configuration Register 2 
+    ADC0_CFG2 = 0; // clear register
+    //ADC0_CFG2 |= ADC_CFG2_ADHSC_MASK ;  // select high-speed conversion
+    ADC0_CFG2 |= ADC_CFG2_MUXSEL_MASK; // select a channels    //possibly change to channel b.
+    ADC1_CFG2 = 0; // clear register
+    //ADC1_CFG2 |= ADC_CFG2_ADHSC_MASK ;  // select high-speed conversion
+    ADC1_CFG2 &= ~ADC_CFG2_MUXSEL_MASK; // select a channels    
+    
+    // Setup Status and Control Register 2 
+    ADC0_SC2 = 0;                    // clear register
+    ADC0_SC2 |= ADC_SC2_REFSEL(0);   // select external voltage reference   //QUESTION
+    //ADC0_SC2 |= ADC_SC2_DMAEN_MASK;  // enable DMA
+    //ADC0_SC2 &= ~ADC_SC2_ADTRG_MASK; // select hardware trigger  //QESTION
+    ADC1_SC2 = 0;                    // clear register
+    ADC1_SC2 |= ADC_SC2_REFSEL(0);   // select external voltage reference
+    //ADC1_SC2 |= ADC_SC2_DMAEN_MASK;  // enable DMA
+    //ADC1_SC2 &= ~ADC_SC2_ADTRG_MASK; // select hardware trigger
+    
+    // Setup Status and Control Register 3 now that calibration is complete
+    ADC0_SC3 = 0; // Hardware Average set to 4 samples averaged
+                  // Hardware Average Disabled
+                  // select single conversion mode
+    ADC1_SC3 = 0; // Hardware Average set to 4 samples averaged
+                  // Hardware Average Disabled
+                  // select single conversion mode
+    //ADC1_SC3 = ADC_SC3_AVGS(0) |  ADC_SC3_AVGE_MASK;//JARED COME BACK AND LOOK AT THIS
+    
+    // Setup Status and Control Register 1A 
+    ADC0_SC1A = 0; // clear register
+    ADC0_SC1A &= ~ADC_SC1_DIFF_MASK; // select single-ended mode
+    ADC0_SC1A |= ADC_SC1_AIEN_MASK;  // enable interrupt (for debugging)
+    ADC0_SC1A |= ADC_SC1_ADCH(12);   // select channel 13
+    ADC1_SC1A = 0; // clear register
+    ADC1_SC1A &= ~ADC_SC1_DIFF_MASK; // select single-ended mode
+    ADC1_SC1A |= ADC_SC1_AIEN_MASK;  // enable interrupt (for debugging)
+    ADC1_SC1A |= ADC_SC1_ADCH(14);   // select channel 14
+    
+    
+    // Check if ADC is finished initializing  TODO:  This part doesn't seem right, but I did it according to 871
+    while( (ADC0_SC1B&ADC_SC1_COCO_MASK)) {}
+    int gain = ADC0_RA; // read the register to clear SC1A[COCO]  //possibly a
+    while( (ADC1_SC1A&ADC_SC1_COCO_MASK)) {}
+    gain = ADC1_RA; // read the register to clear SC1A[COCO]
+    
+    
+//QUESTION
+    ADC0_SC3 &= ~ADC_SC3_ADCO_MASK;  // single conversion mode
+    ADC1_SC3 &= ~ADC_SC3_ADCO_MASK;  // single conversion mode
+    ADC0_SC2 &= ~ADC_SC2_ADTRG_MASK; // set ADC to software trigger
+    ADC1_SC2 &= ~ADC_SC2_ADTRG_MASK; // set ADC to software trigger
+    ADC0_SC1A |= ADC_SC1_ADCH(12);   // write to SC1B causing a trigger
+    ADC1_SC1A |= ADC_SC1_ADCH(14);   // write to SC1A causing a trigger
+}
+
+
+
+void pre_compute_tables() {
+  // This function will precompute the cos and sin tables used in the rest of the program
+  for(int precompute_counter = 0; precompute_counter < pre_compute_length; precompute_counter++){
+    out_val_pre[precompute_counter] = (int) (cos(twopi * CarrierFrequency * TimerInterruptInMicroSeconds * 1e-6 * precompute_counter) * 32500.0 + 33000.0);
+    i_mod_pre[precompute_counter] = (cos(twopi * CarrierFrequency * TimerInterruptInMicroSeconds * 1e-6 * precompute_counter));
+    q_mod_pre[precompute_counter] = (-sin(twopi * CarrierFrequency * TimerInterruptInMicroSeconds * 1e-6 * precompute_counter));
+
+  }
+} //pre_compute_tables
+
+
+
+
+void ISR_repeat() {//40 KHz
+  // Calculation of loop for generating signal to output on DAC
+  
+  WaveOut.write_u16(out_val_pre[phase_counter]);  //creates a wave that bounces between 0 & 3.3 V
+
+  // ISR counter reset and increment
+  if (ISR_counter >= ISR_counter_loop_reset)//10KHz
+  {
+    ISR_counter = 1;
+  //JARED ADD THESE 2 LINES BACK IN
+    ADC0_SC1A |= ADC_SC1_ADCH(12);   // start ADC process Possibly change to B.
+    ADC1_SC1A |= ADC_SC1_ADCH(14);   // start ADC process
+    
+    ADC_reading = true;
+    current_i_mod = i_mod_pre[phase_counter];
+    current_q_mod = q_mod_pre[phase_counter];
+  }
+  else {
+    ISR_counter++;
+  }
+  
+  phase_counter++;
+  if (phase_counter >= pre_compute_length) phase_counter = 0;
+  
+} //ISR_repeat
+
+
+void execute_DSP() {
+bp_fifo_num++;  
+  if (bp_fifo_num >= bp_fir_length) bp_fifo_num = 0;
+
+   x_0[bp_fifo_num] = (float) (value[1] - value[0]);
+   x_1[bp_fifo_num] = (float) (value[0] - 32768);
+
+   // Band pass filter of incoming data
+  float y_0 = x_0[bp_fifo_num] * bp_filter_coeff[0];
+  float y_1 = x_1[bp_fifo_num] * bp_filter_coeff[0];
+  
+  for(int fir_counter = 1; fir_counter < bp_fir_length; fir_counter++){
+    int fir_index = bp_fifo_num + fir_counter;
+    if (fir_index >= bp_fir_length) {
+      fir_index -= bp_fir_length;
+    }
+    y_0 += x_0[fir_index] * bp_filter_coeff[fir_counter];
+    y_1 += x_1[fir_index] * bp_filter_coeff[fir_counter];
+  }
+
+  current_i_mod = i_mod_pre[phase_counter];
+  current_q_mod = q_mod_pre[phase_counter];
+
+  // Demodulation of the signal
+  x_0_i[lp_fifo_num] = (y_0 * current_i_mod);
+  x_0_q[lp_fifo_num] = (y_0 * current_q_mod);
+  x_1_i[lp_fifo_num] = (y_1 * current_i_mod);
+  x_1_q[lp_fifo_num] = (y_1 * current_q_mod); 
+
+  lp_fifo_num++;  
+  if (lp_fifo_num >= lp_fir_length) lp_fifo_num = 0;
+
+  // Low pass filter of demodulated signal
+  float y_0_i = x_0_i[lp_fifo_num] * lp_filter_coeff[0];
+  float y_0_q = x_0_q[lp_fifo_num] * lp_filter_coeff[0];
+  float y_1_i = x_1_i[lp_fifo_num] * lp_filter_coeff[0];
+  float y_1_q = x_1_q[lp_fifo_num] * lp_filter_coeff[0];
+
+  for(int fir_counter = 1; fir_counter < lp_fir_length; fir_counter++){
+    int fir_index = lp_fifo_num + fir_counter;
+    if (fir_index >= lp_fir_length) {
+      fir_index -= lp_fir_length;
+    }
+    y_0_i += x_0_i[fir_index] * lp_filter_coeff[fir_counter];
+    y_0_q += x_0_q[fir_index] * lp_filter_coeff[fir_counter];
+    y_1_i += x_1_i[fir_index] * lp_filter_coeff[fir_counter];
+    y_1_q += x_1_q[fir_index] * lp_filter_coeff[fir_counter];
+  }  
+   
+  real_ip = y_0_i * y_1_i + y_0_q * y_1_q;
+  imag_ip = y_0_q * y_1_i - y_0_i * y_1_q;
+  y_0_squared = y_0_i * y_0_i + y_0_q * y_0_q; 
+  y_1_squared = y_1_i * y_1_i + y_1_q * y_1_q;
+
+//  float mag = sqrt((real_ip * real_ip + imag_ip * imag_ip) / y_0_squared;
+  mag = sqrt(((float) real_ip * (float) real_ip + (float) imag_ip * (float) imag_ip)) / ((float) y_0_squared);
+  mag_total += mag;
+  calc_mag = mag_total / NumInBlock;
+
+  real_ip_total += real_ip;
+  imag_ip_total += imag_ip;
+  calc_phase = -atan2(imag_ip_total, real_ip_total);
+}
+
+
+
+void setup() { 
+  adc_init();
+  pre_compute_tables();
+  wait(1); 
+} //setup
+
+using namespace std;
+ 
+int main() {
+    led_blue = 1;
+    led_green = 1;
+    led_red = 0;
+    
+    setup();
+    int count = 0;
+    t1.start();
+    
+    pc.baud(115200);
+    
+    for(int i = 0; i < 86; i++) 
+    {
+        if(NVIC_GetPriority((IRQn_Type) i) == 0) NVIC_SetPriority((IRQn_Type) i, 2);
+    }
+    
+    // Give hardware associated with 
+    // sampling the highest priority
+    NVIC_SetPriority(ADC1_IRQn,0);
+    NVIC_SetPriority(ADC0_IRQn,0);
+    NVIC_SetPriority(PDB0_IRQn,0);
+    NVIC_SetPriority(DMA0_IRQn,0);
+    NVIC_SetPriority(DMA1_IRQn,0);
+    NVIC_SetPriority(DMA2_IRQn,0);
+    
+    NVIC_SetPriority(ENET_1588_Timer_IRQn,1);
+    NVIC_SetPriority(ENET_Transmit_IRQn,1);
+    NVIC_SetPriority(ENET_Receive_IRQn,1);
+    NVIC_SetPriority(ENET_Error_IRQn,1);
+
+    led_green = 1;
+    led_red = 1;
+    led_blue = 1;
+
+    timer0.attach_us(&ISR_repeat, TimerInterruptInMicroSeconds);
+    while(true) {
+                
+                count++;
+                //printf("Loop");
+                
+                while (!ADC_reading)  
+                {
+                    
+                led_blue = 0;
+                }
+
+            
+              counter++;
+              
+              CyclesInState++;
+              
+              
+              execute_DSP();
+              
+              if (CyclesInState <= PreCalculateCycles) {
+                NumInBlock = 0;
+                mag_total = 0;
+                real_ip_total = 0;
+                imag_ip_total = 0;
+        
+                max_0 = -1;
+                max_1 = -1;
+                min_0 = 65536;
+                min_1 = 65536;
+              }
+              else {
+                NumInBlock++;
+                if (NumInBlock >= NumCalculatedAverages) {
+                  mag_result = calc_mag;
+                  phase_result = calc_phase;
+                  final_mag = mag_result;
+                  final_phase = phase_result;
+                  NumInBlock = 0;
+                  mag_total = 0;
+                  real_ip_total = 0;
+                  imag_ip_total = 0;
+                  printf("Coeficient%f Phase%f\n\r", final_mag, final_phase);
+                }
+              }
+              
+                      
+          value[0] = ADC0_RA;//Read adc
+          value[1] = ADC1_RA;//Read adc
+          //printf("%f %f\n\r", value[0], value[1]);
+          ADC_reading = false;//lower flag
+
+    }// end while(true) 
+}// end main