File content as of revision 16:47d3b1f2e90d:

//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); 
 
int main() {
    char bnum = 0;
    char bhit = 0;
    char needToStopRobot = 0;
    float leftSpeed = 0;
    float rightSpeed = 0;
    float DEFAULT_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 '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;
        } else {
            robot.driveWheels(leftSpeed, rightSpeed);   
        }
    }
}