Diff: main.cpp

Revision:

8:2cca24684c6d

Parent:

7:218e861ea777

--- a/main.cpp	Thu May 12 11:34:15 2011 +0000
+++ b/main.cpp	Wed Jan 11 10:13:36 2017 +0000
@@ -1,42 +1,69 @@
 #include "mbed.h"
-#include "m3pi.h"
+#include "m3pi_ng.h"
+#include "LED.h"
+#include "Umwandeln.h"
 
 m3pi m3pi;
+// Parameters that affect the performance
+    float speed = 0.7;
+    float correction = 0;   
+    float threshold = 0.50;
+    float posalt = 0, posneu=0;
+    float k1=0.7, k2=0.8;
+    float im = 0.275;
+    LED B;
+    Ticker goback;
+    Timer t;
+    int sensors[5]={};
+    int a=0;
+    
+void pdreg()
+
+    {    
+         posneu = m3pi.line_position();  // -1.0 is far left, 1.0 is far right, 0.0 in the middle
+         correction = k1*posneu+k2*(posneu-posalt);
+         if (correction >0)
+           { 
+            m3pi.right_motor(speed+(im*correction));
+            m3pi.left_motor (speed-correction);
+            }
+        else
+            {       
+            m3pi.right_motor(speed+correction);
+            m3pi.left_motor(speed-(im*correction));
+            }
+            
+         posalt = posneu;
+        }
+    
+void setup ()
+    {     
+       //Displayausgabe
+        m3pi.locate(0,0);
+        m3pi.printf("PARTYROB");
+        m3pi.locate(0,1);
+        m3pi.printf("FLOWFLOW");
+        wait(2.0);
+        m3pi.sensor_auto_calibrate();
+        }
 
 int main() {
 
-    // Parameters that affect the performance
-    float speed = 0.2;
-    float correction = 0.1;   
-    float threshold = 0.5;
-
-    m3pi.locate(0,1);
-    m3pi.printf("Line Flw");
+setup ();
 
-    wait(2.0);
-    
-    m3pi.sensor_auto_calibrate();
-    
-    while (1) {
-
-        // -1.0 is far left, 1.0 is far right, 0.0 in the middle
-        float position_of_line = m3pi.line_position();
+goback.attach(&pdreg,0.05);  
 
-        // Line is more than the threshold to the right, slow the left motor
-        if (position_of_line > threshold) {
-            m3pi.right_motor(speed);
-            m3pi.left_motor(speed-correction);
-        }
-
-        // Line is more than 50% to the left, slow the right motor
-        else if (position_of_line < -threshold) {
-            m3pi.left_motor(speed);
-            m3pi.right_motor(speed-correction);
-        }
-
-        // Line is in the middle
-        else {
-            m3pi.forward(speed);
-        }
-    }
-}
\ No newline at end of file
+    while(1)
+        {
+        m3pi.calibrated_sensor(sensors);
+        if ((sensors[0]>400) && (sensors[4]>400))
+        {
+        goback.detach();
+        B.Blau_Blinken(2,0.1);
+        m3pi.stop();
+        wait(1);
+        } 
+        goback.attach(&pdreg,0.005); 
+        } 
+        
+}