implemented Find a Friend with Frequencydetection using FFT

example/FindAFriend/FrequencyFindAFriend/FrequencyFindAFriend/FrequencyFindAFriend.ino

@@ -0,0 +1,156 @@
+#include <Dezibot.h>
+#include <arduinoFFT.h>
+#include <rom/ets_sys.h>
+const uint16_t samples = 256; //This value MUST ALWAYS be a power of 2
+const int centeredThreshold = 50 ;
+//const float signalFrequency = 1000;
+const float samplingFrequency = 4000;
+float vReal[4][samples];
+float vImag[4][samples];
+#define SCL_INDEX 0x00
+#define SCL_TIME 0x01
+#define SCL_FREQUENCY 0x02
+#define SCL_PLOT 0x03
+
+ArduinoFFT<float> FFT = ArduinoFFT<float>(vReal[0], vImag[0], samples, samplingFrequency); /* Create FFT object */
+
+Dezibot dezibot = Dezibot();
+void setup() {
+  dezibot.begin();
+  Serial.begin(115200);
+  //dezibot.infraredLight.front.turnOn();
+  //dezibot.infraredLight.bottom.turnOn();
+}
+
+void loop() {
+  portDISABLE_INTERRUPTS();
+  for(int i = 0; i < samples; i++){
+    vReal[0][i] = dezibot.lightDetection.getValue(IR_FRONT);
+    vImag[0][i] = 0.0;
+    vReal[1][i] = dezibot.lightDetection.getValue(IR_LEFT);
+    vImag[1][i] = 0.0;
+    vReal[2][i] = dezibot.lightDetection.getValue(IR_RIGHT);
+    vImag[2][i] = 0.0;
+    vReal[3][i] = dezibot.lightDetection.getValue(IR_BACK);
+    vImag[3][i] = 0.0;
+    ets_delay_us(125);
+  }
+  
+  portENABLE_INTERRUPTS();
+  //PrintVector(vReal, (samples>>1), 0);
+  
+  //PrintVector(vReal, (samples>>1), 0);
+  float frequency[4];
+  float magnitude[4];
+  for(int index = 0; index <4; index++){
+    FFT.setArrays(vReal[index], vImag[index]);
+    FFT.windowing(FFTWindow::Rectangle, FFTDirection::Forward);	/* Weigh data */
+    FFT.compute(FFTDirection::Forward); /* Compute FFT */
+    FFT.complexToMagnitude(); /* Compute magnitudes */
+    FFT.majorPeak(&frequency[index],&magnitude[index]);
+    if(abs(frequency[index]-1147)>10){
+      magnitude[index] = 0;
+    }
+    Serial.print(index);
+    Serial.print(":");
+    Serial.print(frequency[index]);
+    Serial.print(",");
+    Serial.print(index+4);
+    Serial.print(":");
+    Serial.print(magnitude[index]);
+    if(index < 3){
+      Serial.print(",");
+    }
+    Serial.println();
+  }
+
+//================================================
+float leftValue = magnitude[1];
+float rightValue = magnitude[2]; 
+switch(brightest(magnitude)){
+    case IR_FRONT:
+      //correct Stearing to be centered
+      if( abs(leftValue-rightValue)
+          < centeredThreshold){
+            dezibot.motion.move();
+          }else{
+            if (leftValue > rightValue){
+              dezibot.motion.rotateAntiClockwise();
+            } else{
+              dezibot.motion.rotateClockwise();
+            }
+          }
+      dezibot.multiColorLight.setTopLeds(BLUE);
+      break;
+    case IR_LEFT:
+      dezibot.motion.rotateAntiClockwise();
+      dezibot.multiColorLight.setTopLeds(RED);
+      break;
+    case IR_RIGHT:
+      dezibot.motion.rotateClockwise();
+      dezibot.multiColorLight.setTopLeds(GREEN);
+      break;
+    case IR_BACK:
+      if(leftValue > rightValue){
+        dezibot.motion.rotateAntiClockwise();
+      } else {
+        dezibot.motion.rotateClockwise();
+      }
+      dezibot.multiColorLight.setTopLeds(YELLOW);
+      break;
+  }
+
+
+
+
+
+
+}
+
+photoTransistors brightest(float *magnitudes){
+  int pos;
+  float maxMagnitude = 0; 
+  for(int index = 0; index <4; index++){
+    if (magnitudes[index] > maxMagnitude){
+      pos = index;
+      maxMagnitude = magnitudes[index];
+    }
+  }
+  switch (pos) {
+    case 0:
+      return IR_FRONT;
+    case 1: 
+      return IR_LEFT;
+    case 2: 
+      return IR_RIGHT;
+    case 3:
+      return IR_BACK;
+  }
+}
+
+void PrintVector(float *vData, uint16_t bufferSize, uint8_t scaleType)
+{
+  for (uint16_t i = 0; i < bufferSize; i++)
+  {
+    float abscissa;
+    /* Print abscissa value */
+    switch (scaleType)
+    {
+      case SCL_INDEX:
+        abscissa = (i * 1.0);
+	break;
+      case SCL_TIME:
+        abscissa = ((i * 1.0) / samplingFrequency);
+	break;
+      case SCL_FREQUENCY:
+        abscissa = ((i * 1.0 * samplingFrequency) / samples);
+	break;
+    }
+    Serial.print(abscissa, 6);
+    if(scaleType==SCL_FREQUENCY)
+      Serial.print("Hz");
+    Serial.print(" ");
+    Serial.println(vData[i], 4);
+  }
+  Serial.println();
+}

example/example/example.ino

@@ -1,15 +1,156 @@
 #include <Dezibot.h>
+#include <arduinoFFT.h>
+#include <rom/ets_sys.h>
+const uint16_t samples = 256; //This value MUST ALWAYS be a power of 2
+const int centeredThreshold = 50 ;
+//const float signalFrequency = 1000;
+const float samplingFrequency = 4000;
+float vReal[4][samples];
+float vImag[4][samples];
+#define SCL_INDEX 0x00
+#define SCL_TIME 0x01
+#define SCL_FREQUENCY 0x02
+#define SCL_PLOT 0x03
+
+ArduinoFFT<float> FFT = ArduinoFFT<float>(vReal[0], vImag[0], samples, samplingFrequency); /* Create FFT object */
 
 Dezibot dezibot = Dezibot();
 void setup() {
   dezibot.begin();
+  Serial.begin(115200);
   //dezibot.infraredLight.front.turnOn();
   //dezibot.infraredLight.bottom.turnOn();
 }
 
 void loop() {
-  dezibot.infraredLight.bottom.turnOn();
-  delay(1000);
-  dezibot.infraredLight.bottom.turnOff();
-  delay(1000);
+  portDISABLE_INTERRUPTS();
+  for(int i = 0; i < samples; i++){
+    vReal[0][i] = dezibot.lightDetection.getValue(IR_FRONT);
+    vImag[0][i] = 0.0;
+    vReal[1][i] = dezibot.lightDetection.getValue(IR_LEFT);
+    vImag[1][i] = 0.0;
+    vReal[2][i] = dezibot.lightDetection.getValue(IR_RIGHT);
+    vImag[2][i] = 0.0;
+    vReal[3][i] = dezibot.lightDetection.getValue(IR_BACK);
+    vImag[3][i] = 0.0;
+    ets_delay_us(125);
+  }
+  
+  portENABLE_INTERRUPTS();
+  //PrintVector(vReal, (samples>>1), 0);
+  
+  //PrintVector(vReal, (samples>>1), 0);
+  float frequency[4];
+  float magnitude[4];
+  for(int index = 0; index <4; index++){
+    FFT.setArrays(vReal[index], vImag[index]);
+    FFT.windowing(FFTWindow::Rectangle, FFTDirection::Forward);	/* Weigh data */
+    FFT.compute(FFTDirection::Forward); /* Compute FFT */
+    FFT.complexToMagnitude(); /* Compute magnitudes */
+    FFT.majorPeak(&frequency[index],&magnitude[index]);
+    if(abs(frequency[index]-1147)>10){
+      magnitude[index] = 0;
+    }
+    Serial.print(index);
+    Serial.print(":");
+    Serial.print(frequency[index]);
+    Serial.print(",");
+    Serial.print(index+4);
+    Serial.print(":");
+    Serial.print(magnitude[index]);
+    if(index < 3){
+      Serial.print(",");
+    }
+    Serial.println();
+  }
+
+//================================================
+float leftValue = magnitude[1];
+float rightValue = magnitude[2]; 
+switch(brightest(magnitude)){
+    case IR_FRONT:
+      //correct Stearing to be centered
+      if( abs(leftValue-rightValue)
+          < centeredThreshold){
+            dezibot.motion.move();
+          }else{
+            if (leftValue > rightValue){
+              dezibot.motion.rotateAntiClockwise();
+            } else{
+              dezibot.motion.rotateClockwise();
+            }
+          }
+      dezibot.multiColorLight.setTopLeds(BLUE);
+      break;
+    case IR_LEFT:
+      dezibot.motion.rotateAntiClockwise();
+      dezibot.multiColorLight.setTopLeds(RED);
+      break;
+    case IR_RIGHT:
+      dezibot.motion.rotateClockwise();
+      dezibot.multiColorLight.setTopLeds(GREEN);
+      break;
+    case IR_BACK:
+      if(leftValue > rightValue){
+        dezibot.motion.rotateAntiClockwise();
+      } else {
+        dezibot.motion.rotateClockwise();
+      }
+      dezibot.multiColorLight.setTopLeds(YELLOW);
+      break;
+  }
+
+
+
+
+
+
+}
+
+photoTransistors brightest(float *magnitudes){
+  int pos;
+  float maxMagnitude = 0; 
+  for(int index = 0; index <4; index++){
+    if (magnitudes[index] > maxMagnitude){
+      pos = index;
+      maxMagnitude = magnitudes[index];
+    }
+  }
+  switch (pos) {
+    case 0:
+      return IR_FRONT;
+    case 1: 
+      return IR_LEFT;
+    case 2: 
+      return IR_RIGHT;
+    case 3:
+      return IR_BACK;
+  }
+}
+
+void PrintVector(float *vData, uint16_t bufferSize, uint8_t scaleType)
+{
+  for (uint16_t i = 0; i < bufferSize; i++)
+  {
+    float abscissa;
+    /* Print abscissa value */
+    switch (scaleType)
+    {
+      case SCL_INDEX:
+        abscissa = (i * 1.0);
+	break;
+      case SCL_TIME:
+        abscissa = ((i * 1.0) / samplingFrequency);
+	break;
+      case SCL_FREQUENCY:
+        abscissa = ((i * 1.0 * samplingFrequency) / samples);
+	break;
+    }
+    Serial.print(abscissa, 6);
+    if(scaleType==SCL_FREQUENCY)
+      Serial.print("Hz");
+    Serial.print(" ");
+    Serial.println(vData[i], 4);
+  }
+  Serial.println();
 }

src/infraredLight/InfraredLED.cpp

@@ -16,7 +16,7 @@ void InfraredLED::begin(void){
         .speed_mode = pwmSpeedMode,
         .duty_resolution = LEDC_TIMER_10_BIT,
         .timer_num = this->timer,
-        .freq_hz = 1,
+        .freq_hz = 800,
         .clk_cfg = LEDC_AUTO_CLK
     };
     ledc_timer_config(&pwmTimer);
@@ -53,7 +53,7 @@ void InfraredLED::setState(bool state){
 };
 
 void InfraredLED::sendFrequency(uint16_t frequency){
-    ledc_set_freq(pwmSpeedMode,timer,frequency);
+   // ledc_set_freq(pwmSpeedMode,timer,frequency);
     ledc_set_duty(pwmSpeedMode,channel,512);
     ledc_update_duty(pwmSpeedMode,channel);
 };