Diff: main.cpp

Revision:

30:27838f6fdfd6

Parent:

29:5b181a35ca32

Child:

31:7896ac3a5a1c

--- a/main.cpp	Mon May 21 19:47:50 2018 +0000
+++ b/main.cpp	Fri May 25 17:28:28 2018 +0000
@@ -22,6 +22,8 @@
 DigitalOut ledXY(D10);
 DigitalOut ledZ(D11);
 
+DigitalOut glueValve(D12);
+
 // hardware input signal
 // end-of-stroke sensors
 InterruptIn endX(A2);
@@ -70,6 +72,7 @@
 int z_block_min = 0;
 
 float ppsMax = 1650.0;
+float glue_time = 1.0;
 
 int totalX = 0;
 int totalY = 0;
@@ -138,11 +141,13 @@
 
 void joystickClick(void);
 
-void linear_interpolation(int x1, int y1, int x2, int y2);
+void linear_interpolation(int x1, int y1, int x2, int y2, float speed);
 void circular_interpolation(int, int, int);
 
 void test_direction(void);
 
+void cancelProgram(void);
+
 // MAIN PROGRAM ----------------------------------------------------------------------------------------------
 int main(){
     printf("\nStarting...\r\n");
@@ -160,13 +165,9 @@
     endZ.fall(&endZ_press);
     endZ.rise(&endZ_release);*/
     
-    enable = 0;
-    
-    //test_direction();
+    enable = 1;
     
-    //circular_interpolation(400, 200, 200);
-    
-    //joyClick.fall(&joystickClick);
+    joyClick.fall(&joystickClick);
     
     while(1){
         if(ihm.readable()){
@@ -421,24 +422,29 @@
         points[0][saved] = X;
         points[1][saved] = Y;
         points[2][saved] = Z;
-        path[0][saved] = atof(speed.c_str());
-        path[1][saved] = atof(mode.c_str());
+        path[0][saved] = 1.0;
+        path[1][saved] = atof(speed.c_str());
+        path[2][saved] = atof(mode.c_str());
         saved+=1;
     }
     // save code
-    printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
+    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);   
 }
 
 void jog(void){
+    float speed;
+    
     enable = 0;
     kill_jog = 0;
     printf("JOG Selected %d\n\r", kill_jog);
     
     t.reset();
     
+    speed = ppsMax;
+    
     while(1){  
         if(ihm.readable()){
             readSerial();    
@@ -446,16 +452,18 @@
         valX = joyX;
         valY = joyY;
         
-        if(valX > 0.8 && !toggle){
+        if(valX > 0.52 && !toggle){
             if(x_dir != x_minus){
                 x_dir = x_minus;
             }
+            //speed = ppsMax*(valX - 0.52)/0.48;
             activeX = 1;     
         }
-        else if(valX < 0.3 && !toggle){
+        else if(valX < 0.48 && !toggle){
             if(x_dir != x_plus){
                 x_dir = x_plus;   
             }
+            //speed = ppsMax*valX/0.48;
             activeX = 1;  
         }
         else{
@@ -464,16 +472,18 @@
         
         //----------------------------------------------------------------------
         
-        if(valY > 0.8 && !toggle){
+        if(valY > 0.52 && !toggle){
             if(y_dir != y_plus){
-                y_dir = y_plus;   
+                y_dir = y_plus;  
             }
+            //speed = ppsMax*(valY - 0.52)/0.48; 
             activeY = 1;  
         }
-        else if(valY < 0.3 && !toggle){
+        else if(valY < 0.48 && !toggle){
             if(y_dir != y_minus){
-                y_dir = y_minus;    
+                y_dir = y_minus;  
             }
+            //speed = ppsMax*valY/0.48;  
             activeY = 1;  
         }
         else{
@@ -482,28 +492,37 @@
         
         //----------------------------------------------------------------------   
         
-        if(valY > 0.8 && toggle){
+        if(valY > 0.52 && toggle){
             if(z_dir != z_plus){
-                z_dir = z_plus;    
+                z_dir = z_plus;   
             }
-            activeZ = 1;   
+            activeZ = 1; 
+            //speed = ppsMax*(valY - 0.52)/0.48;   
         }
-        else if(valY < 0.3 && toggle){
+        else if(valY < 0.48 && toggle){
             if(z_dir != z_minus){
-                z_dir = z_minus;   
+                z_dir = z_minus;  
             }
-            activeZ = 1;   
+            activeZ = 1; 
+            //speed = ppsMax*(valY - 0.52)/0.48;    
         }
         else {
             activeZ = 0;    
         }
         
+        /*
+        if(speed < 100.0){
+            speed = 100.0;
+        }*/
+        
+        //printf("%f\n\r", speed);
         //----------------------------------------------------------------------
 
-        move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);  
+        move((int)speed, x_dir, y_dir, z_dir, activeX, activeY, activeZ);  
         X = steps_to_distance(totalX, xPitch);
         Y = steps_to_distance(totalY, yPitch);
         Z = steps_to_distance(totalZ, zPitch);
+        
         /*
         if(t.read() > sendTime){
             //printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
@@ -553,7 +572,7 @@
         z_dir = z_plus;
     }
     
-    linear_interpolation(x_start, y_start, x_end, y_end);
+    linear_interpolation(x_start, y_start, x_end, y_end, ppsMax);
     move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_dist);
 }  
             
@@ -567,28 +586,56 @@
     
     int z_dist;
     
+    float mode;
+    float speed;
+    float glue;
+    
     enable = 0;
+    
     printf("\nStarting automatic routine\n\r");
     
     virtualZero();
-    
+    wait(1);
     // routine
     
-    for(int i = 1; i <= total_points; i++){
+    for(int i = 0; i < total_points-1; i++){
         printf("Move n: %d\n\r", i);
-        x_start = distance_to_steps(points[0][i-1], xPitch);
-        x_end = distance_to_steps(points[0][i], xPitch);
+        
+        x_start = distance_to_steps(points[0][i], xPitch);
+        x_end = distance_to_steps(points[0][i+1], xPitch);
         
-        y_start = distance_to_steps(points[1][i-1], yPitch);
-        y_end = distance_to_steps(points[1][i], yPitch);
+        y_start = distance_to_steps(points[1][i], yPitch);
+        y_end = distance_to_steps(points[1][i+1], yPitch);
         
-        z_start = distance_to_steps(points[2][i-1], zPitch);
-        z_end = distance_to_steps(points[2][i], zPitch);
+        z_start = distance_to_steps(points[2][i], zPitch);
+        z_end = distance_to_steps(points[2][i+1], zPitch);
         
         z_dist = z_end - z_start;
         
-        moveZ(ppsMax, z_dist);
-        linear_interpolation(x_start, y_start, x_end, y_end);
+        mode = path[0][i];
+        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("Glue: %.1f, Speed: %.1f\n\r", glue, speed);
+        
+        moveZ(speed, z_dist);
+        
+        if(glue ==  1.0){
+            glueValve = 1;
+            wait(glue_time);
+            glueValve = 0;    
+        } else if(glue == 0.0){
+            glueValve = 0;
+        } else if(glue == 2.0){
+            glueValve = 1;
+        }
+        
+        linear_interpolation(x_start, y_start, x_end, y_end, speed);
+        
+        glueValve = 0;
     }
     
     killJog();
@@ -596,6 +643,7 @@
     wait(0.1);
     ihm_class.action_complete();
     enable = 1;
+    cancelProgram();
 }
 
 void moveZ(int speed, int z_dist){
@@ -628,7 +676,7 @@
     char recv;
     char last_recv;
     
-    float timeoutTime = 0.2;
+    float timeoutTime = 0.5;
     
     //save
     int save = 0;
@@ -704,7 +752,9 @@
             } else if(upload){
                 ihm_class.read_gcode(upload_buffer);                
             } else if(send_saved){
-                ihm_class.sendSaved();    
+                wait(1);
+                ihm_class.sendSaved();
+                return;
             }
             break;
         }
@@ -792,7 +842,7 @@
     toggle = !toggle;    
 }
 
-void linear_interpolation(int x1, int y1, int x2, int y2){
+void linear_interpolation(int x1, int y1, int x2, int y2, float speed){
     int x = x1;
     int y = y1;
     
@@ -802,8 +852,6 @@
     int direction_x;
     int direction_y;
     
-    printf("dx: %f      dy: %f\n\r", steps_to_distance(dx, xPitch), steps_to_distance(dy, yPitch));
-    
     if(dx > 0 && dy > 0){
         x_dir = x_plus;
         y_dir = y_plus;
@@ -827,7 +875,7 @@
         dx = dx*(-1);
         dy = dy*(-1);  
         direction_x = -1;
-        direction_y = 1;
+        direction_y = -1;
     } else if(dy == 0){
         if(dx > 0){
             x_dir = x_plus;
@@ -859,12 +907,12 @@
             if(d < 0){
                 d = d + incI;
                 x = x + 1*direction_x;
-                move(ppsMax, x_dir, y_dir, z_dir, 1, 0, 0);
+                move((int)speed, x_dir, y_dir, z_dir, 1, 0, 0);
             } else {
                 d = d + incS;
                 x = x + 1*direction_x;
                 y = y + 1*direction_y;
-                move(ppsMax, x_dir, y_dir, z_dir, 1, 1, 0);
+                move((int)speed, x_dir, y_dir, z_dir, 1, 1, 0);
             }
         }  
     } else {
@@ -872,21 +920,20 @@
         int incS = 2*(dx - dy);
         
         int d = (2*dx - dy);
-    
+        
         while(y != y2){        
             if(d < 0){
                 d = d + incI;
                 y = y + 1*direction_y;
-                move(ppsMax, x_dir, y_dir, z_dir, 0, 1, 0);
+                move((int)speed, x_dir, y_dir, z_dir, 0, 1, 0);
             } else {
                 d = d + incS;
                 x = x + 1*direction_x;
                 y = y + 1*direction_y;
-                move(ppsMax, x_dir, y_dir, z_dir, 1, 1, 0);
+                move((int)speed, x_dir, y_dir, z_dir, 1, 1, 0);
             }
         }
-    }
-    printf("x: %d      y: %d\n\r", x, y);      
+    }    
 }
 
 void circular_interpolation(int r, int xc, int yc){