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 void computeMotorSpeeds(float a,float b){
00025     float inputs[2] = {a,b};
00026     const float MAXSPEED = 1.0;
00027     lMotor  = 0.0;
00028     rMotor  = 0.0;
00029     newLine = 0.0;
00030     float lineToSpeedFactor = MAXSPEED / 0.3;
00031     for (int i = 0; i < 2; ++i) {
00032         newLine += inputs[i] * (i + 1);
00033     }
00034     float sum = 0.0;
00035     for (int i = 0; i < 2; ++i)
00036         sum += inputs[i];
00037     newLine = newLine / sum;
00038     if ((a < 0.35) && (b < 0.35)) {
00039         if (oldLine > 1.5)
00040             newLine = 2.0;
00041         else
00042             newLine = 1.0;
00043         oldLine = newLine;
00044         isSearching = true;
00045     }
00046     if (isSearching) {
00047         if ( oldLine > 1.5)
00048             newLine = 2.0;
00049         else
00050             newLine = 1.0;
00051         if (abs(a - b) < 0.2)
00052             newLine = oldLine;
00053         else
00054             isSearching = false;
00055         counter++;
00056         if (counter > 600) {
00057             newLine = 3.0 - oldLine;
00058             counter = 0;
00059         }
00060     }
00061     oldLine = newLine;
00062     lMotor =      newLine * lineToSpeedFactor - 4.0;
00063     rMotor = 2.0-(newLine * lineToSpeedFactor - 4.0);
00064     if (lMotor > MAXSPEED)
00065         lMotor = MAXSPEED;
00066     if (rMotor > MAXSPEED)
00067         rMotor = MAXSPEED;
00068 }
00069 
00070 int main()
00071 {
00072     isSearching = false;
00073     counter = 0;
00074     while (true) {
00075         computeMotorSpeeds(ain0.read(), ain1.read());
00076         pwm0 = lMotor;
00077         pwm1 = rMotor;
00078         wait_ms(10);
00079     }
00080 }