Diff: main.cpp

Revision:

32:5bd3f0d073ac

Parent:

31:7896ac3a5a1c

Child:

33:6ad0d169b154

--- a/main.cpp	Fri May 25 23:53:27 2018 +0000
+++ b/main.cpp	Fri Jun 08 20:57:57 2018 +0000
@@ -26,15 +26,15 @@
 
 // hardware input signal
 // end-of-stroke sensors
-InterruptIn endX(A2);
-InterruptIn endY(A3);
-InterruptIn endZ(A4);
+InterruptIn endX(A3);
+InterruptIn endY(A4);
+InterruptIn endZ(A5);
 
 // JOG Controls
 AnalogIn joyX(A0);
 AnalogIn joyY(A1);
 
-InterruptIn joyClick(D9);
+InterruptIn joyClick(A2);
 
 // variables definition
 int modeStatus = 1;
@@ -71,7 +71,7 @@
 int z_block_max = 0;
 int z_block_min = 0;
 
-float ppsMax = 1650.0;
+float ppsMax = 1500.0;
 float glue_time = 1.0;
 
 int totalX = 0;
@@ -98,7 +98,7 @@
 // machine parameters
 float xPitch = 3.0;
 float yPitch = 3.0;
-float zPitch = 3.0;
+float zPitch = 6.0;
 
 float motor_steps_by_rotation = 200.0;
 float step_mode = 0.5;
@@ -150,6 +150,8 @@
 
 void cancelProgram(void);
 
+void moving_test(void);
+
 void clean_glue(void);
 
 // MAIN PROGRAM ----------------------------------------------------------------------------------------------
@@ -157,7 +159,22 @@
     ihm.baud(9600);
     printf("\nStarting...\r\n");
     
+    enable = 1;
+    
     t.start();
+    /*
+    enable = 0;
+    while (1){
+        moving_test();
+    }*/
+      
+    /*  
+    while(1){
+        glueValve = 1;
+        wait(1);
+        glueValve = 0;
+        wait(1);   
+    }*/
     
     //  interrupções de fim de curso
     /*
@@ -170,8 +187,6 @@
     endZ.fall(&endZ_press);
     endZ.rise(&endZ_release);*/
     
-    enable = 1;
-    
     joyClick.fall(&joystickClick);
     
     while(1){
@@ -412,9 +427,7 @@
     
     X = steps_to_distance(totalX, xPitch);
     Y = steps_to_distance(totalY, yPitch);
-    Z = steps_to_distance(totalZ, zPitch);  
-    
-    ihm_class.send_position(X, Y, Z);
+    Z = steps_to_distance(totalZ, zPitch);
      
     printf("\nSave selected\n\r");
     if(saved >= maxPoints){
@@ -436,7 +449,9 @@
     printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n", points[0][saved-1], points[1][saved-1], points[2][saved-1]);
     printf("Path --> Speed: %.2f      Mode: %.2f\n\r", path[0][saved-1], path[1][saved-1]);
     resting_points = maxPoints-saved;
-    printf("Pontos restantes: %d\n\r", resting_points);   
+    printf("Pontos restantes: %d\n\r", resting_points);     
+    
+    ihm_class.send_position(X, Y, Z);
 }
 
 void jog(void){
@@ -457,16 +472,16 @@
         valX = joyX;
         valY = joyY;
         
-        if(valX > 0.52 && !toggle){
-            if(x_dir != x_minus){
-                x_dir = x_minus;
+        if(valX > 0.7 && !toggle){
+            if(x_dir != x_plus){
+                x_dir = x_plus;
             }
             //speed = ppsMax*(valX - 0.52)/0.48;
             activeX = 1;     
         }
-        else if(valX < 0.48 && !toggle){
-            if(x_dir != x_plus){
-                x_dir = x_plus;   
+        else if(valX < 0.4 && !toggle){
+            if(x_dir != x_minus){
+                x_dir = x_minus;   
             }
             //speed = ppsMax*valX/0.48;
             activeX = 1;  
@@ -477,14 +492,14 @@
         
         //----------------------------------------------------------------------
         
-        if(valY > 0.52 && !toggle){
+        if(valY > 0.7 && !toggle){
             if(y_dir != y_plus){
                 y_dir = y_plus;  
             }
             //speed = ppsMax*(valY - 0.52)/0.48; 
             activeY = 1;  
         }
-        else if(valY < 0.48 && !toggle){
+        else if(valY < 0.4 && !toggle){
             if(y_dir != y_minus){
                 y_dir = y_minus;  
             }
@@ -497,18 +512,18 @@
         
         //----------------------------------------------------------------------   
         
-        if(valY > 0.52 && toggle){
+        if(valY > 0.7 && toggle){
             if(z_dir != z_plus){
                 z_dir = z_plus;   
             }
             activeZ = 1; 
             //speed = ppsMax*(valY - 0.52)/0.48;   
         }
-        else if(valY < 0.48 && toggle){
+        else if(valY < 0.4 && toggle){
             if(z_dir != z_minus){
                 z_dir = z_minus;  
             }
-            activeZ = 1; 
+            activeZ = -1; 
             //speed = ppsMax*(valY - 0.52)/0.48;    
         }
         else {
@@ -523,7 +538,12 @@
         //printf("%f\n\r", speed);
         //----------------------------------------------------------------------
 
-        move((int)speed, x_dir, y_dir, z_dir, activeX, activeY, activeZ);  
+        if(!toggle){
+            move((int)speed, x_dir, y_dir, z_dir, activeX, activeY, activeZ);  
+        } else {
+            moveZ(speed, activeZ);     
+        }
+        
         X = steps_to_distance(totalX, xPitch);
         Y = steps_to_distance(totalY, yPitch);
         Z = steps_to_distance(totalZ, zPitch);
@@ -557,6 +577,7 @@
 }
  
 void virtualZero(void){
+    printf("Movendo para o zero\n\r");
     X = steps_to_distance(totalX, xPitch);
     Y = steps_to_distance(totalY, yPitch);
     Z = steps_to_distance(totalZ, zPitch);
@@ -595,25 +616,30 @@
     float speed;
     float glue;
     
+    int glue_on_point = 0;
+    
     enable = 0;
     
     printf("\nStarting automatic routine\n\r");
     
     virtualZero();
+    
     wait(1);
     // routine
     
-    for(int i = 0; i < total_points-1; i++){
+    printf("Total points: %d\n\r", total_points);
+    
+    for(int i = 0; i <= total_points; i++){
         printf("Move n: %d\n\r", i);
         
-        x_start = distance_to_steps(points[0][i], xPitch);
-        x_end = distance_to_steps(points[0][i+1], xPitch);
+        x_start = totalX;
+        x_end = distance_to_steps(points[0][i], xPitch);
         
-        y_start = distance_to_steps(points[1][i], yPitch);
-        y_end = distance_to_steps(points[1][i+1], yPitch);
+        y_start = totalY;
+        y_end = distance_to_steps(points[1][i], yPitch);
         
-        z_start = distance_to_steps(points[2][i], zPitch);
-        z_end = distance_to_steps(points[2][i+1], zPitch);
+        z_start = totalZ;
+        z_end = distance_to_steps(points[2][i], zPitch);
         
         z_dist = z_end - z_start;
         
@@ -621,17 +647,16 @@
         speed = path[1][i]*ppsMax;
         glue = path[2][i];
         
-        printf("X1: %.2f     X2: %.2f\n\r", x_start, x_end);
-        printf("Y1: %.2f     Y2: %.2f\n\r", y_start, y_end);
-        printf("Z1: %.2f     Z2: %.2f\n\r", z_start, z_end);
+        printf("X1: %.2f     X2: %.2f\n\r", steps_to_distance(x_start, xPitch), steps_to_distance(x_end, xPitch));
+        printf("Y1: %.2f     Y2: %.2f\n\r", steps_to_distance(y_start,  yPitch), steps_to_distance(y_end, yPitch));
+        printf("Z1: %.2f     Z2: %.2f\n\r", steps_to_distance(z_start, zPitch), steps_to_distance(z_end, zPitch));
         printf("Glue: %.1f, Speed: %.1f\n\r", glue, speed);
         
         moveZ(speed, z_dist);
         
         if(glue ==  1.0){
-            glueValve = 1;
-            wait(glue_time);
-            glueValve = 0;    
+            glue_on_point = 1;
+                
         } else if(glue == 0.0){
             glueValve = 0;
         } else if(glue == 2.0){
@@ -641,6 +666,12 @@
         linear_interpolation(x_start, y_start, x_end, y_end, speed);
         
         glueValve = 0;
+        
+        if(glue_on_point){
+            glueValve = 1;
+            wait(glue_time);
+            glueValve = 0;
+        }
     }
     
     killJog();
@@ -659,9 +690,9 @@
         z_dir = z_plus;
     }
     
-    printf("dz: %f\n\r", steps_to_distance(z_dist, zPitch));
+    //printf("dz: %f\n\r", steps_to_distance(z_dist, zPitch));
     
-    move(speed, x_plus, y_plus, z_dir, 0, 0, z_dist); 
+    move(speed*0.6, x_plus, y_plus, z_dir, 0, 0, z_dist); 
 }
 
 void killJog(void){
@@ -738,6 +769,7 @@
             if(save){
                 savePoint(speed_str, mode_recv);
             } else if(begin){
+                printf("Automatic: %d\n\r", saved);
                 automatic_run(saved);    
             } else if(jog_mode){
                 if(kill_jog_mode){
@@ -771,8 +803,11 @@
                 clean_glue();
             } else if(delete_saved){
                 saved = saved - 1;    
+            } else {
+                printf("End\n\r");
+                return;
             }
-            break;
+            return;
         }
         
         if(recv == 's'){
@@ -997,11 +1032,13 @@
 void moving_test(void){
     move(ppsMax, x_plus, y_plus, z_plus, 3000, 0, 0);
     move(ppsMax, x_plus, y_plus, z_plus, 0, 3000, 0);
-    move(ppsMax, x_plus, y_plus, z_plus, 0, 0, 1000);
+    moveZ(ppsMax, 1000);
+    
+    wait(2);
     
     move(ppsMax, x_minus, y_minus, z_minus, 3000, 0, 0);
     move(ppsMax, x_minus, y_minus, z_minus, 0, 3000, 0);
-    move(ppsMax, x_minus, y_minus, z_minus, 0, 0, 1000);
+    moveZ(ppsMax, -1000);
 }
 
 void clean_glue(void){