File content as of revision 0:c2c6f1a295f4:

//#include "HMC6352.h" //compass
#include "mbed.h"
//HMC6352 compass(p28, p27);
Serial pc(USBTX, USBRX);
AnalogIn Rdist(p19); // right sonar
AnalogIn Ldist(p20);// left sonar
AnalogIn Fdist(p18);// front sonar
AnalogIn range(p17); // right range finder
Serial device(p9, p10);  // tx, rx
DigitalOut leftlight(LED1);
DigitalOut rightlight(LED3);
DigitalOut straightlight(LED2);

// Definitions of iRobot Create OpenInterface Command Numbers
// See the Create OpenInterface manual for a complete list


//                 Create Command              // Arguments
const char         Start = 128;
const char         SafeMode = 131;
const char         FullMode = 132;
const char         Drive = 137;                // 4:   [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
const char         DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]
const char         Demo = 136;                 // 2:    Run Demo x
const char         Sensors = 142;              // 1:    Sensor Packet ID
const char         CoverandDock = 143;         // 1:    Return to Charger
const char         SensorStream = 148;               // x+1: [# of packets requested] IDs of requested packets to stream
const char         QueryList = 149;            // x+1: [# of packets requested] IDs of requested packets to stream
const char         StreamPause = 150;          // 1:    0 = stop stream, 1 = start stream
const char         PlaySong = 141;
const char         Song = 140;
/* iRobot Create Sensor IDs */
const char         BumpsandDrops = 7;
const char         Distance = 19;
const char         Angle = 20;

static int speed_left =  200;
static int speed_right = 200;
static int state=0;
//static int arr[10];//0=forwatd, 1=right,2=left,3=finish;
void start();
void forward();
void reverse();
void left();
void right();
void stop();
void playsong();
void charger();
float curveR=0.02;
float curveL=0.02;
float Hsouth=98;
float Hnorth=68;
float Hwest=80;
float Heast=77;
// Demo to move around using basic commands
int main()
{
    int arr[5]= {0,1,0,1,40};//0=forwatd, 1=right,2=left,3=finish;
    float time=0;
// wait for Create to power up to accept serial commands
    wait(3);
    //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
// set baud rate for Create factory default
    device.baud(57600);
// Start command mode and select sensor data to send back
    start();
    wait(.5);
// Move around with motor commands
    while(1) {

        if((Rdist>0.49 || Ldist>0.49)&&(arr[state]<3)) {
            leftlight=1;
            rightlight=1;
            straightlight=1;
            if(float(Fdist)<0.12 ) {
                stop();
                wait(1);
                if(float(Fdist)<0.13 ) {
                    state++;
                    if(arr[state]==1) { //right turn
                        speed_right=200;
                        speed_left=200;
                        right();
                        wait(1.0);
                    } else if(arr[state]==2) {
                        speed_right=200;
                        speed_left=200;
                        left();
                        wait(1.0);
                    }//left
                    stop();
                    wait(0.2);
                    forward();
                    wait(6);
                    state++;
                }
            }

            // else {
            //   speed_right=200;
            // speed_left=200;
            //}
        } else if((float(Ldist)-float(Rdist))>curveL ) {
            speed_right++;
            leftlight=1;
            rightlight=0;
            straightlight=0;
            if(speed_right>206) {
                speed_right=206;
            }
        } else if ( (float(Rdist)-float(Ldist))>curveR ) {
            speed_left++;
            leftlight=0;
            rightlight=1;
            straightlight=0;
            if(speed_left>206) {
                speed_left=206;
            }
        }

        else if (((float(Ldist)-float(Rdist))<curveL) && ( (float(Rdist)-float(Ldist))<curveR )) {
            speed_left=200;
            speed_right=200;
            leftlight=0;
            rightlight=0;
            straightlight=1;
        }
        if(arr[state]>2) {
            time=time+0.1;
            if(time>arr[state]) {
                break;//while
            }
        }

        //pc.printf("Left: %f    ", float(Ldist));
        pc.printf("Right: %f   ", float(Rdist));
        pc.printf("range: %f   ", float(range));
        //pc.printf("front: %f   ", float(Fdist));
        //pc.printf("Diff: %f\n", (float(Rdist)-float(Ldist)));
        forward();
        wait(0.1);
    }
// Play a song
    stop();
    playsong();
    wait(10);
// Search for battery charger IR beacon
    // charger();
}


// Start  - send start and safe mode, start streaming sensor data
void start()
{
    // device.printf("%c%c", Start, SafeMode);
    device.putc(Start);
    device.putc(SafeMode);
    wait(.5);
    //  device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
    device.putc(SensorStream);
    device.putc(1);
    device.putc(BumpsandDrops);
    wait(.5);
}
// Stop  - turn off drive motors
void stop()
{
    device.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
}
// Forward  - turn on drive motors
void forward()
{
    device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),
                  char((speed_left>>8)&0xFF),  char(speed_left&0xFF));

}
// Reverse - reverse drive motors
void reverse()
{
    device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),
                  char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));

}
// Left - drive motors set to rotate to left
void left()
{
    device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),
                  char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));
}
// Right - drive motors set to rotate to right
void right()
{
    device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),
                  char((speed_left>>8)&0xFF),  char(speed_left&0xFF));

}
// Charger - search and return to charger using IR beacons (if found)
void charger()
{
    device.printf("%c%c", Demo, char(1));
}
// Play Song  - define and play a song
void playsong()   // Send out notes & duration to define song and then play song
{

    device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
                  Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
                  char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
                  char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
                  char(24), char(86), char(12), char(87), char(48));

    wait(.2);
    device.printf("%c%c", PlaySong, char(0));
}