Diff: main.cpp

Revision:

19:e69ff16f35aa

Parent:

18:174b4ff351b5

Child:

20:1d98a6f51dee

--- a/main.cpp	Tue May 08 15:40:39 2018 +0000
+++ b/main.cpp	Tue May 08 16:41:37 2018 +0000
@@ -98,8 +98,9 @@
 float steps_by_rotation = motor_steps_by_rotation/step_mode;
 
 int saved = 0;
-const int maxPoints = 4;
+const int maxPoints = 10;
 float points[3][maxPoints];
+int pontos_restantes = 0;
 
 int startSaved = 0;
 
@@ -165,14 +166,14 @@
                     x_dir = x_plus;
                 }
                 activeX = 1; 
-                totalX+=1;    
+                //totalX+=1;    
             }
             else if(valX < 0.3){
                 if(x_dir != x_minus){
                     x_dir = x_minus;   
                 }
                 activeX = 1;   
-                totalX-=1;  
+                //totalX-=1;  
             }
             else{
                 activeX = 0;
@@ -185,14 +186,14 @@
                     y_dir = y_plus;   
                 }
                 activeY = 1;  
-                totalY+=1;   
+                //totalY+=1;   
             }
             else if(valY < 0.3){
                 if(y_dir != y_minus){
                     y_dir = y_minus;    
                 }
                 activeY = 1; 
-                totalY-=1;   
+                //totalY-=1;   
             }
             else{
                 activeY = 0;
@@ -205,14 +206,14 @@
                     z_dir = z_minus;    
                 }
                 activeZ = 1;  
-                totalZ-=1;  
+                //totalZ-=1;  
             }
             else if(!valZDwn && valZUp){
                 if(z_dir != z_plus){
                     z_dir = z_plus;   
                 }
                 activeZ = 1; 
-                totalZ+=1;   
+                //totalZ+=1;   
             }
             else {
                 activeZ = 0;    
@@ -237,18 +238,48 @@
             }
         }
         
-        printf("Starting saved points\n\r");
+        printf("\nStarting saved points\n\r");
+        
+        x_distance_to_run = points[0][0] - points[0][saved-1];
+        y_distance_to_run = points[1][0] - points[1][saved-1];
+        z_distance_to_run = points[2][0] - points[2][saved-1]; 
+        
+        printf("\nGoing to first point\n\r");
         
-        for(int i = 0; i < saved; i++){
-            if(i == 0){
-                x_distance_to_run = points[0][i]-0.0;
-                y_distance_to_run = points[1][i]-0.0;
-                z_distance_to_run = points[2][i]-0.0;
-            } else {
-                x_distance_to_run = points[0][i]-points[0][i-1];
-                y_distance_to_run = points[1][i]-points[1][i-1];
-                z_distance_to_run = points[2][i]-points[2][i-1];
-            }
+        if(x_distance_to_run < 0){
+                x_dir = x_minus;
+                x_distance_to_run = -1.0*x_distance_to_run;
+            } else { x_dir = x_plus; }
+            
+            if(y_distance_to_run < 0){
+                y_dir = y_minus;
+                y_distance_to_run = -1.0*y_distance_to_run;
+            } else { y_dir = y_plus; }
+            
+            if(z_distance_to_run < 0){
+                z_dir = z_minus;
+                z_distance_to_run = -1.0*z_distance_to_run;
+            } else { z_dir = z_plus; }
+            
+        x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch); 
+        y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch); 
+        z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch);
+        
+        printf("deltaX: %.2f        deltaY: %.2f            deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
+        
+        move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0);
+        move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0);
+        move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run);
+        
+        wait(2);
+        
+        printf("\nSaved points\n\r");
+        
+        for(int i = 1; i < saved; i++){
+            x_distance_to_run = points[0][i]-points[0][i-1];
+            y_distance_to_run = points[1][i]-points[1][i-1];
+            z_distance_to_run = points[2][i]-points[2][i-1];
+            
             printf("deltaX: %.2f        deltaY: %.2f            deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
             
             if(x_distance_to_run < 0){
@@ -279,9 +310,7 @@
     }
     
     else {
-        printf("Routine Selected\n\r");
-        
-                
+        printf("Routine Selected\n\r");   
         
         /*
         z_steps_to_run = distance_to_steps(20.0, zPitch); 
@@ -299,7 +328,12 @@
         x_steps_to_run = distance_to_steps(50, xPitch);
         move(ppsMax, x_minus, y_plus, z_plus, x_steps_to_run, 0, 0);   
         
-        move(ppsMax, x_plus, x_plus, z_plus, 0, 0, z_steps_to_run);   */  
+        move(ppsMax, x_plus, x_plus, z_plus, 0, 0, z_steps_to_run);   */ 
+        
+        
+            move(ppsMax, x_plus, y_plus, z_plus, 400, 0, 0); 
+        
+        
         printf("Done\n\r");
     }  
 }  
@@ -506,7 +540,9 @@
         saved+=1;
     }
     // save code
-    printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n\n", X, Y, Z);     
+    printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
+    pontos_restantes = maxPoints-saved;
+    printf("Pontos restantes: %d\n\n\r", pontos_restantes);     
 }
 
 void startSavedPoints(void){