Diff: main_demo.cpp

Revision:

1:8cec72aa7728

Parent:

0:755bc7c0f555

Child:

2:785737b1cd38

--- a/main_demo.cpp	Wed Oct 31 10:44:10 2018 +0000
+++ b/main_demo.cpp	Wed Oct 31 11:01:10 2018 +0000
@@ -35,7 +35,6 @@
 double  point3y = 0.0;
 double  point4x = 200.0;
 double  point4y = 0.0;
-int track;
 const double x0 = 80.0; //zero x position after homing
 const double y0 = 141.0; //zero y position after homing
 volatile double  setpointx = x0;
@@ -75,92 +74,41 @@
 // -------------------------------------------------------------------------
 // -------------- Determine Setpoints --------------------------------------
 // -------------------------------------------------------------------------
-double determinedemosetx(double setpointx, double setpointy)
+void determinedemosetpoints( double &setpointx, double &setpointy)
 {
-
-
-    
+    int track = 1;
+    if( fabs(setpointx - point1x) <= 0.2 && fabs(setpointy - point1y) <= 0.2){
+        track = 2;
+        }
+    if( fabs(setpointx - point2x) <= 0.2 && fabs(setpointy - point2y) <= 0.2){
+        track = 3;
+        }
+    if( fabs(setpointx - point3x) <= 0.2 && fabs(setpointy - point3y) <= 0.2){
+        track = 4;
+        }      
     
- 
-        /*
-    // Van punt 3 naar punt 4. 
-    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
-    {
-        setpointx = setpointx - 0.1;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
-        track = 34;
-    }
-    if (setpointy > point3y && track == 34)
-    {
-        setpointx = setpointx - 0.1;
-    }
-        
-    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3)
-    {
-        setpointx = 80.0;
-    }
-    */
-      return setpointx;
-}  
-
-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){
+    switch(track){
+        case 1:
         setpointx = setpointx + 0.1;
-        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))
-    {
-        setpointx = point3x; 
-        setpointy = setpointy - 0.2;
-        track = 23;
-    }
-    if (setpointy > point3y && track == 23)
-    {
-        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        setpointy = setpointy + 0.2;
+        break;
+        
+        case 2:
+        setpointx = setpointx + 0.2; 
+        setpointy = setpointy;
+        break;    
+        
+        case 3:
+        setpointx = setpointx;
         setpointy = setpointy - 0.2;
-        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)
-    {
+        break;
+        
+        case 4:
+        setpointx = setpointx - 0.2;
         setpointy = setpointy;
-        track = 34;
-    }
-    if (setpointy > point3y && track == 34)
-    {
-        setpointy = setpointy;
-    }     
-    
-    
-    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3)
-    {
-        setpointy = 141.0;
-    }
-    */ 
-    return setpointy;
-    
+        break;
+        }    
 }
-
 // -----------------------------------------------------------------    
 // --------------------------- PI controllers ----------------------
 // -----------------------------------------------------------------
@@ -227,8 +175,6 @@
 // -----------------------------------------------
 void motoraansturing()
 {
-    setpointx = determinedemosetx(setpointx, setpointy);
-    setpointy = determinedemosety(setpointx, setpointy);
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);