File content as of revision 1:30e01a866efa:

// ECE 4180 Gesture Sensor Controlled Robot Project: Robot Comm, Heading and Motor Control, Obstacle Detection

#include "mbed.h"
#include "motordriver.h"

//#define DEBUG

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
AnalogIn IR(p20);//Input for IR sensor, 0.60 seems like a good object detect distance
InterruptIn REN(p15);//INTERRUPT IN PIN FOR RIGHT ENCODER
InterruptIn LEN(p16);//INTERRUPT IN PIN FOR LEFT ENCODER
int countR = 0;
int countL = 0;


Serial pc(USBTX, USBRX);//PC serial for debug
Serial xbee1(p13, p14);//xbee serial connection

int count = 0; //counter for reading through debug command array;
char debugcommand[] = "FRFRFRFRS";//direction commands for debug: forward, right, forward, back, forward, left, forward, stop
//int num = 0;
char command = 'W'; // switch case variable to be used for commands from wireless comm, default w for wait
bool collision_flag = 0;

Motor  left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Motor right(p26, p25, p24, 1);
bool x = true;
//Functions for updating/reading encoder
void Rcount() //count Right encoder increments
{
    countR++;
}
void Lcount() //count Left encoder increments
{
    countL++;
}
void clearEN() //clear the encoder values
{
    countR = 0;
    countL = 0;
}


int main()
{
    float sl = 0.65;//preset wheel speed left
    float sr = 0.65*0.92;//preset wheel speed right
    //************************************************************



    //************************************************************
    REN.fall(&Rcount);
    LEN.fall(&Lcount);

    led1 = 1;
    led2 = 1;
    led3 = 1;
    led4 = 1;

    wait(7);
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;

    bool x = true;//loop variable


    while (x) {//program loop

        //************************************************************
        if(IR >=0.60) {
            left.stop(1);
            right.stop(1);
            collision_flag = 1;
        }
        if(collision_flag) {
            xbee1.putc('C');
            pc.printf("C\n\r");
            collision_flag = 0;
        }
        if(xbee1.readable()) {
            command = xbee1.getc();
            pc.printf("Command: %c\n\r",command);
        }

        //************************************************************

        switch ( command ) {//switch statement for robot motor control
            case 'F': //move forward
                pc.printf("FORWARD\n\r");
                led2 = 1;
                led3 = 1;
                clearEN();
                while ((countL <= 500)||(countR <=340)) {//loop specified distance
                    if(IR >=0.60) {
                        left.stop(1);
                        right.stop(1);
                        collision_flag = 1;
                        break;
                    }   //if IR has detected a close object, stop
                    if(xbee1.readable()) {
                        command = xbee1.getc();
                        pc.printf("Command: %c\n\r",command);
                        break;
                    }
                    left.speed(sl);
                    right.speed(sr);//drift factor associated with our build
                    pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
                    wait(.01);
                }
                led2 = 0;
                led3 = 0;
                left.stop(1);
                right.stop(1);
                command = 'W';
                break;

            case 'R': //turn 90 deg right
                pc.printf("RIGHT\n\r");
                led1 = 1;
                led2 = 1;
                clearEN();
                while ((countL <= 80)) {//loop turn for specified angle
                    //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
                    left.speed(sl);
                    right.speed(-sr);//right.speed(-sr);
                    if(xbee1.readable()) {
                        command = xbee1.getc();
                        pc.printf("Command: %c\n\r",command);
                        break;
                    }
                }
                led1 = 0;
                led2 = 0;
                left.stop(1);
                right.stop(1);
                command = 'W';
                //pc.printf("END OF COMMAND:: RIGHT: %d, LEFT: %d\n\r", countR, countL);
                break;

            case 'L': //turn 90 deg left
                pc.printf("LEFT\n\r");
                led3 = 1;
                led4 = 1;

                clearEN();
                //loop turn for specified angle
                while (( countR<=55)) {
                    left.speed(-sl);
                    right.speed(sr);
                    if(xbee1.readable()) {
                        command = xbee1.getc();
                        pc.printf("Command: %c\n\r",command);
                        break;
                    }
                }
                led3 = 0;
                led4 = 0;
                left.stop(1);
                right.stop(1);
                command = 'W';
                break;

            case 'B': //turn 180 deg left
                pc.printf("BACKWARDS\n\r");
                led1 = 1;
                led4 = 1;
                left.speed(-sl);
                right.speed(-sr);
                wait(.5);//bump robot backwards by reversing both wheels for half a second

                led1 = 0;
                led4 = 0;
                left.stop(1);
                right.stop(1);
                command = 'W';
                break;

            case 'W': //wait for a new command
                pc.printf("WAIT\n\r");
                wait(.5);
                // Code
#ifdef DEBUG
                /*num = rand()%3;
                switch( num ) {
                    case 1:
                        command = 'f';
                        break;
                    case 2:
                        command = 'r';
                        break;
                    case 3:
                        command = 'l';
                        break;
                    default:
                        command = 'b';
                        break;
                }*/
                command = debugcommand[count];
                count++;
                pc.printf("NEW COMMAND: %c", command);
#endif
                command = 'W';
                break;
            default: //wait and show error
                pc.printf("DEFAULT\n\r");
                left.stop(0);
                right.stop(0);
                led1 = 1;
                led4 = 1;
                wait(.5);
                led2 = 1;
                led3 = 1;
                led1 =0;
                led4  = 0;
                wait(.5);
                led2 = 0;
                led3 = 0;
                break;
        }
    }
}