Diff: main.cpp

Revision:

7:27ea14e58788

Parent:

6:af32c68ff4cf

Child:

8:de64672c240f

--- a/main.cpp	Thu May 03 18:00:40 2018 +0000
+++ b/main.cpp	Fri May 04 11:41:31 2018 +0000
@@ -40,18 +40,36 @@
 int y_dir;
 int z_dir;
 
-int contador = 0;
-
 float ppsMax = 600.0;
 
+int totalX = 0;
+int totalY = 0;
+
+float X = 0;
+float Y = 0;
+
+// machine parameters
+float xPitch = 5.0;
+float yPitch = 5.0;
+
+float motor_steps_by_rotation = 200.0;
+float step_mode = 0.5;
+float steps_by_rotation = motor_steps_by_rotation/step_mode;
+
+// prototype functions
 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);
+
+// MAIN PROGRAM ----------------------------------------------------------------
+
+int contador = 0;
 
 int main(){
     printf("Starting...\r\n");
     
     if(modeStatus){
         //Código de JOG
-        printf("JOG Selected");
+        printf("JOG Selected\n");
         while(1){        
             valX = joyX;
             valY = joyY;
@@ -61,15 +79,17 @@
             
             if(valX > 0.7){
                 if(x_dir){
-                    x_dir = 0;    
+                    x_dir = 0;
                 }
-                activeX = 1;    
+                activeX = 1; 
+                totalX-=1;    
             }
             else if(valX < 0.3){
                 if(!x_dir){
-                    x_dir = 1;    
+                    x_dir = 1;   
                 }
-                activeX = 1;    
+                activeX = 1;   
+                totalX+=1;  
             }
             else{
                 activeX = 0;
@@ -79,15 +99,17 @@
             
             if(valY > 0.7){
                 if(!y_dir){
-                    y_dir = 1;    
+                    y_dir = 1;   
                 }
-                activeY = 1;    
+                activeY = 1;  
+                totalY+=1;   
             }
             else if(valY < 0.3){
                 if(y_dir){
                     y_dir = 0;    
                 }
-                activeY = 1;    
+                activeY = 1; 
+                totalY-=1;   
             }
             else{
                 activeY = 0;
@@ -97,13 +119,15 @@
             
             if(!valZUp && valZDwn){
                 if(z_dir){
-                    z_dir = 0;    
+                    z_dir = 0;
+                    //totalZ-=1;    
                 }
                 activeZ = 1;    
             }
             else if(!valZDwn && valZUp){
                 if(!z_dir){
-                    z_dir = 1;    
+                    z_dir = 1;
+                    //totalZ+=1;   
                 }
                 activeZ = 1;    
             }
@@ -113,8 +137,18 @@
             
             //----------------------------------------------------------------------
 
-            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);
+            
+            if(contador > 10000){
+                X = distance(totalX, xPitch);
+                Y = distance(totalY, yPitch);
+                printf("X: %.2f     Y: %.2f\r\n", X, Y);
+                contador = 0;
+            }
+            
+            contador+=1;
         }
     }
     
@@ -124,6 +158,8 @@
     
 }
 
+// FUNCTIONS ----------------------------------------------------------------
+
 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step){
     float time = 1.0/pps/2.0;
     
@@ -171,3 +207,8 @@
         wait(time); 
     }
 }
+
+float distance(int steps, float pitch){
+    float delta_S = (((float)steps)/steps_by_rotation)*pitch;
+    return delta_S;
+}