Diff: LineFollower.cpp

Revision:

1:c319e24af8df

Parent:

0:2623af9e8ef3

Child:

2:7a4179249fa4

--- a/LineFollower.cpp	Tue Feb 18 04:14:48 2014 +0000
+++ b/LineFollower.cpp	Fri Feb 21 04:40:34 2014 +0000
@@ -18,6 +18,14 @@
                 _ir1(ir1), _ir2(ir2), _ir3(ir3), _ir4(ir4), _ir5(ir5), _ir6(ir6),
                 _ir7(ir7), _ir8(ir8){
                     
+        array[0] = _ir1;
+        array[1] = _ir2;
+        array[2] = _ir3;
+        array[3] = _ir4;
+        array[4] = _ir5;
+        array[5] = _ir6;
+        array[6] = _ir7;
+        array[7] = _ir8;
     }
                 
                 
@@ -27,18 +35,93 @@
     */                    
     uint8_t LineFollower::read(){
         uint8_t binary = 0;
+        int multi = 1;
+        for(int i=0; i<=8; i++){
+               binary += array[i]*multi;
+               multi = multi*2;
+        }
+        return binary;
+    }
+    
+    /** Follow a line
+    * 
+    * @param    l left drive motor
+    * @param    r  right drive motor
+    */                    
+    void followLine(Motor l, Motor r){
+           int count = 0;
+           for(int i = 0; i<8; i++){
+                count += array[i];  
+            }
+            
+            switch(count){
+                
+                case 1: if(this->read() == 0b10000000){
+                            l = -(0.75 * MAXSPEED);
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b00000001){
+                            l = MAXSPEED;
+                            r = -(0.75 * MAXSPEED);
+                        }
+                        break;
+                        
+                case 2: if(this->read() == 0b00011000){
+                            l = MAXSPEED;
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b11000000){
+                            l = -(0.5 * MAXSPEED);
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b00000011){
+                            l = MAXSPEED;
+                            r = -(0.5 * MAXSPEED);
+                        }
+                        else if(this->read() == 0b01100000){
+                            l = 0;
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b00000110){
+                            l = MAXSPEED;
+                            r = 0;
+                        }
+                        else if(this->read() == 0b00110000){
+                            l = 0.5*MAXSPEED;
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b00001100){
+                            l = MAXSPEED;
+                            r = 0.5*MAXSPEED;
+                        }
+                        break;
+                        
+                case 3: if(this->read() == 0b11100000){
+                            l = -(0.25*MAXSPEED);
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b00000111){
+                            l = MAXSPEED;
+                            r = -(0.25*MAXSPEED);
+                        }
+                        else if(this->read() == 0b01110000){
+                            l = 0.25*MAXSPEED;
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b00001110){
+                            l = MAXSPEED;
+                            r = .25*MAXSPEED;
+                        }
+                        else if(this->read() == 0b00111000){
+                            l = 0.5*MAXSPEED;
+                            r = MAXSPEED;
+                        }
+                        else if(this->read() == 0b00011100){
+                            l = MAXSPEED;
+                            r = 0.5*MAXSPEED;
+                        }
+                        break
+              default:  break;
+            }
         
-        array[0] = _ir1;
-        array[1] = _ir2;
-        array[2] = _ir3;
-        array[3] = _ir4;
-        array[4] = _ir5;
-        array[5] = _ir6;
-        array[6] = _ir7;
-        array[7] = _ir8;
-        
-        for(int i=0; i<=8; i++){
-               binary += array[i]
-        }
-    
     }
\ No newline at end of file