main.cpp

#include "mbed.h"
#include "Motor.h"
#include "rtos.h"
#include "ultrasonic.h"
#include "math.h"
#include "uLCD_4DGL.h"

#include <time.h>       /* time */


//the display actally has 8 led's, the last one is a dot

BusOut myled(LED1,LED2,LED3,LED4);
RawSerial tooth(p28,p27); // bluetooth serial ports
Motor r_motor(p22, p5, p6); // pwm, fwd, rev
Motor l_motor(p21, p7, p8); // pwm, fwd, rev

PwmOut red(p24);
PwmOut green(p23);

Thread bluetooth_thread;
Thread motor_thread;
Thread sonar_thread;
Thread rgb_thread;
Thread lcd_thread;

Mutex serial_mutex;
Mutex usb_mutex;

uLCD_4DGL uLCD(p13, p14, p15); //serial tx, serial rx, reset pin

//For the motor's speed
volatile float l_motor_speed;
volatile float r_motor_speed;

//Sonar Distances
volatile int l_sonar_dis = 0;
volatile int r_sonar_dis = 0;
volatile float speed_scale = 5.0;

//Potential Field Constants
float particle_charge = 250.0;
float sonar_angle = 45.0*3.14159/180.0;
float distance_danger = 254; //The min turning radius, the distance maximum force occurs

//LCD Display state
volatile int user_input;
volatile int LCD_Mode = 1;
Mutex LCD_Mutex;

//Gets the distance to the nearest obstacle in the view of the left sonar
void l_dist(int distance)
{
    l_sonar_dis = distance;
    //serial_mutex.lock();
    //printf("Left Distance: %d \n Right Distance %d \n\n", l_sonar_dis, r_sonar_dis);
    //serial_mutex.unlock();
}
//Gets the distance to the nearest obstacle in the view of the right sonar
void r_dist(int distance)
{
    r_sonar_dis = distance;
    //serial_mutex.lock();
    //printf("Left Distance: %d \n Right Distance %d \n\n", l_sonar_dis, r_sonar_dis);
    //serial_mutex.unlock();
}
ultrasonic l_sonar(p11, p12, .1, 1, &l_dist);    //Set the trigger pin to p16 and the echo pin to p17
                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes

ultrasonic r_sonar(p9, p10, .1, 1, &r_dist);    //Set the trigger pin to p18 and the echo pin to p19
                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes


//gets the closest distance
float getClosest() {

    float closest;
    if(l_sonar_dis > r_sonar_dis) {
        if(r_sonar_dis > 500) {
            closest = 500;
        } else {
            closest = r_sonar_dis;
        }
    } else {
        if(l_sonar_dis > 500) {
            closest = 500;
        } else {
            closest = l_sonar_dis;
        }
    }
    return closest;
}

// Function for reading bluetooth
//Arrows Control Direction,
//Button b3 increases speed, b4 decreases it
//Button b1 instantly stops the robot
//Button b2 changes the state of the LCD display
void bluetooth_func()
{
    char bnum=0;
    char bhit=0;

    while(1)
    {
        if(tooth.readable())
        {
            serial_mutex.lock();
            if (tooth.getc()=='!') {
                if (tooth.getc()=='B') { //button data
                    bnum = tooth.getc(); //button number
                    bhit = tooth.getc();
                    if(bhit == '0')
                    {
                        continue;
                    }
                    if ((bnum>='1')&&(bnum<='8')) //is a number button 1..8
                    {
                        // For LEDs
                        if(bnum == '1')
                        {
                            user_input = 1;
                            myled = 0b0001;
                            l_motor_speed = 0.0;
                            r_motor_speed = 0.0;
                        } else if(bnum == '2')
                        {
                            if(LCD_Mode == 3){
                                LCD_Mode = 1;
                            } else {
                                LCD_Mode ++;
                            }
                            myled = 0b0010;
                        } else if(bnum == '3')
                        {
                            if (speed_scale < 10) {
                                speed_scale++;
                                myled = 0b0100;
                            }
                        } else if(bnum == '4')
                        {
                            if(speed_scale > 2) {
                                speed_scale--;
                                myled = 0b1000;
                            }
                        }

                        // For motors
                        else if(bnum == '5') //up arrow
                        {
                            user_input = 5;
                            l_motor_speed = 0.1*speed_scale;
                            r_motor_speed = 0.1*speed_scale;
                            myled = 0b0001;
                        } else if(bnum == '6') //down arrow
                        {
                            user_input = 6;
                            l_motor_speed = -0.1*speed_scale;
                            r_motor_speed = -0.1*speed_scale;
                            myled = 0b0010;
                        } else if(bnum == '7') //left arrow
                        {
                            user_input = 7;
                            l_motor_speed = 0.7/5.0*speed_scale;
                            r_motor_speed = 0.3/5.0*speed_scale;
                            myled = 0b0100;
                        }else if(bnum == '8') //right arrow
                        {
                            user_input = 8;
                            l_motor_speed = 0.3/5.0*speed_scale;
                            r_motor_speed = 0.7/5.0*speed_scale;
                            myled = 0b1000;
                        }
                    }
                }
            }
            serial_mutex.unlock();
        }
        Thread::wait(200);
    }
}

//RGB display changes to red as objects approach, becomes more green as objects
// gain distance
void rgb_func()
{
    float closest = 0.0;
    while(1) {
        closest = getClosest();

        red = (-closest+500)/255;
        green = (closest-254)/255;


        Thread::wait(50);
    }

}

//Controls the motor function,
//Controls the speed of the robot
void motor_func()
{
    while (1)
    {

        float rel_distance_r = (r_sonar_dis-distance_danger);
        float rel_distance_l = (l_sonar_dis-distance_danger);
        if(rel_distance_r < 0) {
            rel_distance_r = 0;
        }
        else if(rel_distance_r > 256) {
            rel_distance_r = 1000000;
        }
        if(rel_distance_l < 0) {
            rel_distance_l = 0;
        }
        else if(rel_distance_l > 256) {
            rel_distance_l = 1000000;
        }


        //float r_sonar_force = particle_charge/(rel_distance_r+0.01);
        //float l_sonar_force = particle_charge/(rel_distance_l+0.01);
        float resultant_l = -10.0/(rel_distance_r+1);
        float resultant_r = -10.0/(rel_distance_l+1);


        l_motor.speed(l_motor_speed+resultant_l);
        r_motor.speed(r_motor_speed+resultant_r);


        Thread::wait(10);
    }
}

//Controls the sonar thread and measures distances
void sonar_func()
{
    while(1)
    {
        l_sonar.checkDistance();     //call checkDistance() as much as possible, as this is where the distance gets updated
        r_sonar.checkDistance();

        Thread::wait(20);
    }
}

void LCD_func()
{
    float closest;
    while(1)
    {
        switch (LCD_Mode) {
            case 1: //display speed
                LCD_Mutex.lock();
                uLCD.color(WHITE);
                uLCD.locate(0, 0);
                uLCD.printf("Speed \n\rDisplay: ");
                uLCD.locate(0, 3);
                uLCD.printf("              ");
                uLCD.locate(0, 3);
                if(speed_scale < 5) {
                    uLCD.color(GREEN);
                } else if(speed_scale < 8) {
                    uLCD.color(GREEN+BLUE);
                } else {
                    uLCD.color(RED);
                }
                uLCD.printf("%2.2f", speed_scale);
                LCD_Mutex.unlock();
                break;
            case 2: //displays nearest sonar distance
                closest = getClosest();
                LCD_Mutex.lock();
                uLCD.color(WHITE);
                uLCD.locate(0, 0);
                uLCD.printf("Sonar \n\rDisplay: ");
                uLCD.locate(0, 3);
                uLCD.printf("              ");
                uLCD.locate(0, 3);
                if(closest < 254) {
                    uLCD.color(RED);
                } else if(closest < 499) {
                    uLCD.color(GREEN+BLUE);
                } else {
                    uLCD.color(GREEN);
                }
                if(closest >= 500) {
                    uLCD.printf("%3.2f +", closest);
                }
                else {
                    uLCD.printf("%3.2f", closest);
                }
                LCD_Mutex.unlock();
                break;
            case 3: //displays current user input
                LCD_Mutex.lock();
                uLCD.color(WHITE);
                uLCD.locate(0, 0);
                uLCD.printf("Input \n\rDisplay: ");
                uLCD.locate(0, 3);
                uLCD.printf("              ");
                uLCD.locate(0, 3);
                switch (user_input) {
                    case 1:
                        uLCD.color(RED);
                        uLCD.printf("STOP");
                        break;
                    case 5:
                        uLCD.color(GREEN);
                        uLCD.printf("FORWARD");
                        break;
                    case 6:
                        uLCD.color(GREEN);
                        uLCD.printf("REVERSE");
                        break;
                    case 7:
                        uLCD.color(GREEN);
                        uLCD.printf("LEFT");
                        break;
                    case 8:
                        uLCD.color(GREEN);
                        uLCD.printf("RIGHT");
                        break;
                }
                LCD_Mutex.unlock();
                break;
            } Thread::wait(500);
    }
}

int main() {
    //Configure uLCD
    LCD_Mutex.lock();
    uLCD.cls();
    uLCD.text_width(2); //2X size text
    uLCD.text_height(2);
    uLCD.baudrate(3000000); //jack up baud rate to max for fast display
    LCD_Mutex.unlock();

    l_sonar.startUpdates();
    r_sonar.startUpdates();

    rgb_thread.start(rgb_func);
    bluetooth_thread.start(bluetooth_func);
    motor_thread.start(motor_func);
    sonar_thread.start(sonar_func);
    lcd_thread.start(LCD_func);

    while(1) {
        Thread::wait(5000);
    }
}