File content as of revision 15:4efc66de795a:

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

//communication
Serial pc(USBTX, USBRX);
Serial cam(PA_9, PA_10);  // 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
// should see LEDshow once constructed

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 ;

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

        stingray.startLeds();

        switch(robot_state) {
            case idle:
                if(button == 0) {       // 0 is pressed
                    enableMotorDriver =1;
                    robot_state=search;         // next state
                    //printf("leaving the idle state \n\r");
                    //wait(0.1);
                }
                break;

            case search:
                /*
                while( !stingray.foundGreenBrick() ) {
                    stingray.avoidObstacleAndMove(20);  // in rpm
                }
                */

                stingray.avoidObstacleAndMove(20);  // in rpm
                //robot_state=approach;
                break;

            case approach:
            
                stingray.stop();

                //stingray.approachBrick();
                //stingray.closeGrip();
                // find way to determine if lego is successfully grabbed before switching states
                // robot_state=goHome;

                break;

            case goHome:

                //printf("In the GoHome state \n\r");

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

                robot_state = atHome;
                count =0;

                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

        }
    }
}