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@5:569a11ffee94, 2016-07-15 (annotated)

Committer:
DanArgust
Date:
Fri Jul 15 10:36:00 2016 +0000
Revision:
5:569a11ffee94
Parent:
4:8f614e0b1cea
Child:
6:ffbaca2bd3da
Line following robot; Current settings:;     inputs = 2;     lineToSpeedFactor = 10 / 3;     speedModifier = -4

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 4:8f614e0b1cea 24 void computeMotorSpeeds(float a,float b){
DanArgust 0:466ee63955df 25 float inputs[2] = {a,b};
DanArgust 4:8f614e0b1cea 26 const float MAXSPEED = 1.0;
DanArgust 4:8f614e0b1cea 27 lMotor = 0.0;
DanArgust 4:8f614e0b1cea 28 rMotor = 0.0;
DanArgust 4:8f614e0b1cea 29 newLine = 0.0;
DanArgust 4:8f614e0b1cea 30 float lineToSpeedFactor = MAXSPEED / 0.3;
DanArgust 4:8f614e0b1cea 31 for (int i = 0; i < 2; ++i) {
DanArgust 4:8f614e0b1cea 32 newLine += inputs[i] * (i + 1);
DanArgust 0:466ee63955df 33 }
DanArgust 4:8f614e0b1cea 34 float sum = 0.0;
DanArgust 4:8f614e0b1cea 35 for (int i = 0; i < 2; ++i)
DanArgust 0:466ee63955df 36 sum += inputs[i];
DanArgust 0:466ee63955df 37 newLine = newLine / sum;
DanArgust 4:8f614e0b1cea 38 if ((a < 0.35) && (b < 0.35)) {
DanArgust 4:8f614e0b1cea 39 if (oldLine > 1.5)
DanArgust 4:8f614e0b1cea 40 newLine = 2.0;
DanArgust 0:466ee63955df 41 else
DanArgust 4:8f614e0b1cea 42 newLine = 1.0;
DanArgust 4:8f614e0b1cea 43 oldLine = newLine;
DanArgust 4:8f614e0b1cea 44 isSearching = true;
DanArgust 2:2439f5a4a93d 45 }
DanArgust 4:8f614e0b1cea 46 if (isSearching) {
DanArgust 4:8f614e0b1cea 47 if ( oldLine > 1.5)
DanArgust 4:8f614e0b1cea 48 newLine = 2.0;
DanArgust 4:8f614e0b1cea 49 else
DanArgust 4:8f614e0b1cea 50 newLine = 1.0;
DanArgust 4:8f614e0b1cea 51 if (abs(a - b) < 0.2)
DanArgust 2:2439f5a4a93d 52 newLine = oldLine;
DanArgust 4:8f614e0b1cea 53 else
DanArgust 4:8f614e0b1cea 54 isSearching = false;
DanArgust 3:936111f70e37 55 counter++;
DanArgust 5:569a11ffee94 56 if (counter > 600) {
DanArgust 4:8f614e0b1cea 57 newLine = 3.0 - oldLine;
DanArgust 2:2439f5a4a93d 58 counter = 0;
DanArgust 2:2439f5a4a93d 59 }
DanArgust 0:466ee63955df 60 }
DanArgust 0:466ee63955df 61 oldLine = newLine;
DanArgust 4:8f614e0b1cea 62 lMotor = newLine * lineToSpeedFactor - 4.0;
DanArgust 4:8f614e0b1cea 63 rMotor = 2.0-(newLine * lineToSpeedFactor - 4.0);
DanArgust 4:8f614e0b1cea 64 if (lMotor > MAXSPEED)
DanArgust 4:8f614e0b1cea 65 lMotor = MAXSPEED;
DanArgust 4:8f614e0b1cea 66 if (rMotor > MAXSPEED)
DanArgust 4:8f614e0b1cea 67 rMotor = MAXSPEED;
DanArgust 0:466ee63955df 68 }
DanArgust 0:466ee63955df 69
DanArgust 0:466ee63955df 70 int main()
DanArgust 0:466ee63955df 71 {
DanArgust 4:8f614e0b1cea 72 isSearching = false;
DanArgust 3:936111f70e37 73 counter = 0;
DanArgust 4:8f614e0b1cea 74 while (true) {
DanArgust 4:8f614e0b1cea 75 computeMotorSpeeds(ain0.read(), ain1.read());
DanArgust 4:8f614e0b1cea 76 pwm0 = lMotor;
DanArgust 4:8f614e0b1cea 77 pwm1 = rMotor;
DanArgust 0:466ee63955df 78 wait_ms(10);
DanArgust 0:466ee63955df 79 }
DanArgust 0:466ee63955df 80 }