A simple line following program

Dependencies:   mbed

main.cpp

Committer:
chris
Date:
2010-11-10
Revision:
5:c705b7e7e1a8
Parent:
3:7f077cf1d755
Child:
6:8f46c6ac55ca

File content as of revision 5:c705b7e7e1a8:

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

BusOut leds(LED1,LED2,LED3,LED4);
m3pi m3pi(p23,p9,p10);

int main() {

    m3pi.locate(0,1);
    m3pi.printf("Line Flw");

    wait(2.0);

    float position_of_line = 0.0;
    m3pi.sensor_auto_calibrate();
    float speed = 0.4;

    while (1) {

        // -1.0 is far left, 1.0 is far right
        position_of_line = m3pi.line_position();

        // Line is more than 25% to the left
        if (position_of_line < -0.50) {
            m3pi.left_motor(speed);
            m3pi.right_motor(speed - 0.3);
            leds = 0x4;
        }
        // Line is more than 75% to the right
        else if (position_of_line > 0.50) {
            m3pi.right_motor(speed);
            m3pi.left_motor(speed - 0.3);
            leds = 0x2;
        }
        // Line is in the middle
        else {
            m3pi.forward(speed);
            leds = 0x0;
        }
    }
}