Diff: LineFollower.cpp

Revision:

9:039a74519b89

Parent:

8:12751953ad8e

Child:

10:a9233fa6240d

--- a/LineFollower.cpp	Mon Mar 10 00:03:42 2014 +0000
+++ b/LineFollower.cpp	Mon Mar 10 00:54:55 2014 +0000
@@ -28,6 +28,37 @@
         return array;
     }
     
+    float LineFollower::analogFollow(Motor l, Motor r){
+        float sum = 0;
+        int count = 0;
+        for(int i = 0; i < 8; i++)
+            if(!(array & (1<<i))){
+                sum += (i+1);
+                count++;
+            }
+        if(count == 0) return 0;
+        float center = sum/count;
+        
+        float sp_adj_mult = 0.9; //Change this to change how drastic your adjustments are. 0-1
+        float speed_adjust = sp_adj_mult*(center-4.5)/(3.5)*MAXSPEED;
+            //Also, can change MAXSPEED if the thing is too fast overall. 0-1
+        
+        if(center > 4.5){
+            l.speed(MAXSPEED - speed_adjust);
+            r.speed(MAXSPEED);
+        }
+        else if(center < 4.5){
+            l.speed(MAXSPEED);
+            r.speed(MAXSPEED - abs(speed_adjust));
+        }
+        else{
+            l.speed(MAXSPEED);
+            r.speed(MAXSPEED);
+        }
+        
+        return center;
+    }
+    
     /** Follow a line
     * 
     * @param    l left drive motor
@@ -109,5 +140,8 @@
               default:  break;
               
             }
+            
+            
+            
             return count;
     }
\ No newline at end of file