Diff: main.cpp

Revision:

15:4604a7ee9c77

Parent:

14:7cc41420a12c

Child:

16:3adab04d42a6

--- a/main.cpp	Fri May 04 21:00:25 2018 +0000
+++ b/main.cpp	Mon May 07 19:54:12 2018 +0000
@@ -1,39 +1,34 @@
 #include "mbed.h"
 
-// Classes ---------------------------------------------------------------------------------
-Timer debounce;
-
 // MACHINE SETUP ---------------------------------------------------------------------------
 
 // drivers output signal
 DigitalOut enable(D2);
 
 DigitalOut dirX(D3);
-DigitalOut stepX(D5);
+DigitalOut stepX(D4);
 
-DigitalOut dirY(D6);
-DigitalOut stepY(D7);
+DigitalOut dirY(D5);
+DigitalOut stepY(D6);
 
-DigitalOut dirZ(D8);
-DigitalOut stepZ(D9);
+DigitalOut dirZ(D7);
+DigitalOut stepZ(D8);
 
 // hardware input signal
 // end-of-stroke sensors
-InterruptIn endX1(D10);
-InterruptIn endX2(D11);
-
-InterruptIn endY1(D12);
-InterruptIn endY2(D13);
+InterruptIn endX(D9);
+InterruptIn endY(D10);
+InterruptIn endZ(D11);
 
 // IHM for development
 AnalogIn joyX(A0);
 AnalogIn joyY(A1);
 
-DigitalIn zUp(D14);
-DigitalIn zDwn(D15);
+DigitalIn zUp(D12);
+DigitalIn zDwn(D13);
 
 // variables definition
-int modeStatus = 0;
+int modeStatus = 1;
 
 float valX;
 float valY;
@@ -55,19 +50,24 @@
 int z_plus = 1;
 int z_minus = 0;
 
-int x_limit_min = 0;
-int x_limit_max = 0;
+int x_limit = 0;
+int x_block = 0;
 
-int y_limit_min = 0;
-int y_limit_max = 0;
+int y_limit = 0;
+int y_block = 0;
+
+int z_limit = 0;
+int z_block = 0;
 
 float ppsMax = 600.0;
 
 int totalX = 0;
 int totalY = 0;
+int totalZ = 0;
 
 float X = 0;
 float Y = 0;
+float Z = 0;
 
 int endX1Status;
 int endX2Status;
@@ -75,6 +75,7 @@
 // machine parameters
 float xPitch = 3.0;
 float yPitch = 3.0;
+float zPitch = 3.0;
 
 float motor_steps_by_rotation = 200.0;
 float step_mode = 0.5;
@@ -85,15 +86,14 @@
 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);
 
-void endX1Int_press(void);
-void endX1Int_release(void);
-void endX2Int_press(void);
-void endX2Int_release(void);
+void endX_press(void);
+void endX_release(void);
 
-void endY1Int_press(void);
-void endY1Int_release(void);
-void endY2Int_press(void);
-void endY2Int_release(void);
+void endY_press(void);
+void endY_release(void);
+
+void endZ_press(void);
+void endZ_release(void);
 
 void zeroX(int pps);
 void zeroY(int pps);
@@ -107,15 +107,14 @@
     
     enable = 0;
  
-    endX1.fall(&endX1Int_press);
-    endX1.rise(&endX1Int_release);
-    endX2.fall(&endX2Int_press);
-    endX2.rise(&endX2Int_release);
+    endX.fall(&endX_press);
+    endX.rise(&endX_release);
     
-    endY1.fall(&endY1Int_press);
-    endY1.rise(&endY1Int_release);
-    endY2.fall(&endY2Int_press);
-    endY2.rise(&endY2Int_release);
+    endY.fall(&endY_press);
+    endY.rise(&endY_release);
+    
+    endZ.fall(&endZ_press);
+    endZ.rise(&endZ_release);
     
     if(modeStatus){
         //Código de JOG
@@ -132,14 +131,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;
@@ -169,17 +168,17 @@
             
             if(!valZUp && valZDwn){
                 if(z_dir != z_minus){
-                    z_dir = z_minus;
-                    //totalZ-=1;    
+                    z_dir = z_minus;    
                 }
-                activeZ = 1;    
+                activeZ = 1;  
+                totalZ-=1;  
             }
             else if(!valZDwn && valZUp){
                 if(z_dir != z_plus){
-                    z_dir = z_plus;
-                    //totalZ+=1;   
+                    z_dir = z_plus;   
                 }
-                activeZ = 1;    
+                activeZ = 1; 
+                totalZ+=1;   
             }
             else {
                 activeZ = 0;    
@@ -187,14 +186,13 @@
             
             //----------------------------------------------------------------------
 
-            move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);  
+            //move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);  
             X = distance(totalX, xPitch);
             Y = distance(totalY, yPitch);
+            Z = distance(totalZ, zPitch);
             
             if(contador > 10000){
-                X = distance(totalX, xPitch);
-                Y = distance(totalY, yPitch);
-                printf("X: %.2f     Y: %.2f\r\n", X, Y);
+                printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
                 contador = 0;
             }
             
@@ -203,55 +201,42 @@
     }
     
     else {
-        zeroY(ppsMax);
         
-        X = distance(totalX, xPitch);
-        Y = distance(totalY, yPitch);
-        
-        printf("X: %.2f     Y: %.2f\r\n", X, Y);
-        
-        zeroX(ppsMax);
-        
-        X = distance(totalX, xPitch);
-        Y = distance(totalY, yPitch);
-        
-        printf("X: %.2f     Y: %.2f\r\n", X, Y);
     }  
 }  
 
 // FUNCTIONS ----------------------------------------------------------------
-void endX1Int_press(void){
-    printf("X1 - press\n\r");
-    x_limit_min = 1;  
-}
-void endX1Int_release(void){
-    printf("X1 - release\n\r");
-    x_limit_min = 0; 
+void endX_press(void){
+    printf("X - press\n\r");
+    if(dirX == x_plus){
+        x_limit = x_plus;
+        x_block = 1;
+    }
+    else {
+        x_limit = x_minus;
+        x_block = 1;
+    }
 }
-void endX2Int_press(void){
-    printf("X2 - press\n\r");
-    x_limit_max = 1;   
+void endX_release(void){
+    printf("X - release\n\r");
+    x_block = 0;
 }
-void endX2Int_release(void){
-    printf("X2 - release\n\r");
-    x_limit_max = 0;   
+void endY_press(void){
+    printf("Y - press\n\r");
+    y_limit = 1;   
+}
+void endY_release(void){
+    printf("Y - release\n\r");
+    y_limit = 0;   
 }
 
-void endY1Int_press(void){
-    printf("Y1 - press\n\r");
-    y_limit_min = 1;  
-}
-void endY1Int_release(void){
-    printf("Y1 - release\n\r");
-    y_limit_min = 0; 
+void endZ_press(void){
+    printf("Z - press\n\r");
+    z_limit = 1;  
 }
-void endY2Int_press(void){
-    printf("Y2 - press\n\r");
-    y_limit_max = 1;   
-}
-void endY2Int_release(void){
-    printf("Y2 - release\n\r");
-    y_limit_max = 0;   
+void endZ_release(void){
+    printf("Z - release\n\r");
+    z_limit = 0; 
 }
 
 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step){
@@ -261,18 +246,15 @@
     dirY = y_dir;
     dirZ = z_dir;
     
-    if((x_dir == x_minus) && x_limit_min){
-        x_step = 0;    
-    }
-    if((x_dir == x_plus) && x_limit_max){
+    if((x_dir == x_limit) && x_block){
         x_step = 0;    
     }
     
-    if((y_dir == y_minus) && y_limit_min){
+    if((y_dir == y_limit) && y_block){
         y_step = 0;    
     }
-    if((y_dir == y_plus) && y_limit_max){
-        y_step = 0;    
+    if((z_dir == z_limit) && z_block){
+        z_step = 0;    
     }
     
     int max_val;
@@ -316,7 +298,7 @@
         wait(time); 
     }
 }
-
+/*
 void zeroX(int pps){
     float time = 1.0/pps/2.0;
     
@@ -351,7 +333,7 @@
     printf("Y zero\n\r");
     
     totalY = 0.0;
-}
+}*/
 
 float distance(int steps, float pitch){
     float delta_S = (((float)steps)/steps_by_rotation)*pitch;