Renishaw / Mbed 2 deprecated RenBuggyLineFollower

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@6:ffbaca2bd3da, 2016-08-03 (annotated)

Committer:
DanArgust
Date:
Wed Aug 03 07:10:09 2016 +0000
Revision:
6:ffbaca2bd3da
Parent:
5:569a11ffee94 
comments and format changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DanArgust 0:466ee63955df 1 /*********************************************************
DanArgust 0:466ee63955df 2 *RenBuggyLineFollower *
DanArgust 0:466ee63955df 3 *Author: Dan Argust *
DanArgust 0:466ee63955df 4 * *
DanArgust 0:466ee63955df 5 *This program demonstates the use of two sensors to *
DanArgust 0:466ee63955df 6 *detect and follow a thin black line. *
DanArgust 0:466ee63955df 7 *********************************************************/
DanArgust 0:466ee63955df 8
DanArgust 2:2439f5a4a93d 9 #include "mbed.h"
DanArgust 0:466ee63955df 10
DanArgust 0:466ee63955df 11 AnalogIn ain0(p15);
DanArgust 0:466ee63955df 12 AnalogIn ain1(p16);
DanArgust 0:466ee63955df 13
DanArgust 0:466ee63955df 14 PwmOut pwm0(p25);
DanArgust 0:466ee63955df 15 PwmOut pwm1(p10);
DanArgust 0:466ee63955df 16
DanArgust 4:8f614e0b1cea 17 float lMotor;
DanArgust 4:8f614e0b1cea 18 float rMotor;
DanArgust 0:466ee63955df 19 float newLine;
DanArgust 0:466ee63955df 20 float oldLine;
DanArgust 4:8f614e0b1cea 21 bool isSearching;
DanArgust 4:8f614e0b1cea 22 int counter;
DanArgust 0:466ee63955df 23
DanArgust 6:ffbaca2bd3da 24 /* This function is called every 10 miliseconds
DanArgust 6:ffbaca2bd3da 25 ** Using the two inputs from the sensor
DanArgust 6:ffbaca2bd3da 26 **
DanArgust 6:ffbaca2bd3da 27 ** It calculates the where a mean value for the track would be
DanArgust 6:ffbaca2bd3da 28 ** between 1 and 2
DanArgust 6:ffbaca2bd3da 29 ** if the track is at 1.5 the buggy will travel straight
DanArgust 6:ffbaca2bd3da 30 ** if the track is at 1.0 the buggy will turn left
DanArgust 6:ffbaca2bd3da 31 ** if the track is at 2.0 the buggy will turn right
DanArgust 6:ffbaca2bd3da 32 **
DanArgust 6:ffbaca2bd3da 33 ** if both inputs are less than 0.35
DanArgust 6:ffbaca2bd3da 34 ** the buggy will assume it has lost the line and will begin "seaching"
DanArgust 6:ffbaca2bd3da 35 ** while searching the buggy will turn in one direction until it finds a line
DanArgust 6:ffbaca2bd3da 36 ** if it fails to find a line in 600 attempts it will change direction
DanArgust 6:ffbaca2bd3da 37 ** this repeats
DanArgust 6:ffbaca2bd3da 38 ** the theory and algorithm can be found here:
DanArgust 6:ffbaca2bd3da 39 ** https://www.ikalogic.com/line-tracking-sensors-and-algorithms/
DanArgust 6:ffbaca2bd3da 40 */
DanArgust 4:8f614e0b1cea 41 void computeMotorSpeeds(float a,float b){
DanArgust 0:466ee63955df 42 float inputs[2] = {a,b};
DanArgust 6:ffbaca2bd3da 43 const float MAXSPEED = 1.0; // Motor max speed
DanArgust 4:8f614e0b1cea 44 lMotor = 0.0;
DanArgust 4:8f614e0b1cea 45 rMotor = 0.0;
DanArgust 4:8f614e0b1cea 46 newLine = 0.0;
DanArgust 6:ffbaca2bd3da 47 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
DanArgust 6:ffbaca2bd3da 48
DanArgust 6:ffbaca2bd3da 49
DanArgust 6:ffbaca2bd3da 50 // CALCULATING THE AVERAGE POSITION OF THE TRACK. LEFT = 1.0 / RIGHT = 2.0
DanArgust 4:8f614e0b1cea 51 for (int i = 0; i < 2; ++i) {
DanArgust 4:8f614e0b1cea 52 newLine += inputs[i] * (i + 1);
DanArgust 0:466ee63955df 53 }
DanArgust 4:8f614e0b1cea 54 float sum = 0.0;
DanArgust 4:8f614e0b1cea 55 for (int i = 0; i < 2; ++i)
DanArgust 0:466ee63955df 56 sum += inputs[i];
DanArgust 6:ffbaca2bd3da 57 newLine = newLine / sum; // newLine is now set somewhere between 1.0 and 2.0
DanArgust 6:ffbaca2bd3da 58 // CALCULATING THE AVERAGE POSITION OF THE TRACK. LEFT = 1.0 / RIGHT = 2.0
DanArgust 6:ffbaca2bd3da 59
DanArgust 6:ffbaca2bd3da 60
DanArgust 6:ffbaca2bd3da 61 if ((a < 0.35) && (b < 0.35)) { // if both the sensors are low then the line has been lost
DanArgust 4:8f614e0b1cea 62 if (oldLine > 1.5)
DanArgust 6:ffbaca2bd3da 63 newLine = 2.0; // if the buggy was turning right before losing the line keep doing so
DanArgust 0:466ee63955df 64 else
DanArgust 6:ffbaca2bd3da 65 newLine = 1.0; // if the buggy was turning left before losing the line keep doing so
DanArgust 4:8f614e0b1cea 66 oldLine = newLine;
DanArgust 6:ffbaca2bd3da 67 isSearching = true; // put the buggy in 'searching' mode
DanArgust 2:2439f5a4a93d 68 }
DanArgust 6:ffbaca2bd3da 69
DanArgust 4:8f614e0b1cea 70 if (isSearching) {
DanArgust 4:8f614e0b1cea 71 if ( oldLine > 1.5)
DanArgust 4:8f614e0b1cea 72 newLine = 2.0;
DanArgust 4:8f614e0b1cea 73 else
DanArgust 4:8f614e0b1cea 74 newLine = 1.0;
DanArgust 6:ffbaca2bd3da 75 if (abs(a - b) < 0.2) // if a contrast is not detected (a track) then carry on
DanArgust 2:2439f5a4a93d 76 newLine = oldLine;
DanArgust 6:ffbaca2bd3da 77 else // if a contrast is detected then turn off searching mode
DanArgust 4:8f614e0b1cea 78 isSearching = false;
DanArgust 3:936111f70e37 79 counter++;
DanArgust 6:ffbaca2bd3da 80 if (counter > 600) { // every 600 iteratons if searching change direction
DanArgust 6:ffbaca2bd3da 81 newLine = 3.0 - oldLine; // if the line is 1.0 then 3 - 1 = 2.0
DanArgust 6:ffbaca2bd3da 82 counter = 0; // if the line is 2.0 then 3 - 2 = 1.0
DanArgust 2:2439f5a4a93d 83 }
DanArgust 0:466ee63955df 84 }
DanArgust 0:466ee63955df 85 oldLine = newLine;
DanArgust 6:ffbaca2bd3da 86 lMotor = newLine * lineToSpeedFactor - 4.0; // see below
DanArgust 6:ffbaca2bd3da 87 rMotor = 2.0-(newLine * lineToSpeedFactor - 4.0);// see below
DanArgust 4:8f614e0b1cea 88 if (lMotor > MAXSPEED)
DanArgust 4:8f614e0b1cea 89 lMotor = MAXSPEED;
DanArgust 4:8f614e0b1cea 90 if (rMotor > MAXSPEED)
DanArgust 4:8f614e0b1cea 91 rMotor = MAXSPEED;
DanArgust 0:466ee63955df 92 }
DanArgust 6:ffbaca2bd3da 93 /* The motor speeds are calculated by taking the average sensor reading that can be anywhere from 1 to 2
DanArgust 6:ffbaca2bd3da 94 ** if the sensor reading is 1.3:
DanArgust 6:ffbaca2bd3da 95 ** lMotor = 1.3 * 3.333 - 4 = 0.3333
DanArgust 6:ffbaca2bd3da 96 ** Rmotor = 2 - (1.3 * 3.333 - 4) = 1.6667 which gets capped to 1
DanArgust 6:ffbaca2bd3da 97 **
DanArgust 6:ffbaca2bd3da 98 ** if the sensor reading is 1.2:
DanArgust 6:ffbaca2bd3da 99 ** lMotor = 1.2 * 3.333 - 4 = 0
DanArgust 6:ffbaca2bd3da 100 ** rMotor = 2 - (1.2 * 3.333 - 4) = 2 which gets capped to 1
DanArgust 6:ffbaca2bd3da 101 ** so the left motor is stopped if the average sensor value is less than 0.2
DanArgust 6:ffbaca2bd3da 102 ** this is also true for the right motor if the average sensor value is greater than 1.8
DanArgust 6:ffbaca2bd3da 103 */
DanArgust 0:466ee63955df 104
DanArgust 0:466ee63955df 105 int main()
DanArgust 0:466ee63955df 106 {
DanArgust 4:8f614e0b1cea 107 isSearching = false;
DanArgust 3:936111f70e37 108 counter = 0;
DanArgust 4:8f614e0b1cea 109 while (true) {
DanArgust 6:ffbaca2bd3da 110 computeMotorSpeeds(ain0.read(), ain1.read()); // set the motor speed variables
DanArgust 6:ffbaca2bd3da 111 pwm0 = lMotor; // output the variables to the pwm outs
DanArgust 4:8f614e0b1cea 112 pwm1 = rMotor;
DanArgust 6:ffbaca2bd3da 113 wait_ms(10); // wait 10 miliseconds and start again
DanArgust 0:466ee63955df 114 }
DanArgust 0:466ee63955df 115 }