Diff: main.cpp

Revision:

27:3bbc354adea6

Parent:

26:f891ff6beb33

Child:

28:45a8ce39dd2b

diff -r f891ff6beb33 -r 3bbc354adea6 main.cpp
--- a/main.cpp	Thu May 17 23:08:43 2018 +0000
+++ b/main.cpp	Sun May 20 19:34:20 2018 +0000
@@ -52,8 +52,8 @@
 
 int x_plus = 0;
 int x_minus = 1;
-int y_plus = 0;
-int y_minus = 1;
+int y_plus = 1;
+int y_minus = 0;
 int z_plus = 0;
 int z_minus = 1;
 
@@ -69,7 +69,7 @@
 int z_block_max = 0;
 int z_block_min = 0;
 
-float ppsMax = 1000.0;
+float ppsMax = 1650.0;
 
 int totalX = 0;
 int totalY = 0;
@@ -99,12 +99,6 @@
 float step_mode = 0.5;
 float steps_by_rotation = motor_steps_by_rotation/step_mode;
 
-int saved = 0;
-const int maxPoints = 10;
-float points[3][maxPoints]; // [0] X; [1] Y; [2] Z;
-float path[2][maxPoints]; // [0] speed; [1] mode;
-int resting_points = 0;
-
 int kill_jog;
 
 // PROTOTYPE FUNCTIONS -------------------------------------------------------------------------------
@@ -114,6 +108,7 @@
 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);
+void moveZ(int, int);
 
 float steps_to_distance(int, float);
 int distance_to_steps(float, float);
@@ -132,6 +127,8 @@
 void zeroY(int);
 void zeroZ(int);
 
+void virtualZero(void);
+
 void savePoint(void);
 void startSavedPoints(void);
 
@@ -142,6 +139,9 @@
 void joystickClick(void);
 
 void linear_interpolation(int x1, int y1, int x2, int y2);
+void circular_interpolation(int, int, int);
+
+void test_direction(void);
 
 // MAIN PROGRAM ----------------------------------------------------------------------------------------------
 int main(){
@@ -162,21 +162,18 @@
     
     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);
-    }
+    //test_direction();
     
-    joyClick.fall(&joystickClick);
-    /*
+    //circular_interpolation(400, 200, 200);
+    
+    //joyClick.fall(&joystickClick);
+    
     while(1){
         if(ihm.readable()){
             readSerial();    
         }   
          
-    }*/
+    }
 }  
 
 // FUNCTIONS ----------------------------------------------------------------
@@ -534,79 +531,66 @@
         }
     }
 }
+ 
+void virtualZero(void){
+    X = steps_to_distance(totalX, xPitch);
+    Y = steps_to_distance(totalY, yPitch);
+    Z = steps_to_distance(totalZ, zPitch);
+    
+    int x_start = distance_to_steps(X, xPitch);
+    int x_end = 0;
+    int y_start = distance_to_steps(Y, yPitch);
+    int y_end = 0;
+    int z_start = distance_to_steps(Z, zPitch);
+    int z_end = 0;
+    
+    int z_dist = z_end - z_start;
+    
+    if(z_dist < 0){
+        z_dir = z_minus;
+        z_dist = z_dist*(-1);
+    } else if(z_dist > 0){
+        z_dir = z_plus;
+    }
+    
+    move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_dist);
+    linear_interpolation(x_start, y_start, x_end, y_end);
+}  
             
 void automatic_run(int total_points){
+    int x_start;
+    int x_end;
+    int y_start;
+    int y_end;
+    int z_start;
+    int z_end;
+    
+    int z_dist;
+    
     enable = 0;
     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");
+    virtualZero();
     
     // 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];
+    for(int i = 1; i <= total_points; 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);
         
-        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; }
+        y_start = distance_to_steps(points[1][i-1], yPitch);
+        y_end = distance_to_steps(points[1][i], yPitch);
         
-        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; }
+        z_start = distance_to_steps(points[2][i-1], zPitch);
+        z_end = distance_to_steps(points[2][i], zPitch);
+        
+        z_dist = z_end - z_start;
         
-        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);
+        moveZ(ppsMax, z_dist);
+        linear_interpolation(x_start, y_start, x_end, y_end);
     }
+    
     killJog();
     printf("Done\n\r");
     wait(0.1);
@@ -614,6 +598,19 @@
     enable = 1;
 }
 
+void moveZ(int speed, int z_dist){
+     if(z_dist < 0){
+        z_dir = z_minus;
+        z_dist = z_dist*(-1);
+    } else if(z_dist > 0){
+        z_dir = z_plus;
+    }
+    
+    printf("dz: %f\n\r", steps_to_distance(z_dist, zPitch));
+    
+    move(speed, x_plus, y_plus, z_dir, 0, 0, z_dist); 
+}
+
 void killJog(void){
     enable = 1;
     kill_jog = 1; 
@@ -631,6 +628,8 @@
     char recv;
     char last_recv;
     
+    float timeoutTime = 0.2;
+    
     //save
     int save = 0;
     int mode = 0;
@@ -655,19 +654,26 @@
     int zero = 0;
     int machine_zero = 0;
     
-    //printf("\n\nReceiving...\n\r");
+    //upload program
+    int upload = 0;
+    string upload_buffer;
+    
+    //send saved
+    int send_saved = 0;
+    
+    printf("\n\nReceiving...\n\r");
     
     t.reset();
     
     while(1){
-        if(t.read()>0.2){
+        if(t.read() > timeoutTime){
             printf("Timeout\n\r");
             break;
         }
         
         recv = ihm.getc();
         
-        //printf("%c\n\r", recv);
+        printf("%c\n\r", recv);
         
         if(recv == 'x'){
             if(save){
@@ -695,6 +701,13 @@
                 } else if(last_recv == 'Z'){
                     totalZ = 0;
                 }
+            } else if(upload){
+                printf("Decoding GCODE\r\n");
+                ihm_class.read_gcode(upload_buffer);
+                wait_ms(10);
+                ihm_class.action_complete();                   
+            } else if(send_saved){
+                ihm_class.sendSaved();    
             }
             break;
         }
@@ -723,6 +736,15 @@
             zero = 1; 
         }
         
+        if(recv == 'u'){
+            timeoutTime = 1.0;
+            upload = 1;    
+        }
+        
+        if(recv == 'l'){
+            send_saved = 1;   
+        }
+        
         if(save){
             if(recv == 'M'){
                 mode = 1;
@@ -752,6 +774,10 @@
         } else if(zero && recv != 'm' && recv != 'z'){
             last_recv = recv;    
         }
+        
+        if(upload && recv != 'u'){
+            upload_buffer.push_back(recv); 
+        }
     }
     t.reset();
 }
@@ -776,30 +802,35 @@
     int dx = x2 - x1;
     int dy = y2 - y1;
     
-    int direction;
+    int direction_x;
+    int direction_y;
     
-    printf("dx: %d      dy: %d\n\r", dx, dy);
+    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;
-        direction = 1;
+        direction_x = 1;
+        direction_y = 1;
     } else if(dx > 0 && dy < 0){
         x_dir = x_plus;
         y_dir = y_minus;
-        direction = 1;
+        direction_x = 1;
+        direction_y = -1;
         dy = dy*(-1);
     } else if(dx < 0 && dy > 0){
         x_dir = x_minus;
         y_dir = y_plus;
-        direction = -1;
+        direction_x = -1;
+        direction_y = 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;
+        direction_x = -1;
+        direction_y = 1;
     } else if(dy == 0){
         if(dx > 0){
             x_dir = x_plus;
@@ -820,21 +851,87 @@
         return;
     }
     
-    int incI = 2*dy;
-    int incS = 2*(dy - dx);
+    
+    if(dx >= dy){
+        int incI = 2*dy;
+        int incS = 2*(dy - dx);
+        
+        int d = (2*dy - dx);
     
-    int d = 2*dy - dx;
-    
+        while(x != x2){        
+            if(d < 0){
+                d = d + incI;
+                x = x + 1*direction_x;
+                move(ppsMax, 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);
+            }
+        }  
+    } else {
+        int incI = 2*dx;
+        int incS = 2*(dx - dy);
+        
+        int d = (2*dx - dy);
     
-    while(x != x2){
-        if(d <= 0){
-            d = d + incI;
-            x = x + 1*direction;
-            move(ppsMax, x_dir, y_dir, z_dir, 1, 0, 0);
+        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);
+            } 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);
+            }
+        }
+    }
+    printf("x: %d      y: %d\n\r", x, y);      
+}
+
+void circular_interpolation(int r, int xc, int yc){
+    printf("Circular\n\r");
+    int x = 0;
+    int y = r;
+    int d = (1-r);
+    
+    while(y >= x){
+        if(d < 0){
+            d = (d + 2*x + 3);
+            x = x + 1;
+            move(ppsMax, x_plus, y_plus, z_plus, 1, 0, 0);
         } else {
-            d = d + incS;
-            x = x + 1*direction;
-            move(ppsMax, x_dir, y_dir, z_dir, 1, 1, 0);
+            d = (d + 2*(x-y) + 5);
+            x = x + 1;
+            y = y - 1;
+            move(ppsMax, x_plus, y_minus, z_plus, 1, 1, 0);
         }
-    }        
+                
+    }  
+    while(y <= x){
+        if(d < 0){
+            d = (d + 2*x + 3);
+            x = x + 1;
+            move(ppsMax, x_plus, y_plus, z_plus, 1, 0, 0);
+        } else {
+            d = (d + 2*(x-y) + 5);
+            x = x + 1;
+            y = y - 1;
+            move(ppsMax, x_plus, y_minus, z_plus, 1, 1, 0);
+        }
+                
+    }  
+}
+
+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);
+    
+    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);
 }
\ No newline at end of file