Diff: main_demo.cpp

Revision:

2:785737b1cd38

Parent:

1:8cec72aa7728

--- a/main_demo.cpp	Wed Oct 31 11:01:10 2018 +0000
+++ b/main_demo.cpp	Wed Oct 31 12:32:16 2018 +0000
@@ -37,8 +37,8 @@
 double  point4y = 0.0;
 const double x0 = 80.0; //zero x position after homing
 const double y0 = 141.0; //zero y position after homing
-volatile double  setpointx = x0;
-volatile double  setpointy = y0;
+double  setpointx = x0;
+double  setpointy = y0;
 volatile double  U1;
 volatile double  U2;
 
@@ -79,18 +79,21 @@
     int track = 1;
     if( fabs(setpointx - point1x) <= 0.2 && fabs(setpointy - point1y) <= 0.2){
         track = 2;
+        pc.printf("We gaan nu naar setpoint %i",track);
         }
     if( fabs(setpointx - point2x) <= 0.2 && fabs(setpointy - point2y) <= 0.2){
         track = 3;
+        pc.printf("We gaan nu naar setpoint %i", track);
         }
     if( fabs(setpointx - point3x) <= 0.2 && fabs(setpointy - point3y) <= 0.2){
         track = 4;
+        pc.printf("We gaan nu naar setpoint %i",track);
         }      
     
     switch(track){
         case 1:
-        setpointx = setpointx + 0.1;
-        setpointy = setpointy + 0.2;
+        setpointx = setpointx + (point1x - x0)/150;
+        setpointy = setpointy + (point1y - y0)/150;
         break;
         
         case 2:
@@ -175,6 +178,8 @@
 // -----------------------------------------------
 void motoraansturing()
 {
+    determinedemosetpoints(setpointx, setpointy);
+    
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);