File content as of revision 0:342c14fb10c0:

/**
Author: Giles Barton-Owen
Name: mbed ranger
Description:

This program keeps a m3pi inside a ring, and has 6 (4 Active) different game modes.
It relies on a Sharp Digital Distance sensor (GP2Y0D810) attached to pin 30 and 11 
(the two headers on the right and left of the board). The system refreshes at 10Hz
when it reads the line sensors on the 3pi and the distance states. The states are
then handled in a big FSM to reach the conclusions.

To keep the robot saving itself for longer a timeout was programmed, such that the
robot belives itself on a line (as was reported when the timeout was set) until 
0.3 seconds has elapsed. This could be adjusted.

The fore-ground loop just switches the LEDs on the board as instructed. The current
state of the robot is reported on line 2 of the LCD screen on the 3pi, displaying
first mode and the last character being line detection state (none, front, back).


**/

#include "mbed.h"
#include "SharpDigiDist100.h"
#include "m3pi.h"

DigitalOut Left[2] = {LED1,LED2};           //Some indicator LEDs for the range finders
DigitalOut Right[2] = {LED4,LED3};

SharpDigiDist100 right(p30);                // The range finder class initialisations
SharpDigiDist100 left(p11);

m3pi m3pi;                                  // Initialise the m3pi

InterruptIn button(p21);                    // SW1 on the shield, for stratergy switching

Ticker guidance;                            // The main guidance caller

Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging

Timer debounce;                             // To debounce the switch

int previousLine;                           // A set of variables to sort out the line detection, previousLine is pretty much not used
int isLine;
int lineCatch;

Timeout liner;                              // A timeout to stop it ignoring the line

void CycleMode();                           // Function defs
void guideCall();
void clearLiner();

enum Stratergies {                          // The enum for stratergies
    Off,
    Dormant,
    RunMid,
    RunNear,
    AttackMid,
    AttackFar,
    Hunt
};

void updateScreen(int line, enum Stratergies stratergy);

enum Stratergies stratMode = Off;       // The current stratergy

int main() {
    guidance.attach(&guideCall,0.1);        // Sets up the control loop
    
    button.mode(PullUp);                    // Sets the internal pull up on SW1
    button.fall(&CycleMode);                // Attaches an interupt for when it is pressed
    
    m3pi.locate(0,0);                       // Write the name to the screen
    m3pi.printf("m3PiRngr");                
    
    m3pi.get_white_levels();                // Saves the current levels of the sensors to know what is white
    
    debounce.start();                       // Starts the debounce timer so that the switch thinks it isnt a bounce first time
    
    //pc.printf("\n\n\rMbedRanger\n\r");      // Prints a hello to the computer
    
    while (1) {
        //pc.printf("Loop\n\r");
        switch (right.getDistance()) {      // Sets up the distance indicator LEDs for the right side
            case SharpDigiDist100::Far :
                Right[0] = true;
                Right[1] = false;
                break;
            case SharpDigiDist100::Near :
                Right[1] = true;
                Right[0] = false;
                break;
            case SharpDigiDist100::Mid :
                Right[0] = true;
                Right[1] = true;
                break;
            default:
                break;
        }
        switch (left.getDistance()) {       // Sets up the distance indicator LEDs for the left side
            case SharpDigiDist100::Far :
                Left[0] = true;
                Left[1] = false;
                break;
            case SharpDigiDist100::Near :
                Left[1] = true;
                Left[0] = false;
                break;
            case SharpDigiDist100::Mid :
                Left[0] = true;
                Left[1] = true;
                break;
            default:
                break;
        }
        /*int calibrated[5];
        m3pi.get_raw_sensors(calibrated);
         pc.printf("Sensors read:");
        for (int i = 0; i<5;i++) {
            pc.printf(" %i ",calibrated[i]);
        }
        if (m3pi.is_line() > 0) {
            pc.printf("LINE");
            m3pi.locate(0,1);
            m3pi.printf("LINE");
        }
        pc.printf("\n\r");
        wait(0.1);*/
    }
}



void CycleMode() {                      // Cycles through the modes, probably could have written this with a simple ++ statement...
    debounce.stop();                    // Stops and reads the timer since the last press of the button
    if (debounce.read_ms() > 100) {     // If it was more than 100ms ago it treats it as a button press, otherwise it just ignores it
        switch (stratMode) {
            case Off:
                stratMode = Dormant;
                m3pi.locate(0,1);
                m3pi.printf("\nDormant ");
                break;
            case Dormant:
                stratMode = RunMid;
                m3pi.locate(0,1);
                m3pi.printf("\nRunMid ");
                break;
            case RunMid:
                stratMode = RunNear;
                m3pi.locate(0,1);
                m3pi.printf("\nRunNear ");
                break;
            case RunNear:
                stratMode = AttackMid;
                m3pi.locate(0,1);
                m3pi.printf("\nTakNear ");
                break;
            case AttackMid:
                stratMode = AttackFar;
                m3pi.locate(0,1);
                m3pi.printf("\nTakFar ");
                break;
            case AttackFar:
                stratMode = Hunt;
                m3pi.locate(0,1);
                m3pi.printf("\nHUNTING! ");
                break;
            case Hunt:
                stratMode = Off;
                m3pi.printf("\nOFF ");
                break;
        }
    }
    debounce.reset();
    debounce.start();
}

void guideCall() {

    isLine = m3pi.is_line();            // Gets whether the m3pi is on a line, and if so front/back
    
    if (lineCatch == 0) {               // Has it been off a line for long enough?
        isLine = isLine;                // Yes - then go ahead
    } else {
        isLine = lineCatch;             // No - pretend to still be on that line
        
    }
    float position;                     
    
    switch (isLine) {
        case 0:                         // No line, not even recently so go ahead with the stratergies
            
            updateScreen(isLine, stratMode);
            bool atRight = false;
            bool atLeft = false;
            
            switch (stratMode) {
                case Off:               // No motors
                case Dormant:           // Will take action against lines though
                    m3pi.stop();
                    break;

                case RunNear:           // Runs if something is near
                    if (right.getDistance() == SharpDigiDist100::Near) {
                        atRight = true;
                    } else atRight = false;
                    if (left.getDistance() == SharpDigiDist100::Near) {
                        atLeft = true;
                    } else atLeft = false;

                case RunMid:            // Runs if something is in the middle distance

                    if (right.getDistance() == SharpDigiDist100::Mid) {
                        atRight = true;
                    }
                    if (left.getDistance() == SharpDigiDist100::Mid) {
                        atLeft = true;
                    }


                    if (atRight && atLeft) {
                        m3pi.backward(0.5);
                    } else {
                        if (atRight == true) {
                            m3pi.left_motor(-0.3);
                            m3pi.right_motor(-0.5);
                        } else {
                            if (atLeft == true) {
                                m3pi.left_motor(-0.5);
                                m3pi.right_motor(-0.3);
                            } else {
                                m3pi.stop();
                            }
                        }
                    }
                    break;

                case AttackMid:         // Attacks something in the middle distance
                    if (right.getDistance() == SharpDigiDist100::Mid) {
                        atRight = true;
                    }
                    if (left.getDistance() == SharpDigiDist100::Mid) {
                        atLeft = true;
                    }


                    if (atRight && atLeft) {
                        m3pi.forward(0.6);
                    } else {
                        if (atRight == true) {
                            m3pi.left_motor(0.5);
                            m3pi.right_motor(0.7);
                        } else {
                            if (atLeft == true) {
                                m3pi.left_motor(0.7);
                                m3pi.right_motor(0.5);
                            } else {
                                m3pi.stop();
                            }
                        }
                    }

                    if (right.getDistance() == SharpDigiDist100::Near) {
                        atRight = true;
                    } else atRight = false;
                    if (left.getDistance() == SharpDigiDist100::Near) {
                        atLeft = true;
                    } else atLeft = false;


                    if (atRight && atLeft) {
                        m3pi.forward(0.5);
                    } else {
                        if (atRight == true) {
                            m3pi.left_motor(0.1);
                            m3pi.right_motor(0.2);
                        } else {
                            if (atLeft == true) {
                                m3pi.left_motor(0.2);
                                m3pi.right_motor(0.1);
                            }
                        }
                    }

                    break;

                case AttackFar:

                    break;
                case Hunt:                  // Runs forward until something is really close

                    if (right.getDistance() == SharpDigiDist100::Mid || right.getDistance() == SharpDigiDist100::Near) {
                        atRight = true;
                    } else atRight = false;
                    if (left.getDistance() == SharpDigiDist100::Mid || left.getDistance() == SharpDigiDist100::Near) {
                        atLeft = true;
                    } else atLeft = false;

                    if (atRight && atLeft) {
                        m3pi.stop();
                    } else {
                        if (atRight == true) {
                            m3pi.left_motor(0.1);
                            m3pi.right_motor(0.2);
                        } else {
                            if (atLeft == true) {
                                m3pi.left_motor(0.2);
                                m3pi.right_motor(0.1);
                            } else {
                                m3pi.forward(0.3);
                            }
                        }
                    }
                    break;
                default:
                    break;
            }
            break;

        case 1:                 // Line in front, reverse
            if(stratMode != Off)
            {
            if (lineCatch == 0) {
                lineCatch = 1;

                liner.attach(&clearLiner, 0.3);
            }

            position = m3pi.line_position();
            if (position < 0) {
                m3pi.left_motor(-1);
                m3pi.right_motor(-0.8);
            } else if (position == 0) {
                m3pi.backward(1);
            } else if (position > 0) {
                m3pi.left_motor(-0.8);
                m3pi.right_motor(-1);
            }

            //m3pi.locate(0,1);
            //m3pi.printf("LINE_FWD");
            }
            else
            {
                m3pi.stop();
            }
            updateScreen(isLine, stratMode);
            break;

        case -1:            // Line behind, forward
            if(stratMode != Off)
            {
            if (lineCatch == 0) {
                lineCatch = -1;
                liner.attach(&clearLiner, 0.3);
            }


            position = m3pi.line_position();
            if (position < 0) {
                m3pi.left_motor(1);
                m3pi.right_motor(0.8);
            } else if (position == 0) {
                m3pi.forward(1);
            } else if (position > 0) {
                m3pi.left_motor(0.8);
                m3pi.right_motor(1);
            }
            }
            else
            {
                m3pi.stop();
            }
            //m3pi.locate(0,1);
            //m3pi.printf("LINE_BKD");
            updateScreen(isLine, stratMode);
            break;
    }

    //previousLine = isLine;
}

void clearLiner() {             // Gets called a bit after a line is detected
    lineCatch = 0;
    //pc.printf("Cleared liner\n\r");
}

void updateScreen(int line, enum Stratergies stratergy) {       // Update the bottom line with the running info
    m3pi.locate(0,1);
    
    char lineState;
    
    switch (line) {
        case 0:
            lineState = 'N';
            break;
        case 1:
            lineState = 'F';
            break;
        case -1:
            lineState = 'B';
            break;
        default:
            break;
    }

    char strat[6];
    switch (stratergy) {
        case Off:
            sprintf(strat,"OFF   ");
            break;
        case Dormant:
            sprintf(strat,"DORMNT");
            break;
        case RunMid:
            sprintf(strat,"RUNMID");

            break;
        case RunNear:
            sprintf(strat,"RNNEAR");

            break;
        case AttackMid:
            sprintf(strat,"TAKMID");

            break;
        case AttackFar:
            sprintf(strat,"TAKFAR");

            break;
        case Hunt:
            sprintf(strat,"HUNTER");

            break;
    }

    m3pi.printf("%s %c",strat,lineState);
}