Diff: main.cpp

Revision:

0:d341b3ad1e2e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 03 20:50:10 2016 +0000
@@ -0,0 +1,173 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "Motor.h"
+#include "HALLFX_ENCODER.h"
+//#include "SDFileSystem.h"
+//#include "wave_player.h"
+
+class microphone
+{
+public :
+    microphone(PinName pin);
+    float read();
+    operator float ();
+private :
+    AnalogIn _pin;
+};
+microphone::microphone (PinName pin):
+    _pin(pin)
+{
+}
+float microphone::read()
+{
+    return _pin.read();
+}
+inline microphone::operator float ()
+{
+    return _pin.read();
+}
+
+
+// Declare Components
+
+//Adafruit MEMs SPW2430 microphone demo - LEDs display audio level
+BusOut myleds(LED1,LED2,LED3,LED4);
+microphone mymicrophone(p16);
+Mutex movement_mutex;
+Motor left_motor(p26,p24,p25); // pwm, fwd, rev  
+Motor right_motor(p21, p23, p22); // pwm, fwd, rev
+HALLFX_ENCODER hall_left(p13);
+HALLFX_ENCODER hall_right(p14);
+AnalogIn IRSound(p17);
+
+
+
+// Robot functions
+/* For debugging IR sensor
+void IRsoundRead() {
+        distance = IRSound.read();
+        if (distance >= 0.0f && distance <= .5f/3.3f) { // Furthest away
+            myleds = 1;
+        } else if (distance >= .5f/3.3f && distance <= .75f/3.3f) {
+            myleds = 3;   
+        } else if (distance >= .75f/3.3f && distance <= 1.25f/3.3f) {
+            myleds = 7; 
+        } else if (distance >= 1.25f/3.3f ) { // closest
+            myleds = 15;
+            tooClose = 1;
+        }
+} 
+*/
+// forward
+void forward( float vol ){
+    long diff = hall_left.read()-hall_right.read();
+    float k = 0.8;
+    if (diff < 0) { // left has moved less than right
+        left_motor.speed(vol);
+        right_motor.speed(k*vol);
+    }else if (diff > 0)  { // right has moved less than left
+        left_motor.speed(k*vol);
+        right_motor.speed(vol); 
+    } else {
+        left_motor.speed(vol);
+        right_motor.speed(vol);
+    }
+}
+
+// spin
+void panicSpin() {
+    //Spin really scaredly
+    left_motor.speed(1);
+    right_motor.speed(-1);
+    Thread::wait(2000);
+    
+    //Try to avoid ojbect
+    left_motor.speed(0.5);
+    right_motor.speed(-0.5);    
+    while(IRSound.read()>= 1.25f/3.3f ) {};
+    Thread::wait(100);
+    forward(0);    
+}
+
+
+// Listen mic Function (Continuous, thread)
+float listen() {
+    //read in sample value using AC coupled input option
+    float sample = mymicrophone;
+    
+    int a = int(abs((sample-0.5)*300.0));
+    if (a < 15 ) {
+          myleds = a;
+    } else {
+         myleds = 12;
+    }
+    //myleds = int(abs((sample-0.5)*300.0)); //scale to around 15 for LEDs
+    return abs((sample-0.5)*300.0/15.0);
+}
+
+// IRsound Function (Continuous, thread)
+// When run everything else supspended
+void ActScared(void)
+{
+    Thread::wait(1000);
+    float loudnessBuffer[10] = {0,0,0,0,0,0,0,0,0,0};
+    float volume = 0, currentVal = 0;
+    int i = 0;
+    //float speed = 0;
+    while(1) {
+        currentVal = listen();
+        volume += (currentVal-loudnessBuffer[i])/10;
+        loudnessBuffer[i] = currentVal;
+        i++;
+        if (i==10) {i=0;}
+        if(2.0*volume>0.1){
+            movement_mutex.lock();
+            forward(1);
+            movement_mutex.unlock();
+        } else {
+            hall_left.reset(); hall_right.reset();
+            movement_mutex.lock();
+            forward(0.1);
+            movement_mutex.unlock();
+        }
+        Thread::wait(100);
+        /**/
+    }
+}
+
+void AvoidObject(void)
+{
+    Thread::wait(500);
+    float closenessBuffer[10] = {0,0,0,0,0,0,0,0,0,0};
+    float closeness = 0, currentVal = 0;
+    int i = 0;
+    while(1) {
+        // average 10 values over 1 seconds.
+        currentVal = IRSound.read();
+        closeness += (currentVal-closenessBuffer[i])/10;
+        closenessBuffer[i] = currentVal;
+        i++;
+        if (i==10) {i=0;}
+        //float distance = IRSound.read();
+        movement_mutex.lock();
+        if (closeness >= 1.0f/3.3f ) { // close object detected
+            for (int j = 0; j < 10; j++) {
+              closenessBuffer[j] = 0;  
+            }
+            closeness = 0;
+            forward(0);
+            Thread::wait(250);            
+            panicSpin();
+        }
+        movement_mutex.unlock();
+        Thread::wait(50);
+    }
+}
+
+int main()
+{
+    Thread t2(ActScared);
+    while(1) {       
+        AvoidObject();
+    }
+}