Diff: main.cpp

Revision:

19:c900c40b270e

Parent:

18:f43f482ddede

Child:

20:f0ca65974329

--- a/main.cpp	Wed Jul 22 21:06:09 2015 +0000
+++ b/main.cpp	Wed Jul 22 22:00:05 2015 +0000
@@ -22,17 +22,71 @@
 /*
  * This function will be called at approximately <SPEED> when the control mode is LINE_FOLLOW_MODE
  */
+ 
+ ///////////////////////////////////////////add line following code
 void line_follow_loop(){
     led4 = 1;
+    
+    //varaibles to
+    float prev_position = 0;
+    float derivative, proportional, integral = 0;
+    int sensors[5];
+    float position = 0;
+    float power;    //speed increase or decrease
+    float speed = MAX;
 
-    // set these variables to control the robot
-    leftspeed = 0.0;
-    rightspeed = 0.0;
+
+    //get sensors
+    pi.sensor_reading(sensors);
+    
+    //line position
+    position = pi.line_position();
+    proportional = position;
+    
+    //derivative
+    derivative = position - prev_position;
+    
+    //integral
+    integral += proportional;
+    
+    //last position
+    prev_position = position;
+    
+    power = (proportional * k_p) + (integral * k_i) + (derivative * k_d);
+    
+    //new speeds
+    rightspeed = speed + power;
+    leftspeed = speed - power;
+    
+   if(rightspeed < MIN)
+        rightspeed = MIN;
+    else if(rightspeed > MAX)
+        rightspeed = MAX;
+    
+    if(leftspeed < MIN)
+        leftspeed = MIN;
+    else if(leftspeed > MAX)
+        leftspeed = MAX;
+    
+    //pi.left_motor(leftspeed);
+   // pi.right_motor(rightspeed);
+    
+    if(sensors[0] < 1200 && sensors[1] < 1200 && sensors[2] < 1200 && sensors[3] < 1200 && sensors[4] < 1200)
+    {
+        pi.stop();
+        //dead end
+        mode = MANUAL_MODE;
+    }
+    else if(sensors[0] > 1800 || sensors[4] > 1800)
+    {
+        //intersection
+        mode = MANUAL_MODE;
+    }
 
     // set mode to MANUAL_MODE when the end is detected
     // mode = MANUAL_MODE
     
-    wait_ms(10);
+    // wait_ms(10);
 
     led4 = 0;
 }