Diff: main_demo.cpp

Revision:

1:7afdfab34bf7

Parent:

0:755bc7c0f555

Child:

2:7e86ced5841f

--- a/main_demo.cpp	Wed Oct 31 10:44:10 2018 +0000
+++ b/main_demo.cpp	Wed Oct 31 12:51:31 2018 +0000
@@ -78,9 +78,30 @@
 double determinedemosetx(double setpointx, double setpointy)
 {
 
-
+    if (setpointx < point1x){ 
+        setpointx = setpointx + 0.1;    
+    }
     
+    // Van punt 1 naar punt 2. 
+    if (setpointy >= point1y - 0.3 && setpointx >= point1x - 0.3 && setpointy <= point1y + 0.3 && setpointx <= point1x + 0.3){
+        setpointx = setpointx + 0.1;
+        track = 12;
+    }
+    if (setpointx < point2x && track == 12){
+        setpointx = setpointx + 0.2;
+    }
     
+    // Van punt 2 naar punt 1. 
+    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
+    {
+        setpointx = setpointx - 0.2; 
+        track = 23;
+    }
+    if (setpointy > point3y && track == 23)
+    {
+        setpointx = setpointx - 0.2;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        track = 23;
+    } 
  
         /*
     // Van punt 3 naar punt 4. 
@@ -102,39 +123,31 @@
       return setpointx;
 }  
 
-double determinedemosety(double &setpointx, double &setpointy)
+double determinedemosety(double setpointx, double setpointy)
 {
     // Van reference positie naar punt 1.
     if(setpointy < point1y){
         setpointy = setpointy + 0.2;
     } 
 
-    if (setpointx < point1x){ 
-        setpointx = setpointx + 0.1;    
-    }
-    
     // Van punt 1 naar punt 2. 
-    if (setpointy >= point1y - 0.3 && setpointx >= point1x - 0.3 && setpointy <= point1y + 0.3 && setpointx <= point1x + 0.3){
-        setpointx = setpointx + 0.1;
+    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3){
         setpointy = point2y;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
         track = 12;
     }
     if (setpointx < point2x && track == 12){
-        setpointx = setpointx + 0.2;
         setpointy = point2y;
     }
     
-    // Van punt 2 naar punt 3. 
-    if (setpointx >= (point2x-0.2))
+    // Van punt 2 naar punt 1. 
+    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
     {
-        setpointx = point3x; 
-        setpointy = setpointy - 0.2;
+        setpointy = setpointy;
         track = 23;
     }
     if (setpointy > point3y && track == 23)
     {
-        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
-        setpointy = setpointy - 0.2;
+        setpointy = setpointy;
         track = 23;
     }