File content as of revision 21:c6c33381fc5f:

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

// MACHINE SETUP ---------------------------------------------------------------------------

// drivers output signal
DigitalOut enable(D2);

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

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

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

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

// IHM for development
AnalogIn joyX(A0);
AnalogIn joyY(A1);

DigitalIn zUp(D15);
DigitalIn zDwn(D14);

InterruptIn saveBtn(A5);
InterruptIn startBtn(D12);

// switch to alternate between JOG and routine
DigitalIn botao(D11);

// variables definition
int modeStatus;

float valX;
float valY;
int valZUp;
int valZDwn;

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 = 0;
int y_minus = 1;
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 = 600.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;

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 = 3.0;

float motor_steps_by_rotation = 200.0;
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];
int pontos_restantes = 0;

int startSaved = 0;
int autoReload = 0;
int exitSavedPointsLoop = 0;
int keepSavedPointsReload = 0;

// PROTOTYPE FUNCTIONS -------------------------------------------------------------------------------
void setupPins();
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);

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 zeroX(int pps);
void zeroY(int pps);

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

void printDistance(void);

// MAIN PROGRAM ----------------------------------------------------------------------------------------------

int main(){
    printf("\nStarting...\r\n");
    
    modeStatus = botao;
    
    enable = 0;
 
    //  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);
    
    saveBtn.rise(&savePoint);
    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;    
                }
                
                //----------------------------------------------------------------------
    
                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
        }
    }
}  

// 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");
    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(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){
    float time = 1.0/pps/2.0;
    
    if(dirX != x_minus){
        dirX = x_minus;    
    }
    
    while(!x_block_min){
        stepX = 1;
        wait(time);
        stepX = 0;
        wait(time);  
    }
    
    printf("X zero\n\r");
    
    totalX = 0;
}
void zeroY(int pps){
    float time = 1.0/pps/2.0;
    
    if(dirY != y_minus){
        dirY = y_minus;    
    }
    
    while(!y_block_min){
        stepY = 1;
        wait(time);
        stepY = 0;
        wait(time);  
    }
    
    printf("Y zero\n\r");
    
    totalY = 0;
}

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(void){
    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;
            
            saved+=1;
        }
        // save code
        printf("Saved --> X: %.2f     Y: %.2f     Z: %.2f\r\n", X, Y, Z);
        pontos_restantes = maxPoints-saved;
        printf("Pontos restantes: %d\n\n\r", pontos_restantes);
    }  
    else { 
        exitSavedPointsLoop = 1;
        keepSavedPointsReload = 0;
    }   
}

void startSavedPoints(void){
    if(!autoReload){
        startSaved = 1; 
        keepSavedPointsReload = 0;
        exitSavedPointsLoop = 0;
    } else {
        keepSavedPointsReload = 1;
        exitSavedPointsLoop = 0;    
    } 
}