Diff: TechMoveDXC.cpp

Revision:

3:381aaa338447

Parent:

2:a12d19da681a

Child:

4:bd3bfbaa33bd

--- a/TechMoveDXC.cpp	Fri Apr 17 20:52:22 2015 +0000
+++ b/TechMoveDXC.cpp	Sat Apr 18 13:02:01 2015 +0000
@@ -18,8 +18,14 @@
 PwmOut ML (dp18);   //PWM-signaal voor de linker motor.
 PwmOut MR (dp2);   //PWM-signaal voor de rechter motor.
 
+Serial pc(USBTX, USBRX); //Gebruik consol.
+
 int main()
 {
+    float f1 = SLV.read();
+    float f2 = SRV.read();
+    float f3 = SLA.read();
+    float f4 = SRA.read();
     int n = 0; //Variabel voor de straight line stand.
     MLL = false;
     MLR = false;
@@ -29,31 +35,86 @@
     MR = 0;
     ML = 0;
     while (K1)  //Sumo stand.
+    {
+        n = 0;
+        LED = true;
+        MLL = true;
+        MLR = false;
+        MRL = false;
+        MRR = true;
+        MR = 0.8;
+        ML = 0.8;
+        if (f1 > 0);    //Waarde sensor links voor. (AANPASSEN)
         {
-            n = 0;
+            MLL = true;
+            MLR = false;
+            MRL = true;
+            MRR = false;
+            MR = 0.8;
+            ML = 0.5;
+            wait (1);   //Duur van de bocht. (AANPASSEN)
         }
-    while (K2)  //Straight line stand.
+        if (f2 > 0);    //Waarde sensor rechts voor. (AANPASSEN)
+        {
+            MLL = false;
+            MLR = true;
+            MRL = false;
+            MRR = true;
+            MR = 0.5;
+            ML = 0.8;
+            wait (1);   //Duur van de bocht. (AANPASSEN)
+        }
+        if (f3 > 0);    //Waarde sensor links achter. (AANPASSEN)
+        {
+            MLL = false;
+            MLR = false;
+            MRL = false;
+            MRR = false;
+            MR = 0.9;
+            ML = 1;
+            wait (0.5);   //Duur van de bocht. (AANPASSEN)
+        }
+        if (f4 > 0);    //Waarde sensor rechts achter. (AANPASSEN)
         {
-            LED = !LED;
-            n += 1;
-            if (n == 6)
-            {
-                LED = true;
-                MLR = true;
-                MRR = true;
-                MR = 1.0;
-                ML = 1.0;
-                wait (5.0);   //Tijd van het rijden.
-                MLR = false;
-                MRR = false;
-                MR = 0;
-                ML = 0;
-            }
+            MLL = true;
+            MLR = false;
+            MRL = false;
+            MRR = true;
+            MR = 1;
+            ML = 0.9;
+            wait (0.5);   //Duur van de bocht. (AANPASSEN)
+        }
+    }
+    while (K2)  //Straight line stand.
+    {
+        LED = !LED;
+        n += 1;
+        if (n == 6)
+        {
+            LED = true;
+            MLR = true;
+            MRR = true;
+            MR = 1.0;
+            ML = 1.0;
+            wait (0);   //Tijd van het rijden. (AANPASSEN)
+            MLR = false;
+            MRR = false;
+            MR = 0;
+            ML = 0;
+        }
+        wait (0.5);      
+    }
+    while (K3)  //Service stand.
+    {
+        n = 0;
+        LED = !LED;
+        wait (2);
+        LED = !LED;
+        pc.printf("Waarde sensor links voor: %i $$$$$\n", f1);    //verstuur de waarde van sensor links voor naar het consol
+        pc.printf("Waarde sensor rechts voor: %i $$$$$\n", f2);    //verstuur de waarde van sensor rechts voor naar het consol
+        pc.printf("Waarde sensor links achter: %i $$$$$\n", f3);    //verstuur de waarde van sensor links achter naar het consol
+        pc.printf("Waarde sensor rechts achter: %i $$$$$\n", f4);    //verstuur de waarde van sensor rechts achter naar het consol
         wait (0.5);
-              
-        }
-    while (K3)  //Service stand.
-        {
-            n = 0;
-        }
+        
+    }
 }
\ No newline at end of file