File content as of revision 2:e2b6fe03e630:

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

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

//#define DEBUG
//************************************************************

//Declarations

//************************************************************
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;//global encoder count variables
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[] = "FS";//direction commands for debug sequence, can be used without xbee controll
//int num = 0;
char command = 'W'; // switch case variable to be used for commands from wireless comm, intialize W for wait
bool collision_flag = 0;//flag to set high for collision handling
bool once = 0;//send collision feedback once

Motor  left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Motor right(p26, p25, p24, 1);

bool x = true;//loop variable
//************************************************************

//Helper Functions

//************************************************************
//Functions for updating/reading encoder with interrupts
void Rcount() //count Right encoder increments
{
    countR++;
}
void Lcount() //count Left encoder increments
{
    countL++;
}
void clearEN() //clear the encoder values
{
    countR = 0;
    countL = 0;
}
//************************************************************

//Main

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

int main()
{
    float sl = 0.65;//preset wheel speed left
    float sr = 0.65*0.92;//preset wheel speed right, account for wobble of axel

    REN.fall(&Rcount);//attach counting functions for encoder interrupts
    LEN.fall(&Lcount);

    led1 = 1;led2 = 1;led3 = 1;led4 = 1;
    wait(5);//setup period
    led1 = 0;led2 = 0;led3 = 0;led4 = 0;

    bool x = true;//loop variable


    while (x) {//program loop

        //************************************************************
        if((IR >=0.60) && (collision_flag == 0)) {//if object sensed and flag not already raised
            left.stop(1);//stop if flag not raised
            right.stop(1);
            collision_flag = 1;//raise flag
        }
        if(IR < 0.60) {//if no object sensed, reset flags
            collision_flag = 0;
            once = 0;
        }
        if((collision_flag && !(once))) {//flag high and feeback not yet sense
            xbee1.putc('C');
            //pc.printf("C\n\r");
            once = 1;
        }
        if(xbee1.readable()) {//if new command 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;//middle indicator lights
                clearEN();
                while ((countL <= 500)||(countR <=340)) {//loop specified distance, axel wobble led to different encoder increment rates
                    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()) {// if new command while going forward
                        command = xbee1.getc();
                        //pc.printf("Command: %c\n\r",command);
                        break;
                    }
                    //pc.printf("Speed set\n\r");
                    left.speed(sl);
                    right.speed(0.69);//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 ~180 degrees right
                //pc.printf("RIGHT\n\r");
                led1 = 1;led2 = 1;
                clearEN();
                left.speed(sl);right.speed(-sr);
                while ((countL <= 40)) { //countR <= 20)) {//loop turn for specified angle
                    //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
                    //right.speed(-sr);
                    if(xbee1.readable()) {//if new command
                        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 30 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': //bump robot backwards
                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;//randomization code, will run different commands indefinetly
                switch( num ) {
                    case 1:
                        command = 'f';
                        break;
                    case 2:
                        command = 'r';
                        break;
                    case 3:
                        command = 'l';
                        break;
                    default:
                        command = 'b';
                        break;
                }*/
                command = debugcommand[count];//parse through debug command array
                count++;
                pc.printf("NEW COMMAND: %c", command);
#endif
                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;
        }
    }
}