File content as of revision 36:4d0e131146e6:

// LIBs ------------------------------------------------------------------------------------
#include "mbed.h"
#include "string"
#include "classes.h"

// MACHINE SETUP ---------------------------------------------------------------------------
Timer t;
IHM ihm_class;

// drivers output signal
DigitalOut enable(D2);

DigitalOut dirX(D3);
DigitalOut stepX(D4);

DigitalOut dirY(D5);
DigitalOut stepY(D6);

DigitalOut dirZ(D7);
DigitalOut stepZ(D8);

DigitalOut ledXY(D10);
DigitalOut ledZ(D11);

DigitalOut glueValve(D9);

// hardware input signal
// end-of-stroke sensors
InterruptIn endX(A2);
InterruptIn endY(A1);
InterruptIn endZ(A0);

// JOG Controls
AnalogIn joyX(A5);
AnalogIn joyY(A4);

// variables definition
int modeStatus = 1;

float valX;
float valY;

int toggle = 0;

int activeX = 1;
int activeY = 1;
int activeZ = 1;

int x_dir;
int y_dir;
int z_dir;

int x_plus = 0;
int x_minus = 1;
int y_plus = 1;
int y_minus = 0;
int z_plus = 0;
int z_minus = 1;

int x_limit = 0;
int x_block_max = 0;
int x_block_min = 0;

int y_limit = 0;
int y_block_max = 0;
int y_block_min = 0;

int z_limit = 0;
int z_block_max = 0;
int z_block_min = 0;

float ppsMax = 1500.0;
float glue_time = 1.0;

int totalX = 0;
int totalY = 0;
int totalZ = 0;

int x_steps_to_run;
int y_steps_to_run;
int z_steps_to_run;

int cleaning_glue_status = 0;

float x_distance_to_run;
float y_distance_to_run;
float z_distance_to_run;

float X = 0;
float Y = 0;
float Z = 0;

int endX1Status;
int endX2Status;

// machine parameters
float xPitch = 3.0;
float yPitch = 3.0;
float zPitch = 6.0;

float motor_steps_by_rotation = 200.0;
float step_mode = 0.5;
float steps_by_rotation = motor_steps_by_rotation/step_mode;

int kill_jog;

int emerg_stop = 0;

int gcode_move_upload = 0;

// PROTOTYPE FUNCTIONS -------------------------------------------------------------------------------
void jog(void);
void killJog(void);

void automatic_run(int, bool);

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);

void endX_press(void);
void endX_release(void);

void endY_press(void);
void endY_release(void);

void endZ_press(void);
void endZ_release(void);

void goMachineHome(int);
void zeroX(int);
void zeroY(int);
void zeroZ(int);

void virtualZero(void);

void savePoint(void);
void startSavedPoints(void);

void printDistance(void);

void readSerial(void);

void joystickClick(void);

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);

void moving_test(void);

void clean_glue(void);

// MAIN PROGRAM ----------------------------------------------------------------------------------------------
int main(){
    ihm.baud(9600);
    printf("\nStarting...\r\n");
    
    enable = 1;
    glueValve = 0;
    
    t.start();
    
    //  interrupções de fim de curso
    
    endX.fall(&endX_press);
    endX.rise(&endX_release);
    
    endY.fall(&endY_press);
    endY.rise(&endY_release);
    
    endZ.fall(&endZ_press);
    endZ.rise(&endZ_release);
    
    joyClick.fall(&joystickClick);
    
    while(1){
        if(ihm.readable()){
            readSerial();    
        }   
         
    }
}  

// FUNCTIONS ----------------------------------------------------------------

void endX_press(void){
    printf("X - press\n\r");
    if(dirX == x_plus){
        x_block_max = 1;
    }
    else {
        x_block_min = 1;
    }
}
void endX_release(void){
    printf("X - release\n\r");
    x_block_min = 0;
    x_block_max = 0;
}

void endY_press(void){
    printf("Y - press\n\r");  
    if(dirY == y_plus){
        y_block_max = 1;
    }
    else {
        y_block_min = 1;
    } 
}
void endY_release(void){
    printf("Y - release\n\r");
    y_block_min = 0;   
    y_block_max = 0;
}

void endZ_press(void){
    printf("Z - press\n\r");
    toggle = !toggle;
    if(dirZ == z_plus){
        z_block_max = 1;
    }
    else {
        z_block_min = 1;
    }
}
void endZ_release(void){
    //printf("Z - release\n\r");
    z_block_min = 0;
    z_block_max = 0;
}

void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step){
    float time = 1.0/pps/2.0;
    
    dirX = x_dir;
    dirY = y_dir;
    dirZ = z_dir;
    
    // X
    if((x_dir == x_plus) && x_block_max){
        x_step = 0;    
    }
    if((x_dir == x_minus) && x_block_min){
        x_step = 0;    
    }
    
    // Y
    if((y_dir == y_plus) && y_block_max){
        y_step = 0;    
    }
    if((y_dir == y_minus) && y_block_min){
        y_step = 0;    
    }
    
    // Z
    if((z_dir == z_plus) && z_block_max){
        z_step = 0;    
    }
    if((z_dir == z_minus) && z_block_min){
        z_step = 0;    
    }
    
    int max_val;
    
    // maior número de passos -------
    if(x_step > y_step){
        if(x_step > z_step){
            max_val = x_step;
        }
        else {
            max_val = z_step;
        }
    } else if(y_step > z_step){
        max_val = y_step;
    }
    else {
        max_val = z_step;    
    }
    
    for(int i = 0; i < max_val; i++){
        if(ihm.readable()){
            readSerial();    
        }
        if(emerg_stop){
            break;
        }
        if(i >= x_step){
            stepX = 0;
        }
        else { stepX = 1; }
        
        if(i >= y_step){
            stepY = 0;
        }
        else {
            stepY = 1;}
        
        if(i >= z_step){
            stepZ = 0;
        }
        else { stepZ = 1; }
        
        wait(time);
        stepX = 0;
        stepY = 0;
        stepZ = 0;
        wait(time); 
    }

    if(dirX == x_minus){
        totalX-=x_step;
    }
    else { totalX+=x_step; }
    
    if(dirY == y_minus){
        totalY-=y_step;
    } else { totalY+=y_step; }
    
    if(dirZ == z_minus){
        totalZ-=z_step;
    } else { totalZ+=z_step; }
}

void zeroX(int pps){
    printf("Zero X\n\r");
    float time = 1.0/pps/2.0;
    
    int offsetX;
    
    if(dirX != x_minus){
        dirX = x_minus;    
    }
    
    while(!x_block_min){
        if(ihm.readable()){
            readSerial();    
        }
        if(emerg_stop){
            break;    
        }
        stepX = 1;
        wait(time);
        stepX = 0;
        wait(time);  
    }
    
    offsetX = distance_to_steps(10.0, xPitch);
    
    move(ppsMax, x_plus, y_plus, z_plus, offsetX, 0, 0);
    
    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");
    float time = 1.0/pps/2.0;
    
    int offsetY;
    
    if(dirY != y_minus){
        dirY = y_minus;    
    }
    
    while(!y_block_min){
        if(ihm.readable()){
            readSerial();    
        }
        if(emerg_stop){
            break;    
        }
        stepY = 1;
        wait(time);
        stepY = 0;
        wait(time);  
    }
    
    offsetY = distance_to_steps(10.0, yPitch);
    
    move(ppsMax, x_plus, y_plus, z_plus, 0, offsetY, 0);
    
    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){
    printf("Zero Z\n\r");
    float time = 1.0/pps/2.0;
    
    int offsetZ;
    
    if(dirZ != z_minus){
        dirZ = z_minus;    
    }
    
    while(!z_block_min){
        if(ihm.readable()){
            readSerial();    
        }
        if(emerg_stop){
            break;    
        }
        stepZ = 1;
        wait(time);
        stepZ = 0;
        wait(time);  
    }
    
    offsetZ = distance_to_steps(5.0, yPitch);
    
    move(ppsMax, x_plus, y_plus, z_plus, 0, 0, offsetZ);
    
    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){
    float delta_S = (((float)steps)/steps_by_rotation)*pitch;
    return delta_S;
}

int distance_to_steps(float distance, float pitch){
    int steps = (int)(distance*steps_by_rotation/pitch);
    return steps;
}

void printDistance(void){
    X = steps_to_distance(totalX, xPitch);
    Y = steps_to_distance(totalY, yPitch);
    Z = steps_to_distance(totalZ, zPitch);
    printf("X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
}

void savePoint(string speed, string mode){
    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);
     
    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] = 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", 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);     
    
    //ihm_class.send_position(X, Y, Z);
}

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();    
        }    
        if(emerg_stop){
            break;    
        }  
        valX = joyX;
        valY = joyY;
        
        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.3 && !toggle){
            if(x_dir != x_minus){
                x_dir = x_minus;   
            }
            //speed = ppsMax*valX/0.48;
            activeX = 1;  
        }
        else{
            activeX = 0;
        }
        
        //----------------------------------------------------------------------
        
        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.3 && !toggle){
            if(y_dir != y_minus){
                y_dir = y_minus;  
            }
            //speed = ppsMax*valY/0.48;  
            activeY = 1;  
        }
        else{
            activeY = 0;
        }
        
        //----------------------------------------------------------------------   
        
        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.3 && toggle){
            if(z_dir != z_minus){
                z_dir = z_minus;  
            }
            activeZ = -1; 
            //speed = ppsMax*(valY - 0.52)/0.48;    
        }
        else {
            activeZ = 0;    
        }
        
        /*
        if(speed < 100.0){
            speed = 100.0;
        }*/
        
        //printf("%f\n\r", speed);
        //----------------------------------------------------------------------

        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);
        
        /*
        if(t.read() > sendTime){
            //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 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);
    
    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;
    }
    
    linear_interpolation(x_start, y_start, x_end, y_end, ppsMax);
    move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_dist);
}  
            
void automatic_run(int total_points, bool virtual_zero_first){
    int x_start;
    int x_end;
    int y_start;
    int y_end;
    int z_start;
    int z_end;
    
    int z_dist;
    
    float mode;
    float speed;
    float glue;
    
    int glue_on_point = 0;
    
    enable = 0;
    
    printf("\nStarting automatic routine\n\r");
    
    if(virtual_zero_first){
        virtualZero();
    }
    
    wait(1);
    // routine
    
    printf("Total points: %d\n\r", total_points);
    
    for(int i = 0; i < total_points; i++){
        if(ihm.readable()){
            readSerial();    
        }
        if(emerg_stop){
            break;    
        }
        printf("Move n: %d\n\r", i);
        
        x_start = totalX;
        x_end = distance_to_steps(points[0][i], xPitch);
        
        y_start = totalY;
        y_end = distance_to_steps(points[1][i], yPitch);
        
        z_start = totalZ;
        z_end = distance_to_steps(points[2][i], zPitch);
        
        z_dist = z_end - z_start;
        
        mode = path[0][i];
        speed = path[1][i]*ppsMax;
        glue = path[2][i];
        
        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){
            glue_on_point = 1;
                
        } 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;
        
        if(glue_on_point){
            glueValve = 1;
            wait(glue_time);
            glueValve = 0;
        }
    }
    
    killJog();
    printf("Done\n\r");
    wait(0.1);
    ihm_class.action_complete();
    enable = 1;
    cancelProgram();
}

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*0.6, x_plus, y_plus, z_dir, 0, 0, z_dist); 
}

void killJog(void){
    enable = 1;
    kill_jog = 1; 
    ledZ = 0;
    ledXY = 0;
    printf("Jog killed\n\r"); 
}

void cancelProgram(void){
    saved = 0;
    killJog();    
}

void readSerial(void){
    char recv;
    char last_recv;
    
    float timeoutTime = 1.0;
    
    //save
    int save = 0;
    int mode = 0;
    int speed = 0;
    string mode_recv;
    string speed_str;
    
    //begin
    int begin = 0;
    
    //jog
    int jog_mode = 0;
    int kill_jog_mode = 0;
    
    //reset
    int reset = 0;
    
    //cancel
    int cancel = 0;
    
    //zero
    int zero = 0;
    int machine_zero = 0;
    
    //upload program
    int upload = 0;
    string upload_buffer;
    
    //send saved
    int send_saved = 0;
    
    //clean
    int clean = 0;
    
    //delete
    int delete_saved = 0;
    
    //emergencia
    int emerg_recv = 0;
    
    printf("\n\nReceiving...\n\r");
    
    t.reset();
    
    while(1){
        if(t.read() > timeoutTime){
            printf("Timeout\n\r");
            break;
        }
        
        recv = ihm.getc();
        
        printf("%c\n\r", recv);
        
        if(recv == 'x'){
            if(save){
                savePoint(speed_str, mode_recv);
            } else if(begin){
                printf("Automatic: %d\n\r", saved);
                if(gcode_move_upload){
                    automatic_run(saved, false);
                    gcode_move_upload = 0;
                    ihm_class.action_complete(); 
                } else {
                    automatic_run(saved, true);  
                    ihm_class.action_complete(); 
                } 
            } else if(jog_mode){
                if(kill_jog_mode){
                    killJog();
                } else {
                    jog();    
                }
            } else if(reset){
                NVIC_SystemReset();
            } else if(cancel){
                cancel = 1;
                cancelProgram();    
            } else if(zero){
                if(machine_zero){
                    goMachineHome(ppsMax);
                } else if(last_recv == 'X'){
                    totalX = 0;
                } else if(last_recv == 'Y'){
                    totalY = 0;
                } else if(last_recv == 'Z'){
                    totalZ = 0;
                }
            } else if(upload){
                ihm_class.read_gcode(upload_buffer);                
            } else if(send_saved){
                wait(1);
                ihm_class.sendSaved();
                return;
            } else if(clean){
                cleaning_glue_status = !cleaning_glue_status;
                clean_glue();
            } else if(delete_saved){
                saved = saved - 1;    
            } else if(emerg_recv){
                printf("Emergencia!\n\r");
                killJog();
                emerg_stop = 1;    
            } 
            else {
                printf("End\n\r");
                return;
            }
            return;
        }
        
        if(recv == 'e'){
            emerg_recv = 1;
        }
        
        if(recv == 'm' && !zero){
            gcode_move_upload = 1;
            timeoutTime = 10.0;
            upload = 1;
        }
        
        if(recv == 's'){
            save = 1;
        }
        
        if(recv == 'b'){
            begin = 1;    
        }
        
        if(recv == 'j'){
            jog_mode = 1;    
        } 
        
        if(recv == 'r'){
            reset = 1;
        }
        
        if(recv == 'c'){
            cancel = 1;    
        }
        
        if(recv == 'z'){
            zero = 1; 
        }
        
        if(recv == 'u'){
            timeoutTime = 10.0;
            upload = 1;    
        }
        
        if(recv == 'l'){
            send_saved = 1;   
        }
        
        if(recv == 'g'){
            clean = 1;
        }
        
        if(recv == 'd'){
            delete_saved = 1;    
        }
        
        if(save){
            if(recv == 'M'){
                mode = 1;
                speed = 0;
            } else if(recv == 'F'){
                mode = 0;
                speed = 1;
            } 
            
            if(mode){
                if(recv != 'M'){
                    mode_recv.push_back(recv);
                }  
            } else if(speed){
                if(recv != 'F'){
                    speed_str.push_back(recv);
                }
            }
        }
        
        if(jog_mode && recv == 'k'){
            kill_jog_mode = 1;    
        }
        
        if(zero && recv == 'm'){
            machine_zero = 1;
        } else if(zero && recv != 'm' && recv != 'z'){
            last_recv = recv;    
        }
        
        if(upload && recv != 'u' && recv != 'm'){
            upload_buffer.push_back(recv); 
        }
    }
    t.reset();
}

void goMachineHome(int ppsMax){
    enable = 0;
    zeroX(ppsMax);
    zeroY(ppsMax);
    zeroZ(ppsMax);
    printf("Machine Zero\n\r");
    wait(0.1);
    ihm_class.action_complete();
    enable = 0;
}

void joystickClick(void){
    toggle = !toggle;    
}

void linear_interpolation(int x1, int y1, int x2, int y2, float speed){
    int x = x1;
    int y = y1;
    
    int dx = x2 - x1;
    int dy = y2 - y1;
    
    int direction_x;
    int direction_y;
    
    if(dx > 0 && dy > 0){
        x_dir = x_plus;
        y_dir = y_plus;
        direction_x = 1;
        direction_y = 1;
    } else if(dx > 0 && dy < 0){
        x_dir = x_plus;
        y_dir = y_minus;
        direction_x = 1;
        direction_y = -1;
        dy = dy*(-1);
    } else if(dx < 0 && dy > 0){
        x_dir = x_minus;
        y_dir = y_plus;
        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_x = -1;
        direction_y = -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;
    }
    
    
    if(dx >= dy){
        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_x;
                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((int)speed, 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(y != y2){        
            if(d < 0){
                d = d + incI;
                y = y + 1*direction_y;
                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((int)speed, x_dir, y_dir, z_dir, 1, 1, 0);
            }
        }
    }    
}

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 + 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);
    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);
    moveZ(ppsMax, -1000);
}

void clean_glue(void){
    printf("Cleaning glue\n\r");
    killJog();
    if(cleaning_glue_status){ 
        printf("Activating\n\r");  
        glueValve = 1;
    } else {
        printf("Deactivating\n\r");
        glueValve = 0;    
    }
}