Diff: main.cpp

Revision:

18:174b4ff351b5

Parent:

17:55e6270adab5

Child:

19:e69ff16f35aa

diff -r 55e6270adab5 -r 174b4ff351b5 main.cpp
--- a/main.cpp	Tue May 08 13:06:10 2018 +0000
+++ b/main.cpp	Tue May 08 15:40:39 2018 +0000
@@ -27,6 +27,9 @@
 DigitalIn zUp(D15);
 DigitalIn zDwn(D14);
 
+InterruptIn saveBtn(D13);
+InterruptIn startBtn(D12);
+
 DigitalIn botao(D11);
 
 // variables definition
@@ -74,6 +77,10 @@
 int y_steps_to_run;
 int z_steps_to_run;
 
+float x_distance_to_run;
+float y_distance_to_run;
+float z_distance_to_run;
+
 float X = 0;
 float Y = 0;
 float Z = 0;
@@ -90,6 +97,12 @@
 float step_mode = 0.5;
 float steps_by_rotation = motor_steps_by_rotation/step_mode;
 
+int saved = 0;
+const int maxPoints = 4;
+float points[3][maxPoints];
+
+int startSaved = 0;
+
 // prototype functions
 void setupPins();
 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step);
@@ -109,6 +122,9 @@
 void zeroX(int pps);
 void zeroY(int pps);
 
+void savePoint(void);
+void startSavedPoints(void);
+
 void printDistance(void);
 
 // MAIN PROGRAM ----------------------------------------------------------------
@@ -131,6 +147,9 @@
     endZ.fall(&endZ_press);
     endZ.rise(&endZ_release);
     
+    saveBtn.rise(&savePoint);
+    startBtn.rise(&startSavedPoints);
+    
     if(modeStatus){
         //Código de JOG
         printf("JOG Selected\n\r");
@@ -212,12 +231,59 @@
             }
             
             contador+=1;
+            
+            if(startSaved){
+                break;
+            }
         }
+        
+        printf("Starting saved points\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];
+            }
+            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){
+                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);
+            
+            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);
+        }
+        
+        printf("Done\n\r");
     }
     
     else {
         printf("Routine Selected\n\r");
         
+                
+        
+        /*
         z_steps_to_run = distance_to_steps(20.0, zPitch); 
         move(ppsMax, x_plus, x_plus, z_minus, 0, 0, z_steps_to_run);
         
@@ -233,7 +299,7 @@
         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);   */  
         printf("Done\n\r");
     }  
 }  
@@ -422,4 +488,27 @@
     Y = steps_to_distance(totalY, yPitch);
     Z = steps_to_distance(totalZ, zPitch);
     printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
+}
+
+void savePoint(void){
+    printf("\nSave selected\n\r");
+    if(saved >= maxPoints){
+        printf("Max points reached\n\r");
+    } else{
+        X = steps_to_distance(totalX, xPitch);
+        Y = steps_to_distance(totalY, yPitch);
+        Z = steps_to_distance(totalZ, zPitch);
+        
+        points[0][saved] = X;
+        points[1][saved] = Y;
+        points[2][saved] = Z;
+        
+        saved+=1;
+    }
+    // save code
+    printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n\n", X, Y, Z);     
+}
+
+void startSavedPoints(void){
+    startSaved = 1;    
 }
\ No newline at end of file