Diff: main.cpp

Revision:

17:55e6270adab5

Parent:

16:3adab04d42a6

Child:

18:174b4ff351b5

--- a/main.cpp	Tue May 08 11:41:56 2018 +0000
+++ b/main.cpp	Tue May 08 13:06:10 2018 +0000
@@ -27,8 +27,10 @@
 DigitalIn zUp(D15);
 DigitalIn zDwn(D14);
 
+DigitalIn botao(D11);
+
 // variables definition
-int modeStatus = 1;
+int modeStatus;
 
 float valX;
 float valY;
@@ -45,8 +47,8 @@
 
 int x_plus = 0;
 int x_minus = 1;
-int y_plus = 1;
-int y_minus = 0;
+int y_plus = 0;
+int y_minus = 1;
 int z_plus = 0;
 int z_minus = 1;
 
@@ -68,6 +70,10 @@
 int totalY = 0;
 int totalZ = 0;
 
+int x_steps_to_run;
+int y_steps_to_run;
+int z_steps_to_run;
+
 float X = 0;
 float Y = 0;
 float Z = 0;
@@ -87,7 +93,9 @@
 // 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);
-float distance(int steps, float pitch);
+
+float steps_to_distance(int steps, float pitch);
+int distance_to_steps(float distance, float pitch);
 
 void endX_press(void);
 void endX_release(void);
@@ -101,6 +109,8 @@
 void zeroX(int pps);
 void zeroY(int pps);
 
+void printDistance(void);
+
 // MAIN PROGRAM ----------------------------------------------------------------
 
 int contador = 0;
@@ -108,6 +118,8 @@
 int main(){
     printf("\nStarting...\r\n");
     
+    modeStatus = botao;
+    
     enable = 0;
  
     endX.fall(&endX_press);
@@ -150,15 +162,15 @@
             //----------------------------------------------------------------------
             
             if(valY > 0.7){
-                if(y_dir != y_minus){
-                    y_dir = y_minus;   
+                if(y_dir != y_plus){
+                    y_dir = y_plus;   
                 }
                 activeY = 1;  
                 totalY+=1;   
             }
             else if(valY < 0.3){
-                if(y_dir != y_plus){
-                    y_dir = y_plus;    
+                if(y_dir != y_minus){
+                    y_dir = y_minus;    
                 }
                 activeY = 1; 
                 totalY-=1;   
@@ -190,9 +202,9 @@
             //----------------------------------------------------------------------
 
             move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);  
-            X = distance(totalX, xPitch);
-            Y = distance(totalY, yPitch);
-            Z = distance(totalZ, zPitch);
+            X = steps_to_distance(totalX, xPitch);
+            Y = steps_to_distance(totalY, yPitch);
+            Z = steps_to_distance(totalZ, zPitch);
             
             if(contador > 10000){
                 printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
@@ -204,10 +216,25 @@
     }
     
     else {
-        while(1){
-            move(ppsMax, x_plus, y_plus, z_plus, 0, 0, 400);
-            move(ppsMax, x_plus, y_plus, z_minus, 0, 0, 400);    
-        }
+        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);
+        
+        y_steps_to_run = distance_to_steps(50, yPitch);
+        move(ppsMax, x_plus, y_minus, z_plus, 0, y_steps_to_run, 0);
+        
+        x_steps_to_run = distance_to_steps(50, xPitch);
+        move(ppsMax, x_plus, y_plus, z_plus, x_steps_to_run, 0, 0); 
+        
+        y_steps_to_run = distance_to_steps(50, yPitch);
+        move(ppsMax, x_plus, y_plus, z_plus, 0, y_steps_to_run, 0);
+        
+        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);     
+        printf("Done\n\r");
     }  
 }  
 
@@ -328,15 +355,29 @@
         stepZ = 0;
         wait(time); 
     }
+
+    if(dirX == x_minus){
+        totalX-=x_step;
+    }
+    else { totalX+=x_step; }
+    
+    if(dirY == y_minus){
+        totalY-=y_step;
+    } else { totalY+=y_step; }
+    
+    if(dirZ == z_minus){
+        totalZ-=z_step;
+    } else { totalZ+=z_step; }
 }
-/*
+
 void zeroX(int pps){
     float time = 1.0/pps/2.0;
     
-    while(!x_limit_min){
-        if(dirX != x_minus){
-            dirX = x_minus;    
-        }
+    if(dirX != x_minus){
+        dirX = x_minus;    
+    }
+    
+    while(!x_block_min){
         stepX = 1;
         wait(time);
         stepX = 0;
@@ -345,16 +386,16 @@
     
     printf("X zero\n\r");
     
-    totalX = 0.0;
+    totalX = 0;
 }
-
 void zeroY(int pps){
     float time = 1.0/pps/2.0;
     
-    while(!y_limit_min){
-        if(dirY != y_minus){
-            dirY = y_minus;    
-        }
+    if(dirY != y_minus){
+        dirY = y_minus;    
+    }
+    
+    while(!y_block_min){
         stepY = 1;
         wait(time);
         stepY = 0;
@@ -363,10 +404,22 @@
     
     printf("Y zero\n\r");
     
-    totalY = 0.0;
-}*/
+    totalY = 0;
+}
 
-float distance(int steps, float pitch){
+float steps_to_distance(int steps, float pitch){
     float delta_S = (((float)steps)/steps_by_rotation)*pitch;
     return delta_S;
 }
+
+int distance_to_steps(float distance, float pitch){
+    int steps = (int)(distance*steps_by_rotation/pitch);
+    return steps;
+}
+
+void printDistance(void){
+    X = steps_to_distance(totalX, xPitch);
+    Y = steps_to_distance(totalY, yPitch);
+    Z = steps_to_distance(totalZ, zPitch);
+    printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
+}
\ No newline at end of file