main.cpp

00001 /*********************************************************
00002 *RenBuggyLineFollower                                    *
00003 *Author: Dan Argust                                      *
00004 *                                                        *  
00005 *This program demonstates the use of two sensors to      *
00006 *detect and follow a thin black line.                    *
00007 *********************************************************/
00008 
00009 #include "mbed.h"
00010 
00011 AnalogIn ain0(p15);
00012 AnalogIn ain1(p16);
00013 
00014 PwmOut pwm0(p25);
00015 PwmOut pwm1(p10);
00016 
00017 float lMotor;
00018 float rMotor;
00019 float newLine;
00020 float oldLine;
00021 bool  isSearching;
00022 int   counter;
00023 
00024 /* This function is called every 10 miliseconds
00025 ** Using the two inputs from the sensor
00026 **
00027 ** It calculates the where a mean value for the track would be
00028 ** between 1 and 2
00029 ** if the track is at 1.5 the buggy will travel straight
00030 ** if the track is at 1.0 the buggy will turn left
00031 ** if the track is at 2.0 the buggy will turn right
00032 **
00033 ** if both inputs are less than 0.35
00034 ** the buggy will assume it has lost the line and will begin "seaching"
00035 ** while searching the buggy will turn in one direction until it finds a line
00036 ** if it fails to find a line in 600 attempts it will change direction
00037 ** this repeats
00038 ** the theory and algorithm can be found here: 
00039 ** https://www.ikalogic.com/line-tracking-sensors-and-algorithms/
00040 */
00041 void computeMotorSpeeds(float a,float b){
00042     float inputs[2] = {a,b};
00043     const float MAXSPEED = 1.0; // Motor max speed
00044     lMotor  = 0.0;
00045     rMotor  = 0.0;
00046     newLine = 0.0;
00047     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
00048     
00049     
00050     // CALCULATING THE AVERAGE POSITION OF THE TRACK. LEFT = 1.0 / RIGHT = 2.0
00051     for (int i = 0; i < 2; ++i) {
00052         newLine += inputs[i] * (i + 1);
00053     }
00054     float sum = 0.0;
00055     for (int i = 0; i < 2; ++i)
00056         sum += inputs[i];
00057     newLine = newLine / sum; // newLine is now set somewhere between 1.0 and 2.0
00058     // CALCULATING THE AVERAGE POSITION OF THE TRACK. LEFT = 1.0 / RIGHT = 2.0
00059     
00060     
00061     if ((a < 0.35) && (b < 0.35)) { // if both the sensors are low then the line has been lost
00062         if (oldLine > 1.5)
00063             newLine = 2.0; // if the buggy was turning right before losing the line keep doing so
00064         else
00065             newLine = 1.0; // if the buggy was turning left before losing the line keep doing so
00066         oldLine = newLine;
00067         isSearching = true; // put the buggy in 'searching' mode
00068     }
00069     
00070     if (isSearching) {
00071         if ( oldLine > 1.5)
00072             newLine = 2.0;
00073         else
00074             newLine = 1.0;
00075         if (abs(a - b) < 0.2) // if a contrast is not detected (a track) then carry on
00076             newLine = oldLine;
00077         else                  // if a contrast is detected then turn off searching mode
00078             isSearching = false;
00079         counter++;
00080         if (counter > 600) {  // every 600 iteratons if searching change direction
00081             newLine = 3.0 - oldLine; // if the line is 1.0 then 3 - 1 = 2.0
00082             counter = 0;             // if the line is 2.0 then 3 - 2 = 1.0
00083         }
00084     }
00085     oldLine = newLine;
00086     lMotor =      newLine * lineToSpeedFactor - 4.0; // see below
00087     rMotor = 2.0-(newLine * lineToSpeedFactor - 4.0);// see below
00088     if (lMotor > MAXSPEED)
00089         lMotor = MAXSPEED;
00090     if (rMotor > MAXSPEED)
00091         rMotor = MAXSPEED;
00092 }
00093 /* The motor speeds are calculated by taking the average sensor reading that can be anywhere from 1 to 2
00094 ** if the sensor reading is 1.3:
00095 ** lMotor = 1.3 * 3.333 - 4 = 0.3333
00096 ** Rmotor = 2 - (1.3 * 3.333 - 4) = 1.6667 which gets capped to 1
00097 **
00098 ** if the sensor reading is 1.2:
00099 ** lMotor = 1.2 * 3.333 - 4 = 0
00100 ** rMotor = 2 - (1.2 * 3.333 - 4) = 2 which gets capped to 1
00101 ** so the left motor is stopped if the average sensor value is less than 0.2
00102 ** this is also true for the right motor if the average sensor value is greater than 1.8
00103 */
00104 
00105 int main()
00106 {
00107     isSearching = false;
00108     counter = 0;
00109     while (true) {
00110         computeMotorSpeeds(ain0.read(), ain1.read()); // set the motor speed variables
00111         pwm0 = lMotor; // output the variables to the pwm outs
00112         pwm1 = rMotor;
00113         wait_ms(10); // wait 10 miliseconds and start again
00114     }
00115 }