Diff: main.cpp

Revision:

26:f891ff6beb33

Parent:

25:7b3826dddcba

Child:

27:3bbc354adea6

diff -r 7b3826dddcba -r f891ff6beb33 main.cpp
--- a/main.cpp	Wed May 16 12:19:49 2018 +0000
+++ b/main.cpp	Thu May 17 23:08:43 2018 +0000
@@ -19,6 +19,9 @@
 DigitalOut dirZ(D7);
 DigitalOut stepZ(D8);
 
+DigitalOut ledXY(D10);
+DigitalOut ledZ(D11);
+
 // hardware input signal
 // end-of-stroke sensors
 InterruptIn endX(A2);
@@ -29,16 +32,15 @@
 AnalogIn joyX(A0);
 AnalogIn joyY(A1);
 
-DigitalIn zUp(D15);
-DigitalIn zDwn(D14);
+InterruptIn joyClick(D9);
 
 // variables definition
 int modeStatus = 1;
 
 float valX;
 float valY;
-int valZUp;
-int valZDwn;
+
+int toggle = 0;
 
 int activeX = 1;
 int activeY = 1;
@@ -103,11 +105,6 @@
 float path[2][maxPoints]; // [0] speed; [1] mode;
 int resting_points = 0;
 
-int startSaved = 0;
-int autoReload = 0;
-int exitSavedPointsLoop = 0;
-int keepSavedPointsReload = 0;
-
 int kill_jog;
 
 // PROTOTYPE FUNCTIONS -------------------------------------------------------------------------------
@@ -118,8 +115,8 @@
 
 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step);
 
-float steps_to_distance(int steps, float pitch);
-int distance_to_steps(float distance, float pitch);
+float steps_to_distance(int, float);
+int distance_to_steps(float, float);
 
 void endX_press(void);
 void endX_release(void);
@@ -130,8 +127,10 @@
 void endZ_press(void);
 void endZ_release(void);
 
-void zeroX(int pps);
-void zeroY(int pps);
+void goMachineHome(int);
+void zeroX(int);
+void zeroY(int);
+void zeroZ(int);
 
 void savePoint(void);
 void startSavedPoints(void);
@@ -140,11 +139,14 @@
 
 void readSerial(void);
 
+void joystickClick(void);
+
+void linear_interpolation(int x1, int y1, int x2, int y2);
+
 // MAIN PROGRAM ----------------------------------------------------------------------------------------------
 int main(){
     printf("\nStarting...\r\n");
     
-    enable = 0;
     t.start();
     
     //  interrupções de fim de curso
@@ -158,11 +160,23 @@
     endZ.fall(&endZ_press);
     endZ.rise(&endZ_release);*/
     
+    enable = 0;
+    
+    while(1){
+        linear_interpolation(0, 0, 0, -5000);
+        linear_interpolation(0, -5000, 5000, 0);
+        linear_interpolation(5000, 0, 5000, 2500);
+        linear_interpolation(5000, 2500, 0, 0);
+    }
+    
+    joyClick.fall(&joystickClick);
+    /*
     while(1){
         if(ihm.readable()){
             readSerial();    
-        }    
-    }
+        }   
+         
+    }*/
 }  
 
 // FUNCTIONS ----------------------------------------------------------------
@@ -316,6 +330,10 @@
     printf("X zero\n\r");
     
     totalX = 0;
+    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);
 }
 void zeroY(int pps){
     printf("Zero Y\n\r");
@@ -335,6 +353,10 @@
     printf("Y zero\n\r");
     
     totalY = 0;
+    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);
 }
 
 void zeroZ(int pps){
@@ -355,6 +377,10 @@
     printf("Z zero\n\r");
     
     totalZ = 0;
+    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);
 }
 
 float steps_to_distance(int steps, float pitch){
@@ -375,37 +401,42 @@
 }
 
 void savePoint(string speed, string mode){
-    if(!autoReload){
-        keepSavedPointsReload = 0;
-        exitSavedPointsLoop = 0;
-        printf("\nSave selected\n\r");
-        if(saved >= maxPoints){
-            printf("Max points reached\n\r");
-        } else{
-            X = steps_to_distance(totalX, xPitch);
-            Y = steps_to_distance(totalY, yPitch);
-            Z = steps_to_distance(totalZ, zPitch);
-            
-            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());
-            saved+=1;
-        }
-        // save code
-        printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
-        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);
-    }  
-    else { 
-        exitSavedPointsLoop = 1;
-        keepSavedPointsReload = 0;
-    }   
+    if(saved == 0){
+        totalX = 0;
+        totalY = 0;
+        totalZ = 0;
+    }
+    
+    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);
+     
+    printf("\nSave selected\n\r");
+    if(saved >= maxPoints){
+        printf("Max points reached\n\r");
+    } else{
+        X = steps_to_distance(totalX, xPitch);
+        Y = steps_to_distance(totalY, yPitch);
+        Z = steps_to_distance(totalZ, zPitch);
+        
+        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());
+        saved+=1;
+    }
+    // save code
+    printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
+    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){
+    enable = 0;
     kill_jog = 0;
     printf("JOG Selected %d\n\r", kill_jog);
     
@@ -418,18 +449,15 @@
         valX = joyX;
         valY = joyY;
         
-        valZUp = zUp;
-        valZDwn = zDwn;
-        
-        if(valX > 0.7){
-            if(x_dir != x_plus){
-                x_dir = x_plus;
+        if(valX > 0.8 && !toggle){
+            if(x_dir != x_minus){
+                x_dir = x_minus;
             }
             activeX = 1;     
         }
-        else if(valX < 0.3){
-            if(x_dir != x_minus){
-                x_dir = x_minus;   
+        else if(valX < 0.3 && !toggle){
+            if(x_dir != x_plus){
+                x_dir = x_plus;   
             }
             activeX = 1;  
         }
@@ -439,13 +467,13 @@
         
         //----------------------------------------------------------------------
         
-        if(valY > 0.7){
+        if(valY > 0.8 && !toggle){
             if(y_dir != y_plus){
                 y_dir = y_plus;   
             }
             activeY = 1;  
         }
-        else if(valY < 0.3){
+        else if(valY < 0.3 && !toggle){
             if(y_dir != y_minus){
                 y_dir = y_minus;    
             }
@@ -457,15 +485,15 @@
         
         //----------------------------------------------------------------------   
         
-        if(!valZUp && valZDwn){
-            if(z_dir != z_minus){
-                z_dir = z_minus;    
+        if(valY > 0.8 && toggle){
+            if(z_dir != z_plus){
+                z_dir = z_plus;    
             }
             activeZ = 1;   
         }
-        else if(!valZDwn && valZUp){
-            if(z_dir != z_plus){
-                z_dir = z_plus;   
+        else if(valY < 0.3 && toggle){
+            if(z_dir != z_minus){
+                z_dir = z_minus;   
             }
             activeZ = 1;   
         }
@@ -479,23 +507,36 @@
         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);
+            //printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
             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);
             t.reset();
-        }
+        }*/
         
         if(kill_jog){
             break;
         }
+        
+        if(toggle){
+            if(!ledZ){
+                ledZ = 1;
+                ledXY = 0;
+            }
+        } else {
+            if(!ledXY){
+                ledXY = 1;
+                ledZ = 0;
+            }
+        }
     }
 }
             
 void automatic_run(int total_points){
+    enable = 0;
     printf("\nStarting automatic routine\n\r");
     
     // home
@@ -568,10 +609,16 @@
     }
     killJog();
     printf("Done\n\r");
+    wait(0.1);
+    ihm_class.action_complete();
+    enable = 1;
 }
 
 void killJog(void){
+    enable = 1;
     kill_jog = 1; 
+    ledZ = 0;
+    ledXY = 0;
     printf("Jog killed\n\r"); 
 }
 
@@ -608,7 +655,7 @@
     int zero = 0;
     int machine_zero = 0;
     
-    printf("\n\nReceiving...\n\r");
+    //printf("\n\nReceiving...\n\r");
     
     t.reset();
     
@@ -620,7 +667,7 @@
         
         recv = ihm.getc();
         
-        printf("%c\n\r", recv);
+        //printf("%c\n\r", recv);
         
         if(recv == 'x'){
             if(save){
@@ -640,9 +687,7 @@
                 cancelProgram();    
             } else if(zero){
                 if(machine_zero){
-                    zeroX(ppsMax);
-                    zeroY(ppsMax);
-                    zeroZ(ppsMax);
+                    goMachineHome(ppsMax);
                 } else if(last_recv == 'X'){
                     totalX = 0;
                 } else if(last_recv == 'Y'){
@@ -704,9 +749,92 @@
         
         if(zero && recv == 'm'){
             machine_zero = 1;
-        } else if(zero && recv != 'm'){
+        } else if(zero && recv != 'm' && recv != 'z'){
             last_recv = recv;    
         }
     }
     t.reset();
+}
+
+void goMachineHome(int ppsMax){
+    /*zeroX(ppsMax);
+    zeroY(ppsMax);
+    zeroZ(ppsMax);*/
+    printf("Machine Zero\n\r");
+    wait(0.1);
+    ihm_class.action_complete();
+}
+
+void joystickClick(void){
+    toggle = !toggle;    
+}
+
+void linear_interpolation(int x1, int y1, int x2, int y2){
+    int x = x1;
+    int y = y1;
+    
+    int dx = x2 - x1;
+    int dy = y2 - y1;
+    
+    int direction;
+    
+    printf("dx: %d      dy: %d\n\r", dx, dy);
+    
+    if(dx > 0 && dy > 0){
+        x_dir = x_plus;
+        y_dir = y_plus;
+        direction = 1;
+    } else if(dx > 0 && dy < 0){
+        x_dir = x_plus;
+        y_dir = y_minus;
+        direction = 1;
+        dy = dy*(-1);
+    } else if(dx < 0 && dy > 0){
+        x_dir = x_minus;
+        y_dir = y_plus;
+        direction = -1;
+        dx = dx*(-1);
+    } else if(dx < 0 && dy < 0){
+        x_dir = x_minus;
+        y_dir = y_minus;  
+        dx = dx*(-1);
+        dy = dy*(-1);  
+        direction = -1;
+    } else if(dy == 0){
+        if(dx > 0){
+            x_dir = x_plus;
+        } else if (dx < 0){
+            x_dir = x_minus;
+            dx = dx*(-1);
+        }
+        move(ppsMax, x_dir, y_dir, z_dir, dx, 0, 0);
+        return;
+    } else if(dx == 0){
+        if(dy > 0){
+            y_dir = y_plus;
+        } else if (dy < 0){
+            y_dir = y_minus;
+            dy = dy*(-1);
+        }
+        move(ppsMax, x_dir, y_dir, z_dir, 0, dy, 0);
+        return;
+    }
+    
+    int incI = 2*dy;
+    int incS = 2*(dy - dx);
+    
+    int d = 2*dy - dx;
+    
+    
+    while(x != x2){
+        if(d <= 0){
+            d = d + incI;
+            x = x + 1*direction;
+            move(ppsMax, x_dir, y_dir, z_dir, 1, 0, 0);
+        } else {
+            d = d + incS;
+            x = x + 1*direction;
+            move(ppsMax, x_dir, y_dir, z_dir, 1, 1, 0);
+        }
+    }        
 }
\ No newline at end of file