Revision 3:03a07e01fa18, committed 2019-12-06

Comitter:

dlewis80

Date:

Fri Dec 06 03:52:05 2019 +0000

Parent:

2:04ffcc436ac0

Commit message:

4180 Receiver Code

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Fri Dec 06 03:52:05 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/Motor/#f265e441bcd9

--- a/main.cpp	Fri Jan 26 00:55:11 2018 +0000
+++ b/main.cpp	Fri Dec 06 03:52:05 2019 +0000
@@ -1,43 +1,117 @@
 #include "mbed.h"
-//I2S Demo to display values from I2S microphone
-//
-//Only works on mbed LPC1768 - I2S is not an mbed API!
-//Microphone used is SPH0645LM4H
-//see https://www.adafruit.com/product/3421
+#include "I2S.h"
+#include "Motor.h"
+#define sample_freq 8000.0
+#define M_PI 3.14159265358979323846
+//108 is good bin size
 
-#include "I2S.h"
-//uses patched mbed I2S library from 
-//https://os.mbed.com/users/p07gbar/code/I2S/
-//Needed a typo patch - clock was swapped p30/p15 in pin function setup code
-//"if (_clk == p15)" changed to "if (_clk != p15)" in I2S.cpp line 494
-BusOut mylevel(LED4,LED3,LED2,LED1);
-//4 built in mbed LEDs display audio level
+// Globals
+I2S i2srx(I2S_RECEIVE, p17, p16, p15);
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled4(LED4);
+Motor Lmotor(p21,p6,p5);
+Motor Rmotor(p22,p7,p8);
+
+volatile int updating_data[108] ={0}; //i2s data
+volatile int i = 0; // counter
+volatile bool dv = 0; // data valid flag
+volatile bool gr = 1; // goertzel ready flag
+volatile float data[108] = {0.0}; //complete data block
 
-#define sample_freq 32000.0
-
-//Uses I2S hardware for input
-I2S i2srx(I2S_RECEIVE, p17, p16, p15);
-//p17(PWM value sent as serial data bit) <-> Dout
-//p16(WS) <-> LRC left/right channel clock
-//p15(bit clock) <-> BCLK 
+// Interrupt to read microphone
+void i2s_isr()
+{
+    if (i<108){
+        updating_data[i] = (i2srx.read()>>7);//read value using I2S input
+        dv = 0;
+        i++;
+    } 
+    else{
+        if (gr){
+            i = 0;
+            for (i=0; i<108; i++)
+                data[i] = float(updating_data[i]); //shift data to constant register
+            dv = 1;
+        }
+        i = 0;
+        updating_data[i] = i2srx.read();
+    }
+}
 
-volatile int i=0;
-
-void i2s_isr() //interrupt routine to read microphone
+// Goertzal function
+float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, volatile float* data)
 {
-    i = i2srx.read();//read value using I2S input
-    mylevel = (i>>7)& 0xF; //Display Audio Level on 4 LEDs
+    float realW,imagW,d1,d2,y,resultr,resulti,magnitude;
+    realW = 2.0*cos(2.0*M_PI*TARGET_FREQUENCY/numSamples);
+    imagW = sin(2.0*M_PI*TARGET_FREQUENCY/numSamples);
+    
+    d1=0;
+    d2=0;
+    for (int n=0; n<numSamples; n++)
+    {
+        y = data[n] + realW*d1 - d2;
+        d2=d1;
+        d1=y;
+    }
+    resultr = 0.5*realW*d1 - d2;
+    resulti = imagW*d1;
+    
+    magnitude = sqrtf(resultr*resultr + resulti*resulti);
+    return magnitude;
 }
+
 int main()
 {
+    //Goertzel function initialize
+    int numSamples = 108; //block size
+    int TARGET_FREQUENCY = 800; //frequency to detect
+    int SAMPLING_RATE = sample_freq; //I2S sampling frequency
+    float mag1 = 0.0;
+    float mag2 = 0.0;
+    
+    //Motor Initialize
+    bool lm_moving = 0;
+    bool rm_moving = 0;
+    
+    //i2s microphone initialize
     i2srx.stereomono(I2S_MONO);//mono not stereo mode
     i2srx.masterslave(I2S_MASTER);//master drives clocks
     i2srx.frequency(sample_freq);//set sample freq
     i2srx.set_interrupt_fifo_level(1);//interrupt on one sample
     i2srx.attach(&i2s_isr);//set I2S ISR
     i2srx.start();//start I2S and interrupts
+
     while(1) {
-        printf("%X\n\r",(i>>16)&(0x0000FFFF));
-        wait(0.5);
+        if(dv && gr){
+            gr = 0;
+            mag1 = goertzel_mag(numSamples, TARGET_FREQUENCY, SAMPLING_RATE, data);
+            mag2 = goertzel_mag(108.0,1000.0,SAMPLING_RATE,data);
+            gr = 1;
+            myled4 = !myled4;
+        }
+        //3000000
+        if (mag1>3000000)
+        {
+            myled1=1;
+            rm_moving = !rm_moving;
+        }
+        else
+            myled1=0;
+        if (mag2>3000000)
+        {
+            myled2=1;
+            lm_moving = !lm_moving;
+        }
+        else
+            myled2=0;
+        if (lm_moving)
+            Lmotor.speed(-0.5);
+        else
+            Lmotor.speed(0);
+        if (rm_moving)
+            Rmotor.speed(0.5);
+        else
+            Rmotor.speed(0);
     }
-}
+}
\ No newline at end of file