Basic Line Following Program motors: p25, p10 inputs: p15, p16 Current settings: lineToSpeedFactor = 10 / 3 speedModifier = -4

Dependencies:   mbed

Fork of RenBuggyLineFollower by Dan Argust

This small program lets a buggy follow a line (black line on white background)

main.cpp

Committer:
DanArgust
Date:
2016-07-14
Revision:
4:8f614e0b1cea
Parent:
3:936111f70e37
Child:
5:569a11ffee94

File content as of revision 4:8f614e0b1cea:

/*********************************************************
*RenBuggyLineFollower                                    *
*Author: Dan Argust                                      *
*                                                        *  
*This program demonstates the use of two sensors to      *
*detect and follow a thin black line.                    *
*********************************************************/

#include "mbed.h"

AnalogIn ain0(p15);
AnalogIn ain1(p16);

PwmOut pwm0(p25);
PwmOut pwm1(p10);

float lMotor;
float rMotor;
float newLine;
float oldLine;
bool  isSearching;
int   counter;

void computeMotorSpeeds(float a,float b){
    float inputs[2] = {a,b};
    const float MAXSPEED = 1.0;
    lMotor  = 0.0;
    rMotor  = 0.0;
    newLine = 0.0;
    float lineToSpeedFactor = MAXSPEED / 0.3;
    for (int i = 0; i < 2; ++i) {
        newLine += inputs[i] * (i + 1);
    }
    float sum = 0.0;
    for (int i = 0; i < 2; ++i)
        sum += inputs[i];
    newLine = newLine / sum;
    if ((a < 0.35) && (b < 0.35)) {
        if (oldLine > 1.5)
            newLine = 2.0;
        else
            newLine = 1.0;
        oldLine = newLine;
        isSearching = true;
    }
    if (isSearching) {
        if ( oldLine > 1.5)
            newLine = 2.0;
        else
            newLine = 1.0;
        if (abs(a - b) < 0.2)
            newLine = oldLine;
        else
            isSearching = false;
        counter++;
        if (counter > 300) {
            newLine = 3.0 - oldLine;
            counter = 0;
        }
    }
    oldLine = newLine;
    lMotor =      newLine * lineToSpeedFactor - 4.0;
    rMotor = 2.0-(newLine * lineToSpeedFactor - 4.0);
    if (lMotor > MAXSPEED)
        lMotor = MAXSPEED;
    if (rMotor > MAXSPEED)
        rMotor = MAXSPEED;
}

int main()
{
    isSearching = false;
    counter = 0;
    while (true) {
        computeMotorSpeeds(ain0.read(), ain1.read());
        pwm0 = lMotor;
        pwm1 = rMotor;
        wait_ms(10);
    }
}