File content as of revision 6:ffbaca2bd3da:

/*********************************************************
*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;

/* This function is called every 10 miliseconds
** Using the two inputs from the sensor
**
** It calculates the where a mean value for the track would be
** between 1 and 2
** if the track is at 1.5 the buggy will travel straight
** if the track is at 1.0 the buggy will turn left
** if the track is at 2.0 the buggy will turn right
**
** if both inputs are less than 0.35
** the buggy will assume it has lost the line and will begin "seaching"
** while searching the buggy will turn in one direction until it finds a line
** if it fails to find a line in 600 attempts it will change direction
** this repeats
** the theory and algorithm can be found here: 
** https://www.ikalogic.com/line-tracking-sensors-and-algorithms/
*/
void computeMotorSpeeds(float a,float b){
    float inputs[2] = {a,b};
    const float MAXSPEED = 1.0; // Motor max speed
    lMotor  = 0.0;
    rMotor  = 0.0;
    newLine = 0.0;
    float lineToSpeedFactor = MAXSPEED / 0.3; // This is a modifier that will be used to set the motor speeds after calculating the position of the track
    
    
    // CALCULATING THE AVERAGE POSITION OF THE TRACK. LEFT = 1.0 / RIGHT = 2.0
    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; // newLine is now set somewhere between 1.0 and 2.0
    // CALCULATING THE AVERAGE POSITION OF THE TRACK. LEFT = 1.0 / RIGHT = 2.0
    
    
    if ((a < 0.35) && (b < 0.35)) { // if both the sensors are low then the line has been lost
        if (oldLine > 1.5)
            newLine = 2.0; // if the buggy was turning right before losing the line keep doing so
        else
            newLine = 1.0; // if the buggy was turning left before losing the line keep doing so
        oldLine = newLine;
        isSearching = true; // put the buggy in 'searching' mode
    }
    
    if (isSearching) {
        if ( oldLine > 1.5)
            newLine = 2.0;
        else
            newLine = 1.0;
        if (abs(a - b) < 0.2) // if a contrast is not detected (a track) then carry on
            newLine = oldLine;
        else                  // if a contrast is detected then turn off searching mode
            isSearching = false;
        counter++;
        if (counter > 600) {  // every 600 iteratons if searching change direction
            newLine = 3.0 - oldLine; // if the line is 1.0 then 3 - 1 = 2.0
            counter = 0;             // if the line is 2.0 then 3 - 2 = 1.0
        }
    }
    oldLine = newLine;
    lMotor =      newLine * lineToSpeedFactor - 4.0; // see below
    rMotor = 2.0-(newLine * lineToSpeedFactor - 4.0);// see below
    if (lMotor > MAXSPEED)
        lMotor = MAXSPEED;
    if (rMotor > MAXSPEED)
        rMotor = MAXSPEED;
}
/* The motor speeds are calculated by taking the average sensor reading that can be anywhere from 1 to 2
** if the sensor reading is 1.3:
** lMotor = 1.3 * 3.333 - 4 = 0.3333
** Rmotor = 2 - (1.3 * 3.333 - 4) = 1.6667 which gets capped to 1
**
** if the sensor reading is 1.2:
** lMotor = 1.2 * 3.333 - 4 = 0
** rMotor = 2 - (1.2 * 3.333 - 4) = 2 which gets capped to 1
** so the left motor is stopped if the average sensor value is less than 0.2
** this is also true for the right motor if the average sensor value is greater than 1.8
*/

int main()
{
    isSearching = false;
    counter = 0;
    while (true) {
        computeMotorSpeeds(ain0.read(), ain1.read()); // set the motor speed variables
        pwm0 = lMotor; // output the variables to the pwm outs
        pwm1 = rMotor;
        wait_ms(10); // wait 10 miliseconds and start again
    }
}