A simple line following program

Dependencies:   mbed

main.cpp

Committer:
chris
Date:
2010-11-01
Revision:
0:eb1ece444e15
Child:
1:5ddd3faed06d

File content as of revision 0:eb1ece444e15:

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

BusOut leds(LED1,LED2,LED3,LED4);
m3pi pi (p27,p9,p10); //nRST, TX, RX

int main() {

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

    float position_of_line = 0.0;

    pi.sensor_auto_calibrate();
    float speed = 0.4;

    while (1) {
        
        // -1.0 is far left, 1.0 is far right
        position_of_line = pi.line_position();
        
        // Line is more than 75% to the left
        if (position_of_line < -0.25) {
            pi.left_motor(speed + 0.3);
            pi.right_motor(speed - 0.3);
            leds = 0xc;
        } 
        // Line is more than 25% to the left
        else if (position_of_line < -0.10) {
            pi.left_motor(speed);
            pi.right_motor(speed - 0.3);
            leds = 0x4;
        } 
        // Line is more than 75% to the right
        else if (position_of_line > 0.25) {
            pi.right_motor(speed + 0.3);
            pi.left_motor(speed - 0.3);            
            leds = 0x3;
        } 
        // Line is more than 75% to the right        
        else if (position_of_line > 0.10) {
            pi.right_motor(speed);
            pi.left_motor(speed - 0.3);
            leds = 0x2;
        } 
        // Line is inthe middle        
        else {
            pi.forward(speed);
            leds = 0x0;
        }

    }

}