Diff: main.cpp

Revision:

37:c31268270bb2

Parent:

35:09f92a88c0a7

--- a/main.cpp	Tue Jul 12 04:01:41 2016 +0000
+++ b/main.cpp	Wed Jul 13 19:48:12 2016 +0000
@@ -20,9 +20,11 @@
     // line_position, speed and other
     // variables in this scope.
     ////////////////////////////////////
-
+    double derivative = line_position - previous_position;
+    double delta = k_p * line_position + k_d * derivative + k_i * integral;
     
-
+    leftspeed = speed + delta;
+    rightspeed = speed - delta;
     // set motor speeds here
     // leftspeed = ???;
     // rightspeed = ???;
@@ -42,7 +44,7 @@
         rightspeed = MIN;
     }
     
-    // Code to check if we need to stop
+    // Code to check if we need to stop (uncomment this if you want)
     if(sensors[0] < 1000 && sensors[1] < 1000 && sensors[2] < 1000 && sensors[3] < 1000 && sensors[4] < 1000){
         //dead end
         leftspeed = 0.0;
@@ -225,6 +227,4 @@
         }
 
     }
-
-    return 0;
 }