main.cpp

#include "mbed.h"
#include "Servo.h"

Serial blue(p28,p27);
AnalogIn infared(p15);

Servo myservo(p25);

PwmOut leftSpeed(p21);
DigitalOut leftA(p23);
DigitalOut leftB(p24);

PwmOut rightSpeed(p22);
DigitalOut rightA(p20);
DigitalOut rightB(p19);

PwmOut speaker(p26);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

float leftDistance = 0.0f;
float rightDistance = 0.0f;

void StopMotors()
{
    leftSpeed.write(0.0f);
    leftA = 0;
    leftB = 0;
    rightSpeed.write(0.0f);
    rightA = 0;
    rightB = 0;
}

void Forward()
{
    leftSpeed.write(0.35f);
    leftA = 1;
    leftB = 0;
    rightSpeed.write(0.35f);
    rightA = 1;
    rightB = 0;
}

void TurnLeft()
{
    leftSpeed.write(0.35f);
    leftA = 0;
    leftB = 1;
    rightSpeed.write(0.35f);
    rightA = 1;
    rightB = 0;
}

void TurnRight()
{
    leftSpeed.write(0.35f);
    leftA = 1;
    leftB = 0;
    rightSpeed.write(0.35f);
    rightA = 0;
    rightB = 1;
}    

int main() 
{ 
    char bnum=0;
    char bhit=0;
    while (1)
    {
        led2 = led3 = led4 = 0;
        if (blue.readable() && 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 '5': //button 5 up arrow
                            if (bhit=='1') {
                                led2 = 1;
                                Forward();
                            } else {
                                StopMotors();
                            }
                            break;
                        case '6': //button 6 down arrow
                            if (bhit=='1') {
                                //add hit code here
                            } else {
                                StopMotors();
                            }
                            break;
                        case '7': //button 7 left arrow
                            if (bhit=='1') {
                                led3 = 1;
                                TurnRight();
                            } else {
                                StopMotors();
                            }
                            break;
                        case '8': //button 8 right arrow
                            if (bhit=='1') {
                                led4 = 1;
                                TurnLeft();
                            } else {
                                StopMotors();
                            }
                            break;
                        default:
                            StopMotors();
                            break;
                    }
                }
            }
        }
        if(infared > 0.6f)
        {
            wait(0.1);
            if(infared > 0.6f)
            {
                led1 =1;
                //speaker.period(1.0/500.0); // 500hz period
                speaker =0.5; //50% duty cycle - max volume
                StopMotors();
                wait(1);
                speaker=0.0;
                myservo = 0.0f;
                wait(1);
                for(int i = 0; i < 30; i++)
                {
                    leftDistance += infared;
                }
                leftDistance /= 30;
                myservo = 1.0f;
                wait(1);
                for(int i = 0; i < 30; i++)
                {
                    rightDistance += infared;
                }
                rightDistance /= 30;
                myservo = 0.5f;
                wait(1);
                
                if(leftDistance < rightDistance)
                {
                    led2 = 1;
                    TurnRight();
                    wait(1);
                    StopMotors();
                    wait(1);
                    led2 = 0;
                }
                else
                {
                    led3 = 1;
                    TurnLeft();
                    wait(1);
                    StopMotors();
                    wait(1);
                    led3 = 0;
                } 
            }
        }
        else
        {
            led1 = 0;
        }       
    }
}