Diff: main_demo.cpp

Revision:

3:fd1380ffb200

Parent:

2:7e86ced5841f

Child:

4:98b3dfdd9ae1

--- a/main_demo.cpp	Wed Oct 31 14:24:21 2018 +0000
+++ b/main_demo.cpp	Wed Oct 31 17:06:04 2018 +0000
@@ -34,11 +34,11 @@
 double  point1y = 200.0;
 double  point2x = 200.0;
 double  point2y = 100.0;
-double  point3x = 350.0;
-double  point3y = 0.0;
-double  point4x = 200.0;
-double  point4y = 0.0;
-int track;
+//double  point3x = 350.0;
+//double  point3y = 0.0;
+//double  point4x = 200.0;
+//double  point4y = 0.0;
+volatile int track = 1;
 const double x0 = 80.0; //zero x position after homing
 const double y0 = 141.0; //zero y position after homing
 volatile double  setpointx = x0;
@@ -80,28 +80,42 @@
 // -------------------------------------------------------------------------
 void determinedemoset()
 {
-
-    if (setpointx < point1x){ 
+    if ((setpointx < point1x) && (track == 1)){ 
         setpointx = setpointx + 0.2;    
     }
-    if(setpointy < point1y){
+    if ((setpointy < point1y) && (track == 1)){
         setpointy = setpointy + 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 = point2x;
-        setpointy = setpointy + (-0.2);          // Van punt 1 naar punt 2 op dezelfde y blijven. 
-        ledr = 0;
+    if ((setpointy >= point1y - 0.3) && (setpointx >= point1x - 0.3) && (setpointy <= point1y + 0.3) && (setpointx <= point1x + 0.3 && track == 1)){
+        //setpointx = point2x;
+        //setpointy = setpointy + (-0.2);          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        ledr = !ledr;                                // Aangeven met een ROOD lampje dat hij op de plaats van bestemming is
         track = 12;
     }
+    
+    if ((setpointy > point2y) && (track == 12)) {
+        setpointx = point2x;
+        //setpointx = setpointx + 0.2;
+        setpointy = setpointy + (0.2);
+    }
+    
+    if ((fabs(setpointx - point2x) <= 0.3) && (fabs(setpointy - point2y) <= 0.3) && (track == 12)) {
+        ledr = !ledr;
+        ledg = !ledg;
+        track = 23;
+        }
+    
+    
+    /**
     if (setpointy > point2y && track == 12){
         setpointx = point2x;
         setpointy = setpointy + (-0.2);
     }
     
     
-    // Van punt 2 naar punt 1. 
+    // Van punt 2 naar punt 3. 
     if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
     {
         //setpointx = setpointx - 0.2; 
@@ -116,7 +130,8 @@
         //setpointy = setpointy;
         track = 23;
     } 
-/*     
+    
+        
     // Van punt 3 naar punt 4. 
     if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
     {
@@ -131,6 +146,7 @@
 
     }
     */
+
 }  
 
 
@@ -236,7 +252,16 @@
     motor2_pwm.period_us(60);
     Demo.attach(&rundemo, 0.005f); 
     
+    pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
+    
     while (true) {
-
+        pc.printf("Setpointx: %0.2f en Setpointy: %02f \r\n", setpointx,setpointy);
+        if (track == 1) {
+            pc.printf("Gaat naar positie 1\r\n");
+        }
+        else if (track == 12) {
+            pc.printf("Gaat naar positie 2\r\n");
+        }
+        wait(0.5f);
     }
 }    
\ No newline at end of file