File content as of revision 18:1b0196cf779f:

//This will rotate the wheels so that after 5 seconds the robot counterclockwise and then clockwise
#include "mbed.h"
#include "rtos.h"
#include "motordriver.h"
#include "MultiModalRobot.h"

Motor lw(p26, p29, p30, 1); // pwm, fwd, rev   LEFT WHEEL
Motor rw(p25, p28, p27, 1); // pwm, fwd, rev   RIGHT WHEEL
MultiModalRobot robot(lw, rw);

Serial blue(p13, p14);
Serial py(USBTX, USBRX);  
char y;

bool visible = false;
Thread t1;

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

void thread_read() { // Read from Serial Port Thread
    char buffer[4];
    while (true) {
        if (!py.readable()) { // nothing read - serial empty
            myled4 = 1;
            visible = false;
        } else { // found something
            myled4 = 0;
            y = py.getc();
            if (y == 'c') {
                visible = true;
                myled3 = !myled3;
                //for (int count = 0; count < 4; count++) {
//                    y = py.getc();
//                    buffer[count%4] = y;
//                    count++;
//                    if (count % 3 == 0) {
//                        myled1 = !myled1;
//                        int i;
//                        memcpy(&i, buffer, 4);
//                        if (i == 84) {
//                            myled2 = 1;
//                        }
//                    }
//                }
            }
            wait(.05);
        }
    }
}

int main() {
    char bnum = 0;
    char bhit = 0;
    char needToStopRobot = 0;
    float leftSpeed = 0;
    float rightSpeed = 0;
    float DEFAULT_SPEED = 0.75;
    float ROTATE_SPEED = 0.75;
    while(1) {
        wait(0.1);
        if(blue.readable() && blue.getc() == '!') {
            if(blue.readable() && blue.getc() == 'B') {
                bnum = blue.getc();
                bhit = blue.getc();
                blue.getc();
                switch(bnum) {
                    case '1':
                        needToStopRobot = 1;
                        break;
                    case '2': // become follower
                        t1.start(thread_read);
                        while (1) {
                            if (visible) { // CAMERA CAN SEE A CIRCLE
                                myled1 = 1;
                                myled2 = 0;
                                leftSpeed = DEFAULT_SPEED;
                                rightSpeed = DEFAULT_SPEED;
                                robot.driveWheels(leftSpeed, rightSpeed);
                            } else { // CIRCLE NOT VISIBLE
                                myled1 = 0;
                                myled2 = 1;
                                leftSpeed = ROTATE_SPEED;
                                rightSpeed = -ROTATE_SPEED;
                                robot.driveWheels(leftSpeed, rightSpeed);
                                wait(0.1);
                            }
                        }
                        // stop thread
                    case '5': //up
                        if(bhit=='1'){
                            leftSpeed = rightSpeed = DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    case '6': //down
                        if(bhit=='1') {
                            leftSpeed = rightSpeed = -DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    case '7': //left
                        if(bhit=='1') {
                            leftSpeed = -DEFAULT_SPEED;
                            rightSpeed = DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    case '8': //right
                        if(bhit=='1') {
                            leftSpeed = DEFAULT_SPEED;
                            rightSpeed = -DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    default:
                        robot.stop(0.5);
                        break;
                }
            }
        }
        if(needToStopRobot){
            robot.stop(0.5);
            needToStopRobot = 0;
            leftSpeed = 0;
            rightSpeed = 0;
            robot.driveWheels(leftSpeed, rightSpeed);
        } else {
            robot.driveWheels(leftSpeed, rightSpeed);
        }
    }
}