File content as of revision 17:ec52258b9472:

#include "mbed.h"
#include "Brobot.h"
#include "SpeedControl.h"

//communication
Serial pc(USBTX, USBRX);
Serial cam(PB_10, PC_5);  // we are only using PA_9 to recieve

//Perophery for distance sensors
AnalogIn distance(PB_1);
DigitalIn button(USER_BUTTON);
DigitalOut enable(PC_1);  // sensors
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);
DigitalOut led(LED1);
Pixy pixy(cam);


//indicator leds arround robot
DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };

//motor stuff
DigitalOut enableMotorDriver(PB_2);
PwmOut pwmL(PA_8);
PwmOut pwmR(PA_9);
UniServ servo(PC_4); // Digital Out Pin

EncoderCounter counterLeft(PB_6, PB_7);
EncoderCounter counterRight(PA_6, PC_7);

SpeedControl speedctrl(&pwmL, &pwmR, &counterLeft, &counterRight);

Brobot stingray(speedctrl, distance, enable, bit0, bit1, bit2, leds, pixy, servo);  // passing in variables connected to PIN

int main()
{
    enableMotorDriver = 0;  //safety precaution
    enable = 1 ;            //for debugging and output

    enum robot_states {idle=0, search, approach, goHome, atHome};   //so that we can reference states by name vs number
    int robot_state  = idle;    // define and start in idle state
    int count = 0 ;
    int timer = 0;

    while(1) {
        wait( 0.01f ); //required to allow time for ticker

        if( ++timer%100==0)
            led=!led;

        switch(robot_state) {
            case idle:
                if(button == 0) {       // 0 is pressed
                    enableMotorDriver = 1;
                    robot_state=search;         // next state
                }
                break;

            case search:

                stingray.avoidObstacleAndMove(12);  // in rpm
                stingray.openGrip();  //just trying to get lucky

                if(stingray.foundGreenBrick()) {
                    stingray.stop();
                    leds[3] = 1;
                    robot_state = approach;
                }
                break;

            case approach:

                int rotateAndApproachState = stingray.rotateAndApproach();
                if(rotateAndApproachState== 1) robot_state = goHome ; // gripper has block
                else if(rotateAndApproachState == 2) robot_state = search ; // block never made it to gripper\
                count = 0;

                break; // continue approach

            case goHome:

                if(stingray.foundHome() == false && count <= 750) { // tune 5 so that robot rotates ~360 degrees
                    stingray.rotate(5);
                    count++;
                }
                if( stingray.foundHome() == false && count > 750 ) {
                    stingray.avoidObstacleAndMove(10);  // search until home is found
                }
                static bool foundHomeFlag=false;
                // if this point is reached, we have foundHome() == true
                if( stingray.foundHome() || foundHomeFlag ) {
                    foundHomeFlag=true;
                    if(stingray.approachHome())
                        robot_state = atHome ;
                }
                break;

            case atHome:

                stingray.openGrip();
                wait(0.5);  // may not be necessary but allows grip time
                stingray.back(); // goes at 15rpms
                wait(1);
                stingray.rotate(10);  // enter rotational velocity in rpm between +-25
                wait(0.5);  // tune so that rotation is about 180deg
                robot_state=search;  // restart at searching state

        }
    }
}