File content as of revision 9:bf4124478580:

#include "mbed.h"
// Juan
Serial pc (USBTX, USBRX);


AnalogIn eixo_Z(A2); 
InterruptIn posicao_salva(PA_8);

BusOut motor_y(PA_11, PB_12, PB_11, PB_2);
BusOut motor_z(PB_1, PB_15, PB_14, PB_13);

double frequencia; //Hz
double velocidade = 4; //RPM
double tempo; //Segundos
int i;
int X, Y, Z;
int pegaX, pegaY, pegaZ;
int posicao_salva_estado, botao_emergencia_estado;

void rotina_posicao_salva(void);
void rotina_emergencia(void);



int main()
{
    motor_x = 0x00;
    motor_y = 0x00;
    motor_z = 0x00;
    pc.baud(115200);
    posicao_salva.rise(&rotina_posicao_salva);
    botao_emergencia.rise(&rotina_emergencia);
    frequencia = (2048*velocidade)/60;
    tempo = (1/frequencia);

    while(1) {

        X = eixo_X.read_u16();
        Y = eixo_Y.read_u16();
        Z = eixo_Z.read_u16();
        posicao_salva_estado = posicao_salva.read();
        botao_emergencia_estado = botao_emergencia.read();

        pc.printf("X=%4d, Y=%4d, Z=%4d, PegaX=%4d, PegaY=%4d, PegaZ=%4d, Posicao_salva_estado=%d, Botao_emergencia_estado=%d \r\n", X, Y, Z, pegaX, pegaY, pegaZ, posicao_salva_estado, botao_emergencia_estado);

        if(31000 <= X &  X <= 35000) {

            motor_x = 0x00;
        } else {
            if (X > 31000) {
                for(i = 0; i < 4; i++) {
                    motor_x = 1 << i;
                    wait(tempo);
                }
            }

            if(X<35000) {
                for(i = 3; i > -1; i--) {
                    motor_x = 1 << i;
                    wait(tempo);
                }
            }
        }

        if(31000 <= Y &  Y <= 35000) {

            motor_y = 0x00;
        }

        else {
            if (Y > 31000) {
                for(i = 0; i < 4; i++) {
                    motor_y = 1 << i;
                    wait(tempo);
                }
            }

            if(Y < 35000) {
                for(i = 3; i > -1; i--) {
                    motor_y = 1 << i;
                    wait(tempo);
                }
            }
        }

        if(31000 <= X &  X <= 35000) {

            motor_z = 0x00;
        } else {
            if (X > 31000) {
                for(i = 0; i < 4; i++) {
                    motor_z = 1 << i;
                    wait(tempo);
                }
            }

            if(X<35000) {
                for(i = 3; i > -1; i--) {
                    motor_z = 1 << i;
                    wait(tempo);
                }
            }
        }

    }
}


void rotina_posicao_salva()
{
    if(posicao_salva_estado == 0) {

        pegaX = X;
        pegaY = Y;
        pegaZ = Z;

    } else {

        pegaX = 0;
        pegaY = 0;
        pegaZ = 0;

    }
}

void rotina_emergencia()
{
    if(botao_emergencia_estado == 0) {

        motor_x = 0x00;
        motor_y = 0x00;
        motor_z = 0x00;

    }
}