File content as of revision 1:5a36b4e912da:

#include "mbed.h"
#include "motordriver.h"
#include "LSM9DS1.h"
#include "uLCD_4DGL.h"
#include "rtos.h"

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

// motor stuff
Motor left(p21, p26, p25, 1); // pwm, fwd, rev, has brake feature (pb,b2,b2)
Motor right(p22, p24, p23, 1);

// sonar stuff
// sonar 1
DigitalOut trigger(p6);
DigitalIn  echo(p7);
int distance = 0;
int correction = 0;
Timer sonar;
// sonar 2 
/*
DigitalOut trigger2(p12); // fix this
DigitalIn  echo2(p11); // fix this
int distance2 = 0;
int correction2 = 0;
Timer sonar2;
*/
// bluetooth stuff
Serial pc(USBTX, USBRX); // and IMU
Serial blue(p28,p27);

// Mutex for LCD
Mutex lcd_mutex;

// IMU stuff
LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
int old_y = 0;
int old_x = SIZE_X/2;

void sonar_thread(void const *args)
{
    // SETUP SONAR 1
    sonar.reset();
    sonar.start();   // start timer
    // min software polling delay to read echo pin
    while (echo==2) {};
    sonar.stop();    // stop timer
    correction = sonar.read_us();
 /*   // SETUP SONAR 2
    sonar2.reset();
    sonar2.start();
    while (echo2==2) {};
    sonar2.stop();
    correction2 = sonar2.read_us();
*/
    while(true) {            // thread loop
        // SONAR CODE
        trigger = 1;         // trigger sonar to send a ping
        sonar.reset();
        wait_us(10.0);
        trigger = 0;
        while (echo==0) {};  //wait for echo high
        sonar.start();       //echo high, so start timer
        while (echo==1) {};  //wait for echo low
        sonar.stop();        //stop timer and read value
        //subtract software overhead timer delay and scale to cm
        distance = (sonar.read_us()-correction)/58.0;

 /*       // SONAR 2 CODE
        trigger2 = 1;
        sonar2.reset();
        wait_us(10.0);
        trigger2 = 0;
        while (echo2==0) {};
        sonar2.start();
        while (echo2==1) {};
        sonar2.stop();
        distance2 = (sonar2.read_us()-correction2)/58.0; */
        //wait(0.2);
        // if object detected using sonar 1 & 2 - draw a circle
        if (distance <= 8) {
            lcd_mutex.lock();
            uLCD.filled_circle(old_x-2, old_y + 4, 2, RED);
            lcd_mutex.unlock();
        }
        //pc.printf("1: %d cm \n\r",distance);
        Thread::wait(1000); // wait till thread is done
    }
}

void imu_thread(void const *args)
{
    // SETUP IMU
    imu.begin();
    //if (!imu.begin()) {
    //pc.printf("Failed to communicate with LSM9DS1.\n");
    //}
    imu.calibrate();
    int magnitude = 0;
    //pc.baud(9600);

    while(true) {            // thread loop
        // IMU CODE
        imu.readMag();
        // determine magnitude of the magnetic field
        magnitude = (imu.mx*imu.mx + imu.my*imu.my + imu.mz*imu.mz);
        //pc.printf("mag: %d %d %d %d\n\n\r", imu.mx, imu.my, imu.mz, magnitude);
        if (magnitude > 30000000) {
            lcd_mutex.lock();
            uLCD.filled_circle(old_x+2, old_y + 4, 2, LGREY);
            lcd_mutex.unlock();
        }
        Thread::wait(200); // wait till thread is done
    }
}

int main()
{
    // pc print
    //pc.baud(9600);

    // lcd initialization
    uLCD.cls();
    int y = 0;
    int x = SIZE_X/2;
    float theta = 0;

    // bluetooth stuff
    char bnum=0;
    char bhit=0;
    float s = 0;

    Thread t2(imu_thread); //start imu thread
    Thread t1(sonar_thread); //start sonar thread

    while(1) {
        // BLUETOOTH CODE
        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?
                    switch (bnum) {
                        case '1': //number button 1
                            if (bhit=='1') {
                                // MOTOR CODE
                                // move forward
                                left.speed(0.5);
                                right.speed(0.5);
                                wait(0.4);
                                //}
                                left.stop(1);
                                right.stop(1);
                                y = 6*cos(theta);
                                x = 6*sin(theta);
                                lcd_mutex.lock();
                                uLCD.line(old_x,old_y,old_x+x,old_y+y,GREEN);
                                lcd_mutex.unlock();
                                old_x += x;
                                old_y += y;
                                wait(0.25);
                            }
                            break;
                        case '2': //number button 2
                            if (bhit=='1') {
                                theta += -3.14/6;
                                // MOTOR CODE
                                // turn right
                                s=(-1)*(0.55);
                                left.speed(0.55);
                                right.speed(s);
                                wait(0.25);
                                left.stop(1);
                                right.stop(1);
                                wait(0.25);
                            }
                            break;
                        case '3': //number button 3
                            if (bhit=='1') {
                                theta += 3.14/6;
                                // MOTOR CODE
                                // turn left
                                s=(-1)*(0.55);
                                left.speed(s);
                                right.speed(0.55);
                                wait(0.25);
                                left.stop(1);
                                right.stop(1);
                                wait(0.25);
                            }
                            break;
                        case '4': //number button 4
                            if (bhit=='1') {
                                // MOTOR CODE
                                // move back
                                s=(-1)*(0.5);
                                left.speed(s);
                                right.speed(s);
                                wait(0.4);
                                left.stop(1);
                                right.stop(1);
                                y = 6*cos(theta);
                                x = 6*sin(theta);
                                lcd_mutex.lock();
                                uLCD.line(old_x,old_y,old_x-x,old_y-y,RED);
                                lcd_mutex.unlock();
                                old_x += -x;
                                old_y += -y;
                                wait(0.25);
                            }
                            break;
                    }
                }
            }
        }
        Thread::wait(200);
    }
}