LineFollower.cpp

00001 #include "LineFollower.h"
00002 #include "mbed.h"
00003 #include <stdint.h>
00004 
00005     /** Create a Line Follower interface for an IR Sensor Array
00006     *
00007     * @param ir1  IR Sensor 1
00008     * @param ir2  IR Sensor 2
00009     * @param ir3  IR Sensor 3
00010     * @param ir4  IR Sensor 4
00011     * @param ir5  IR Sensor 5
00012     * @param ir6  IR Sensor 6
00013     * @param ir7  IR Sensor 7
00014     * @param ir8  IR Sensor 8
00015     */   
00016     LineFollower::Linefollower(DigitalIn ir1, DigitalIn ir2, DigitalIn ir3, DigitalIn ir4,
00017                 DigitalIn ir5, DigitalIn ir6, DigitalIn ir7, DigitalIn ir8):
00018                 _ir1(ir1), _ir2(ir2), _ir3(ir3), _ir4(ir4), _ir5(ir5), _ir6(ir6),
00019                 _ir7(ir7), _ir8(ir8){
00020                     
00021         array[0] = _ir1;
00022         array[1] = _ir2;
00023         array[2] = _ir3;
00024         array[3] = _ir4;
00025         array[4] = _ir5;
00026         array[5] = _ir6;
00027         array[6] = _ir7;
00028         array[7] = _ir8;
00029     }
00030                 
00031                 
00032     /** Read the value of a LineFollower object
00033     * 
00034     * @return The value of the Sensor
00035     */                    
00036     uint8_t LineFollower::read(){
00037         uint8_t binary = 0;
00038         int multi = 1;
00039         for(int i=0; i<8; i++){
00040                binary += array[i]*multi;
00041                multi = multi*2;
00042         }
00043         return binary;
00044     }
00045     
00046     /** Follow a line
00047     * 
00048     * @param    l left drive motor
00049     * @param    r  right drive motor
00050     */                    
00051     void followLine(Motor l, Motor r){
00052            int count = 0;
00053            for(int i = 0; i<8; i++){
00054                 count += array[i];  
00055             }
00056             
00057             switch(count){
00058                 
00059                 case 1: if(this->read() == 0b10000000){
00060                             l.speed(-(0.75 * MAXSPEED));
00061                             r.speed(MAXSPEED);
00062                         }
00063                         else if(this->read() == 0b00000001){
00064                             l.speed(MAXSPEED);
00065                             r.speed(-(0.75 * MAXSPEED));
00066                         }
00067                         break;
00068                         
00069                 case 2: if(this->read() == 0b00011000){
00070                             l.speed(MAXSPEED);
00071                             r.speed(MAXSPEED);
00072                         }
00073                         else if(this->read() == 0b11000000){
00074                             l.speed(-(0.5 * MAXSPEED));
00075                             r.speed(MAXSPEED);
00076                         }
00077                         else if(this->read() == 0b00000011){
00078                             l.speed(MAXSPEED);
00079                             r.speed(-(0.5 * MAXSPEED));
00080                         }
00081                         else if(this->read() == 0b01100000){
00082                             l.speed(0);
00083                             r.speed(MAXSPEED);
00084                         }
00085                         else if(this->read() == 0b00000110){
00086                             l.speed(MAXSPEED);
00087                             r.speed(0);
00088                         }
00089                         else if(this->read() == 0b00110000){
00090                             l.speed(0.5*MAXSPEED);
00091                             r.speed(MAXSPEED);
00092                         }
00093                         else if(this->read() == 0b00001100){
00094                             l.speed(MAXSPEED);
00095                             r.speed(0.5*MAXSPEED);
00096                         }
00097                         break;
00098                         
00099                 case 3: if(this->read() == 0b11100000){
00100                             l.speed(-(0.25*MAXSPEED));
00101                             r.speed(MAXSPEED);
00102                         }
00103                         else if(this->read() == 0b00000111){
00104                             l.speed(MAXSPEED);
00105                             r.speed(-(0.25*MAXSPEED));
00106                         }
00107                         else if(this->read() == 0b01110000){
00108                             l.speed(0.25*MAXSPEED);
00109                             r.speed(MAXSPEED);
00110                         }
00111                         else if(this->read() == 0b00001110){
00112                             l.speed(MAXSPEED);
00113                             r.speed(0.25*MAXSPEED);
00114                         }
00115                         else if(this->read() == 0b00111000){
00116                             l.speed(0.5*MAXSPEED);
00117                             r.speed(MAXSPEED);
00118                         }
00119                         else if(this->read() == 0b00011100){
00120                             l.speed(MAXSPEED);
00121                             r.speed(0.5*MAXSPEED);
00122                         }
00123                         break
00124               default:  break;
00125             }
00126         
00127     }