Diff: main.cpp

Revision:

23:d64d37561b4a

Parent:

22:000890b38b32

Child:

24:92f612ca218b

--- a/main.cpp	Mon May 14 04:04:56 2018 +0000
+++ b/main.cpp	Tue May 15 15:31:29 2018 +0000
@@ -7,6 +7,7 @@
 #define rx PC_11
 
 Serial ihm(tx, rx); //tx e rx (D1 e D0)
+Timer t;
 
 // drivers output signal
 DigitalOut enable(D2);
@@ -26,20 +27,17 @@
 InterruptIn endY(A3);
 InterruptIn endZ(A4);
 
-// IHM for development
+InterruptIn user(USER_BUTTON);
+
+// JOG Controls
 AnalogIn joyX(A0);
 AnalogIn joyY(A1);
 
 DigitalIn zUp(D15);
 DigitalIn zDwn(D14);
 
-InterruptIn startBtn(D12);
-
-// switch to alternate between JOG and routine
-DigitalIn botao(D11);
-
 // variables definition
-int modeStatus;
+int modeStatus = 1;
 
 float valX;
 float valY;
@@ -114,8 +112,16 @@
 int exitSavedPointsLoop = 0;
 int keepSavedPointsReload = 0;
 
+int kill_jog;
+
 // PROTOTYPE FUNCTIONS -------------------------------------------------------------------------------
-void setupPins();
+void jog(void);
+void killJog(void);
+
+void x(void);
+
+void automatic_run(int);
+
 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);
@@ -138,19 +144,15 @@
 
 void printDistance(void);
 
-void rxCallback(void);
+void readSerial(void);
 
 // MAIN PROGRAM ----------------------------------------------------------------------------------------------
-
-char buffer[8] = "Hello\r";
-
 int main(){
     printf("\nStarting...\r\n");
     
     enable = 0;
     
-    // interrupção serial
-    ihm.attach(&rxCallback, Serial::RxIrq);
+    user.rise(&x);
     
     //  interrupções de fim de curso
     /*
@@ -161,204 +163,20 @@
     endY.rise(&endY_release);
     
     endZ.fall(&endZ_press);
-    endZ.rise(&endZ_release);
-
-    startBtn.rise(&startSavedPoints);*/
-
-    int contador = 0;
-    
-    // loop de movimento 
-    while(1){
-        // movimento em JOG e salvamento dos pontos
-        if(modeStatus){
-            autoReload = 0;
-            saved = 0;
-            startSaved = 0;
-            
-            //Código de JOG
-            printf("JOG Selected\n\r");
-            while(1){        
-                valX = joyX;
-                valY = joyY;
-                
-                valZUp = zUp;
-                valZDwn = zDwn;
-                
-                if(valX > 0.7){
-                    if(x_dir != x_plus){
-                        x_dir = x_plus;
-                    }
-                    activeX = 1;     
-                }
-                else if(valX < 0.3){
-                    if(x_dir != x_minus){
-                        x_dir = x_minus;   
-                    }
-                    activeX = 1;  
-                }
-                else{
-                    activeX = 0;
-                }
-                
-                //----------------------------------------------------------------------
-                
-                if(valY > 0.7){
-                    if(y_dir != y_plus){
-                        y_dir = y_plus;   
-                    }
-                    activeY = 1;  
-                }
-                else if(valY < 0.3){
-                    if(y_dir != y_minus){
-                        y_dir = y_minus;    
-                    }
-                    activeY = 1;  
-                }
-                else{
-                    activeY = 0;
-                }
-                
-                //----------------------------------------------------------------------   
-                
-                if(!valZUp && valZDwn){
-                    if(z_dir != z_minus){
-                        z_dir = z_minus;    
-                    }
-                    activeZ = 1;   
-                }
-                else if(!valZDwn && valZUp){
-                    if(z_dir != z_plus){
-                        z_dir = z_plus;   
-                    }
-                    activeZ = 1;   
-                }
-                else {
-                    activeZ = 0;    
-                }
-                
-                //----------------------------------------------------------------------
+    endZ.rise(&endZ_release);*/
     
-                move(ppsMax, 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(contador > 10000){
-                    printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
-                    contador = 0;
-                }
-                
-                contador+=1;
-                
-                if(startSaved){
-                    break;
-                }
-            }
-            
-            // loop for executing saved points
-            while(1){
-                printf("\nStarting saved points\n\r");
-                
-                // home
-                x_distance_to_run = points[0][0] - points[0][saved-1];
-                y_distance_to_run = points[1][0] - points[1][saved-1];
-                z_distance_to_run = points[2][0] - points[2][saved-1]; 
-                
-                printf("\nGoing to first point\n\r");
-                
-                if(x_distance_to_run < 0){
-                        x_dir = x_minus;
-                        x_distance_to_run = -1.0*x_distance_to_run;
-                    } else { x_dir = x_plus; }
-                    
-                    if(y_distance_to_run < 0){
-                        y_dir = y_minus;
-                        y_distance_to_run = -1.0*y_distance_to_run;
-                    } else { y_dir = y_plus; }
-                    
-                    if(z_distance_to_run < 0){
-                        z_dir = z_minus;
-                        z_distance_to_run = -1.0*z_distance_to_run;
-                    } else { z_dir = z_plus; }
-                    
-                x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch); 
-                y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch); 
-                z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch);
-                
-                printf("deltaX: %.2f        deltaY: %.2f            deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
-                
-                move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0);
-                move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0);
-                move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run);
-                
-                wait(2);
-                
-                printf("\nSaved points\n\r");
-                
-                // routine
-                
-                for(int i = 1; i < saved; i++){
-                    x_distance_to_run = points[0][i]-points[0][i-1];
-                    y_distance_to_run = points[1][i]-points[1][i-1];
-                    z_distance_to_run = points[2][i]-points[2][i-1];
-                    
-                    printf("deltaX: %.2f        deltaY: %.2f            deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
-                    
-                    if(x_distance_to_run < 0){
-                        x_dir = x_minus;
-                        x_distance_to_run = -1.0*x_distance_to_run;
-                    } else { x_dir = x_plus; }
-                    
-                    if(y_distance_to_run < 0){
-                        y_dir = y_minus;
-                        y_distance_to_run = -1.0*y_distance_to_run;
-                    } else { y_dir = y_plus; }
-                    
-                    if(z_distance_to_run < 0){
-                        z_dir = z_minus;
-                        z_distance_to_run = -1.0*z_distance_to_run;
-                    } else { z_dir = z_plus; }
-                    
-                    x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch); 
-                    y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch); 
-                    z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch);
-                    
-                    move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0);
-                    move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0);
-                    move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run);
-                }
-                
-                printf("Done\n\r");
-                
-                printf("\nTo restart press blue, to return to JOG press yellow.\n\r");
-                
-                autoReload = 1;
-                exitSavedPointsLoop = 0;
-                keepSavedPointsReload = 0;
-                
-                while(1){
-                    if(exitSavedPointsLoop){
-                        printf("Exiting...");
-                        break;
-                    } else if(keepSavedPointsReload){
-                        printf("Restarting...\n\r");
-                        exitSavedPointsLoop = 0;
-                        break;    
-                    }
-                
-                    wait_ms(10);
-                }
-                if(exitSavedPointsLoop){
-                    break;
-                }                
-            }  
-        } else {
-            // specified routine
-        }
+    while(1){
+        if(ihm.readable()){
+            readSerial();    
+        }    
     }
 }  
 
 // FUNCTIONS ----------------------------------------------------------------
+void x(void){
+    printf("User\n\r");    
+}
+
 void endX_press(void){
     printf("X - press\n\r");
     if(dirX == x_plus){
@@ -567,7 +385,7 @@
         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\n\r", resting_points);
+        printf("Pontos restantes: %d\n\r", resting_points);
     }  
     else { 
         exitSavedPointsLoop = 1;
@@ -575,20 +393,175 @@
     }   
 }
 
-void startSavedPoints(void){
-    if(!autoReload){
-        startSaved = 1; 
-        keepSavedPointsReload = 0;
-        exitSavedPointsLoop = 0;
-    } else {
-        keepSavedPointsReload = 1;
-        exitSavedPointsLoop = 0;    
-    } 
+void jog(void){
+    kill_jog = 0;
+    int contador = 0;
+    printf("JOG Selected %d\n\r", kill_jog);
+    
+    while(1){  
+        if(ihm.readable()){
+            readSerial();    
+        }      
+        valX = joyX;
+        valY = joyY;
+        
+        valZUp = zUp;
+        valZDwn = zDwn;
+        
+        if(valX > 0.7){
+            if(x_dir != x_plus){
+                x_dir = x_plus;
+            }
+            activeX = 1;     
+        }
+        else if(valX < 0.3){
+            if(x_dir != x_minus){
+                x_dir = x_minus;   
+            }
+            activeX = 1;  
+        }
+        else{
+            activeX = 0;
+        }
+        
+        //----------------------------------------------------------------------
+        
+        if(valY > 0.7){
+            if(y_dir != y_plus){
+                y_dir = y_plus;   
+            }
+            activeY = 1;  
+        }
+        else if(valY < 0.3){
+            if(y_dir != y_minus){
+                y_dir = y_minus;    
+            }
+            activeY = 1;  
+        }
+        else{
+            activeY = 0;
+        }
+        
+        //----------------------------------------------------------------------   
+        
+        if(!valZUp && valZDwn){
+            if(z_dir != z_minus){
+                z_dir = z_minus;    
+            }
+            activeZ = 1;   
+        }
+        else if(!valZDwn && valZUp){
+            if(z_dir != z_plus){
+                z_dir = z_plus;   
+            }
+            activeZ = 1;   
+        }
+        else {
+            activeZ = 0;    
+        }
+        
+        //----------------------------------------------------------------------
+
+        move(ppsMax, 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(contador > 10000){
+            printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
+            contador = 0;
+        }
+        
+        contador+=1;
+        
+        if(kill_jog){
+            break;
+        }
+    }
+}
+            
+void automatic_run(int total_points){
+    printf("\nStarting automatic routine\n\r");
+    
+    // home
+    x_distance_to_run = points[0][0] - points[0][total_points-1];
+    y_distance_to_run = points[1][0] - points[1][total_points-1];
+    z_distance_to_run = points[2][0] - points[2][total_points-1]; 
+    
+    printf("\nGoing to first point\n\r");
+    
+    if(x_distance_to_run < 0){
+            x_dir = x_minus;
+            x_distance_to_run = -1.0*x_distance_to_run;
+        } else { x_dir = x_plus; }
+        
+        if(y_distance_to_run < 0){
+            y_dir = y_minus;
+            y_distance_to_run = -1.0*y_distance_to_run;
+        } else { y_dir = y_plus; }
+        
+        if(z_distance_to_run < 0){
+            z_dir = z_minus;
+            z_distance_to_run = -1.0*z_distance_to_run;
+        } else { z_dir = z_plus; }
+        
+    x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch); 
+    y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch); 
+    z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch);
+    
+    printf("deltaX: %.2f        deltaY: %.2f            deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
+    
+    move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0);
+    move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0);
+    move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run);
+    
+    wait(2);
+    
+    printf("\nSaved points\n\r");
+    
+    // routine
+    
+    for(int i = 1; i < total_points; i++){
+        x_distance_to_run = points[0][i]-points[0][i-1];
+        y_distance_to_run = points[1][i]-points[1][i-1];
+        z_distance_to_run = points[2][i]-points[2][i-1];
+        
+        printf("deltaX: %.2f        deltaY: %.2f            deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
+        
+        if(x_distance_to_run < 0){
+            x_dir = x_minus;
+            x_distance_to_run = -1.0*x_distance_to_run;
+        } else { x_dir = x_plus; }
+        
+        if(y_distance_to_run < 0){
+            y_dir = y_minus;
+            y_distance_to_run = -1.0*y_distance_to_run;
+        } else { y_dir = y_plus; }
+        
+        if(z_distance_to_run < 0){
+            z_dir = z_minus;
+            z_distance_to_run = -1.0*z_distance_to_run;
+        } else { z_dir = z_plus; }
+        
+        x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch); 
+        y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch); 
+        z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch);
+        
+        move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0);
+        move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0);
+        move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run);
+    }
+    killJog();
+    printf("Done\n\r");
 }
 
-void rxCallback(void){
-    int first_rec_c = 1;
-    char mode_c;
+void killJog(void){
+    saved = 0;
+    kill_jog = 1; 
+    printf("Jog killed\n\r"); 
+}
+
+void readSerial(void){
     char recv;
     
     //save
@@ -598,19 +571,40 @@
     string mode_recv;
     string speed_str;
     
-    printf("Receiving...\n\r");
+    //begin
+    int begin = 0;
+    
+    //jog
+    int jog_mode = 0;
+    int kill_jog_mode = 0;
+    
+    printf("\n\nReceiving...\n\r");
+    
+    t.reset();
+    t.start();
     
     while(1){
+        if(t.read()>0.2){
+            printf("Timeout\n\r");
+            break;
+        }
+        
         recv = ihm.getc();
         
-        if(first_rec_c && recv != 'x'){
-            mode_c = recv;
-            first_rec_c = 0;       
-        } else if(recv == 'x'){
+        printf("%c\n\r", recv);
+        
+        if(recv == 'x'){
             if(save){
-                printf("Saved: mode->%s, speed->%s\n\r", mode_recv, speed_str);
                 savePoint(speed_str, mode_recv);
-            }            
+            } else if(begin){
+                automatic_run(saved);    
+            } else if(jog_mode){
+                if(kill_jog_mode){
+                    killJog();
+                } else {
+                    jog();    
+                }
+            }        
             break;
         }
         
@@ -618,6 +612,14 @@
             save = 1;
         }
         
+        if(recv == 'b'){
+            begin = 1;    
+        }
+        
+        if(recv == 'j'){
+            jog_mode = 1;    
+        } 
+        
         if(save){
             if(recv == 'M'){
                 mode = 1;
@@ -637,5 +639,9 @@
                 }
             }
         }
+        
+        if(jog_mode && recv == 'k'){
+            kill_jog_mode = 1;    
+        }
     }
 }
\ No newline at end of file