main.cpp

#include "mbed.h"
#include "motordriver.h"
#include <stdio.h>
#include  "Speaker.h"
#include "rtos.h"
#include "ultrasonic.h"

BusOut myled(LED1,LED2,LED3,LED4);
Serial blue(p13,p14);
Motor right(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Motor left(p26, p25, p24, 1);

// Debugging tools
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Serial pc(USBTX, USBRX);

Speaker speaker(p18);

double distance;
double holdDistance;
int status;

float defaultLeft = -0.6;
float defaultRight = -0.6;
float vl = -0.6;
float vr = -0.6;
float temp;


/*
Function is called when there is a massive distance 
differential detected in dist() denoting an edge detected
*/
void movementInterrupt() {
    //avoid abrupt speed changes
    left.speed(0);
    right.speed(0);
    wait(0.5);
    
    //goes backwards for 0.5s
    left.speed(-1 * defaultLeft);
    right.speed(-1 * defaultRight);
    wait(0.5);
    
    //avoid abrupt speed changes
    left.speed(0);
    right.speed(0);
    wait(0.5);

    // turn 90
    left.speed(0.8);
    right.speed(-0.8);  
    wait(0.5);
    
    //avoid abrupt speed changes
    left.speed(0);
    right.speed(0);
    wait(0.5);

    //goes forwards for 0.5s
    left.speed(defaultLeft);
    right.speed(defaultRight);
    wait(0.5);
    
    //avoid abrupt speed changes
    left.speed(0);
    right.speed(0);
    wait(0.5);
    
    // turn 90
    left.speed(0.8);
    right.speed(-0.8);  
    wait(0.6);
    
    // return to default speed
    left.speed(defaultLeft);
    right.speed(defaultRight);
    wait(0.5);
}

/*
Will create an audio and LED notification, called 
when an edge is detected 
*/
void speakerInterrupt() {
    led1 = 1;
    wait(0.05);
    led2 = 1;
    wait(0.05);
    led3 = 1;
    wait(0.05);
    led4 = 1;
    wait(0.05);
    led1 = !led1;
    led2 = !led2;
    led3 = !led3;
    led4 = !led4;
    for (int i = 0; i < 3; i++) {
        speaker.PlayNote(969.0, 0.3, 5.0);
        speaker.PlayNote(800.0, 0.3, 5.0);
        
    }
}
/*
Function is called when checkDistance() detects a distance differential
*/
void dist(int distance) { 
    // code executes when distance has changed 
     //pc.printf("Distance %d mm\r\n", distance);

    if (abs(distance - holdDistance) > 100) {
        //pc.printf("MOVEMENT INTERRUPT\n");
        movementInterrupt();
        left.speed(defaultLeft);
        speakerInterrupt();
    } else {
        led1 = 0;
    }
    holdDistance = distance;
}

// ultrasonic intialization -- much proceed dist() declaration
ultrasonic mu(p6, p7, .1, 1, &dist);

/*
checkDistance() run constantly probing for distance differentials
*/
void thread1(void const *args) {
    // ultrasonic sensor 
    while(true) {
        mu.checkDistance(); 
        //Thread::wait(1000);
    }
}

//Thread thread;
int main() {
    Thread t1(thread1);

    // Default speed
    left.speed(vl);
    right.speed(vr);
    wait(1);
        
    // Initialize ultrasonic sensor
    mu.startUpdates();//start measuring the distance
    
    char bnum=0;
    char bhit=0;
    while(1) {
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    //myled = bnum - '0'; //current button number will appear on LEDs
                    switch (bnum) {
                        // Let up and down arrow control speed of the sweeper
                        // Let right and left do motor correction control
                        case '1': //number button 1
                            if (bhit=='1') {
                               movementInterrupt();
                               left.speed(defaultLeft);
                               speakerInterrupt();
                                wait(0.1);
                            } 
                            break;
                        case '2': //number button 1
                        if (bhit=='1') {
                           left.speed(defaultLeft);
                           right.speed(defaultRight);
                        } 
                        break;
                        case '5': //button 5 up arrow
                            if (bhit=='1') {
                                led1 = !led1;
                                wait(0.5);
                                if (vl > -1.0 && vl < 0.0) {
                                     vl = vl - 0.1;
                                }
                                if (vr > -1.0 && vl < 0.0) {
                                    vr = vr - 0.1;
                                }
                            } 
                            break;
                        case '6': //button 6 down arrow
                            if (bhit=='1') {
                                led2 = !led2;
                                if (vl < 0.0 && vl > -1.0) {
                                     vl = vl + 0.1;
                                }
                                if (vr < 0.0 && vl > -1.0) {
                                    vr = vr + 0.1;
                                }
                            }
                            break;
                        case '7': //button 7 left arrow
                            if (bhit=='1') {
                                temp = vl;
                                vl = vl - 0.1;
                                left.speed(vl);
                                wait(1);
                            } else {
                                // Resets speed to default
                                vl = temp;
                            }
                            break;
                        case '8': //button 8 right arrow
                            if (bhit=='1') {
                                temp = vr; 
                                vr = vr - 0.1;
                                right.speed(vr);
                                wait(1);
                            } else {
                                // Resets speed to default
                                vr = temp;
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
        }
        left.speed(vl);
        right.speed(vr);
    }
}