File content as of revision 5:452d20780aac:

#include "mbed.h"
#include "TextLCD.h"

TextLCD lcd(D8, D9, D4, D5, D6, D7); //Comunicação com LCD
BusOut motor_x(PC_8, PC_6, PC_5, PA_12); //Pinos para acionamento do motor do eixo X
BusOut motor_y(PA_11, PB_12, PB_11, PB_2); //Pinos para acionamento do motor do eixo Y
BusOut motor_z(PB_1, PB_15, PB_14, PB_13); //Pinos para acionamento do motor do eixo Z
DigitalOut sinal(PC_10); // Pino para o acionamento da pipeta
//DigitalIn botao_seleciona(PC_3); //botao de SELECT geral

Serial pc (D1, D0); //Comunicação com USB
AnalogIn joystick(PC_2); //Joystick potenciometro
//AnalogIn botao_SELECT(A0); //Botão SELECT da IHM
//Teste
InterruptIn botao_seleciona(PA_3); //Botão de salva posição Joystick

InterruptIn botao_emergencia_in(PC_4); //Botão de emergência para detectar acionamento
InterruptIn botao_fim_curso_rise(PC_12); //Chave de fim de curso


Timer debounce1; //Correção para botão de posição de salva
Timer debounce2; //Correção para botão de emergência
Timer debounce3; //Correção para chave de fim de curso

int pulsos = 0; //Contagem dos pulsos para o motor

int i; //Variável para contar o incremento no motor
int cont_fim_curso = 0; //Variável para contar o incremento do fim de curso
int J_XYZ; //Variável para ler o potenciometro do Joystick
int passo_fuso = 5; //Declara o passo do fuso
int pegaZ; //Variável que receberá o valor da posição de salva
int SELECT; //Variável para leitura do botão SELECT da IHM
int contador_SELECT = 0; //Define o contator do SELECT da IHM
int contador_emergencia = 0; //Define o contator do botão de emergência

int contador_rise = 0;
int seguranca_fdc = 0;

// ------------------Acionamento da Pipeta------------------
// Valores de Teste
int total_pega = 6; // quantidade de mL na posição de pega disponível
int quant_desejada = 3; // quantidade de mL na posição de solta 1
//int npos_solta = [1,2,3,4];
int pos_solta_total = 4;
float posZ_atual = 3;
float posX_atual = 4;
float posY_atual = 5;
float posX0_desejada = 3;
float posY0_desejada = 5;
float posZ0_desejada = 4;
float posX_desejada = 2;
float posY_desejada = 6;
float posZ_desejada = 2;

float deslocamento_X = 0; //Define o deslocamento linear total
float deslocamento_Y = 0; //Define o deslocamento linear total
float deslocamento_Z = 0; //Define o deslocamento linear total

float deslocamento_max_X = 0; //Define o deslocamento linear total
float deslocamento_max_Y = 0; //Define o deslocamento linear total
float deslocamento_max_Z = 0; //Define o deslocamento linear total

float velocidade = 0.003; //Armazena a velocidade dos motores

int atual_pega = total_pega; // "total_pega" é a quantidade inserida pelo usuário na posição de pega
//int quant_desejada; // "quant desejada" é a quantidade inserida pelo usuário na posição de solta
int quant_depositada = 0;
int n_pulsosX;
int n_pulsosY;
int n_pulsosZ;
int posZ_origem = 0;
int deslocamentoZ_partida = posZ_origem - deslocamento_Z; // deslocamento do eixo Z até a posição de partida (origem)
int deslocamento_X0 = posX0_desejada - posX_atual; // deslocamento do eixo X até a posição de pega desejada
int deslocamento_Y0 = posY0_desejada - posY_atual; // deslocamento do eixo Y até a posição de pega desejada
int deslocamento_Z0 = posZ0_desejada - posZ_atual; // deslocamento do eixo Z até a posição de pega desejada
int deslocamento_solta_X = posX_desejada - posX_atual; // deslocamento do eixo X até a posição de solta desejada
int deslocamento_solta_Y = posY_desejada - posY_atual; // deslocamento do eixo Y até a posição de solta desejada
int deslocamento_solta_Z = posZ_desejada - posZ_atual; // deslocamento do eixo Z até a posição de solta desejada

// ------------------Acionamento da Pipeta------------------

float tempo_horario, tempo_anti_horario; //Define os tempos de acionamento do motor
float passo_motor = 5.625/32; //Declara o passo angular do motor
float tempo = 1.5; // Define o tempo que a pipeta fica acionada

int pulsos_Z = 0; //Contagem dos pulsos para o motor_Z
int pulsos_X = 0; //Contagem dos pulsos para o motor_X
int pulsos_Y = 0; //Contagem dos pulsos para o motor_Y

int contador_botao_seleciona = 0; //Boleana para a lógica do botão SELECT do joystick

bool valor_SELECT = false; //Boleana para lógica do botão SELECT da IHM
bool estado_referenciamento = false; //Boleana para identificar a necessidade do referênciamento
bool estado_botao_emergencia_in = false; //Boleana para identificar a necessidade do referênciamentosatisfazer a lógica da rotina_emergencia_in
bool motor_referenciamento = false; //Boleana para permitir o movimento automático do motot no referenciamento

// Rotinas do programa
void rotina_posicao_salva (void);
void rotina_emergencia_in (void);
void rotina_velocidade_eixo_Z (void);
void rotina_deslocamento_eixo_Z (void);
void rotina_deslocamento_eixo_menos_Z (void);
void rotina_JOG (void);
void rotina_botoes_IHM (void);
void Rotina_Pega (void);
void Rotina_Solta (void);
void teste2dora (void);
void reposiciona_pipeta_origem (void);

void rotina_fdc_rise (void);
void referenciamento (void);

void rotina_estado_botao_seleciona (void);

int main()
{
    pc.baud(115200); //Define a velocidade da porta USB
    motor_x = 0x00; //Estabelece a condição inicial do motor do eixo X
    motor_y = 0x00; //Estabelece a condição inicial do motor do eixo Y
    motor_z = 0x00; //Estabelece a condição inicial do motor do eixo Z

    //Declara os pinos de interrupção com suas funções
    botao_seleciona.rise(&rotina_estado_botao_seleciona);

    botao_emergencia_in.rise(&rotina_emergencia_in);
    botao_fim_curso_rise.rise(&rotina_fdc_rise);


    //Inicializa os timers
    debounce1.start();
    debounce2.start();
    debounce3.start();

    //Inicia o LCD
    wait_ms(500);
    lcd.locate(2,0);
    lcd.printf("SELECT para");
    lcd.locate(0,1);
    lcd.printf("referenciamento");

    while(1) {
        //Leitura de variáveis
        J_XYZ = joystick.read_u16();
        //SELECT = botao_SELECT.read_u16();
        //pc.printf("estado_referenciamento=%4d, estado_botao_emergencia_in=%d valor_SELECT=%d\r\n", estado_referenciamento, estado_botao_emergencia_in, valor_SELECT);
        //pc.printf("deslocamento_Z=%4f, pulsos=%d\r\n", deslocamento_Z, pulsos);
        //pc.printf("J_XYZ= %d\r\n", J_XYZ);
        if(estado_referenciamento == true & estado_botao_emergencia_in == false & contador_botao_seleciona == 2) {
            //Condição para poder disponibilizar a função JOG
            //Chama a rotina do JOG
            rotina_JOG();
            //LCD

            lcd.locate(2,0);
            lcd.printf("PosicaoZ:");
            lcd.locate(0,1);
            lcd.printf("%4fmm",deslocamento_Z);
            //pc.printf("J_XYZ= %d\r\n", J_XYZ);
        }

        else {
            //Chama a rotina dos botões da IHM
            //rotina_botoes_IHM();

            if(valor_SELECT == false) { //se estiver 0, mudar para 1 -- no PC da dora fica 0 pela LCD dela

                lcd.cls();
                lcd.locate(2,0);
                lcd.printf("Iniciando o");
                lcd.locate(0,1);
                lcd.printf("referenciamento");

                pc.printf("Referenciamento \r\n");

                contador_SELECT = 1;
                estado_referenciamento = false;
                //Permite o movimento automático do motor até atingir o fim de curso
                referenciamento();

                pc.printf("Referenciamento feito\r\n");
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Pressione SELECT");
                lcd.locate(0,1);
                lcd.printf("para continuar");

            }
        }

//========================================================================================

        //while (npos_solta < pos_solta_total) {
        //checa se existem mais posições de solta

        if(botao_seleciona == 1) {
            //pressiona para entrar nesse if
            // o usuario nao quer alterar nenhum parametro
            pc.printf("pressionado\r\n");

            quant_depositada = 0;

            while (quant_depositada < quant_desejada) {
                // "quant_desejada" definido pelo usuário na IHM

                if (atual_pega == 0) {
                    lcd.cls();
                    lcd.locate(0,0);
                    lcd.printf("Coleta vazia");
                    lcd.locate(0,1);
                    lcd.printf("Insira mais subst");
                    wait(4);
                    pc.printf("Coleta vazia \r\n");
                    exit(0); // obriga o usuário a recomeçar
                    //levanta_pipeta(); //void para levantar a pipeta até o zero
                    //ALTERAR: PIPETA VAI PARA A ORIGEM NO EIXO Z
                }
                    //referenciamento();
                    //break;
                else {
                    Rotina_Pega();
                    wait(tempo);
                    Rotina_Solta();
                    wait(tempo);
                }

                atual_pega = atual_pega - 1;
                quant_depositada = quant_depositada + 1;

                pc.printf("atual_pega: %d \r\n",atual_pega);
                pc.printf("quant_depositada: %d \r\n",quant_depositada);
            }
        }
        pc.printf("fora do if\r\n");
        //referenciamento();
    } //while(1)
} //int main()

void rotina_estado_botao_seleciona()
{
    if(debounce1.read_ms() >= 10 & estado_referenciamento == true & estado_botao_emergencia_in == false) {
        contador_botao_seleciona += 1;
        pc.printf("contador_botao_seleciona= %d\r\n", contador_botao_seleciona);
        lcd.cls();
    }
    debounce1.reset();
}

void rotina_emergencia_in()
{
    if(debounce2.read_ms() >= 10) {

        motor_x = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo X
        motor_y = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Y
        motor_z = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Z

        estado_referenciamento = false; //Solicita um novo referenciamento
        estado_botao_emergencia_in =! estado_botao_emergencia_in;
        contador_emergencia += 1;

        lcd.cls();
        wait_ms(500);
        lcd.locate(0,0);
        lcd.printf("Emergencia ON");
        lcd.locate(0,1);
        lcd.printf("Pressione RST");

        exit(0); //Encerra o programa e obriga o usuário a resetar o sistema

        debounce2.reset();
    }
}

void rotina_velocidade_eixo_Z()
{
    //Converte a leitura do Joystick para tempo de acionamento do motor
    J_XYZ = joystick.read_u16();
    tempo_horario = -1*J_XYZ*0.0000028 + 0.1875;
    tempo_anti_horario = J_XYZ*0.000002785 + 0.0025;
}

void rotina_velocidade_referenciamento_eixo_Z()
{
    //Converte a leitura do Joystick para tempo de acionamento do motor
    J_XYZ = joystick.read_u16();
    tempo_horario = (-1*J_XYZ*0.0000028 + 0.1875)*2;
    tempo_anti_horario = (J_XYZ*0.000002785 + 0.0025)*2;
}

void rotina_deslocamento_eixo()
{
    //Calcula os deslocamentos no eixo Z

    deslocamento_X = (passo_fuso*pulsos_X*passo_motor)/360;
    deslocamento_Y = (passo_fuso*pulsos_Y*passo_motor)/360;
    deslocamento_Z = (passo_fuso*pulsos_Z*passo_motor)/360;

//    recebe_arduino.printf("X%3.2f\n", deslocamento_X);
//    recebe_arduino.printf("Y%3.2f\n", deslocamento_Y);
//    recebe_arduino.printf("Z%3.2f\n", deslocamento_Z);

    pc.printf("X%3.2f, Y%3.2f, Z%3.2f, J_XYZ=%d\n", deslocamento_X, deslocamento_Y, deslocamento_Z, J_XYZ);

}

//void rotina_botoes_IHM()
//{
//    if(SELECT > 60000 & SELECT < 65000) valor_SELECT =! valor_SELECT;
//    else valor_SELECT = 0;
//}

void referenciamento()
{

    while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_SELECT == 1) {//W1

        for(i = 0; i < 4; i++) {
            motor_x = 1 << i;
            motor_y = 1 << i;
            motor_z = 1 << i;
            wait(0.003);
        }

        if(contador_rise == 1) {//if1

            while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 1) {//W2

                for(i = 3; i > -1; i--) {
                    motor_x = 1 << i;
                    motor_y = 1 << i;
                    motor_z = 1 << i;
                    wait(0.003);

                }

                if(contador_rise == 2) {//if2

                    while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 2) {//W3

                        for(i = 0; i < 4; i++) {
                            motor_x = 1 << i;
                            motor_y = 1 << i;
                            motor_z = 1 << i;
                            wait(0.003);
                        }

                        if(contador_rise == 3) {//if3

                            while(seguranca_fdc != 8 & estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 3) {//W4
                                for(i = 3; i > -1; i--) {
                                    motor_x = 1 << i;
                                    motor_y = 1 << i;
                                    motor_z = 1 << i;
                                    wait(0.003);

                                }
                                seguranca_fdc += 1;

                            }//Fecha W4

                            estado_referenciamento = true;
                            contador_SELECT = 0;
                            contador_rise = 0;
                            seguranca_fdc = 0;

                            valor_SELECT = true;
                            contador_botao_seleciona = 0;
                            deslocamento_X = 0; //Define o deslocamento linear total
                            deslocamento_Y = 0; //Define o deslocamento linear total
                            deslocamento_Z = 0; //Define o deslocamento linear total
                            pulsos_X = 0;
                            pulsos_Y = 0;
                            pulsos_Z = 0;
                            deslocamento_max_X = 5; //Define o deslocamento linear total
                            deslocamento_max_Y = 5; //Define o deslocamento linear total
                            deslocamento_max_Z = 5; //Define o deslocamento linear total
//                            velocidade = 0.1;

//                            recebe_arduino.printf("D\n"); //Done referenciamento

                        }//Fecha if3
                    }//Fecha W3

                }//Fecha if2
            }//Fecha W2
        }//Fecha if1
    }//Fecha W1
    motor_x = 0x00;
    motor_y = 0x00;
    motor_z = 0x00;
}//Fecha funcao


void rotina_fdc_rise()
{
    if(debounce3.read_ms() >= 50 & estado_botao_emergencia_in == false & estado_referenciamento == false & contador_SELECT == 1) {
        contador_rise += 1;
        pc.printf("contador_rise=%d\n", contador_rise);
        debounce3.reset();
    }
}


void rotina_JOG()
{
    J_XYZ = joystick.read_u16();

    if(32400 <= J_XYZ &  J_XYZ <= 34100) {
        motor_x = 0x00;
        motor_y = 0x00;
        motor_z = 0x00;
    }

    else {
//============== Movimenta Eixo Y ======================================

        if (J_XYZ > 34100 & deslocamento_Y > 0 & pulsos_Y > 0) {
//            tempo_motor.start();
//            while(deslocamento_Y > 0) {
                J_XYZ = joystick.read_u16();
                for(i = 0; i < 4; i++) {
                    motor_y = 1 << i;
//                    rotina_velocidade_eixo();
                    rotina_deslocamento_eixo();
                    pulsos_Y-=1;
                    wait(velocidade);

                }
//            }
//            tempo_motor.reset();
//            tempo_motor.stop();
        }

        if(J_XYZ < 32400 & deslocamento_max_Y > deslocamento_Y  & pulsos_Y < 2048) {
//            tempo_motor.start();
//            while(deslocamento_max_Y > deslocamento_Y) {
                J_XYZ = joystick.read_u16();
                for(i = 3; i > -1; i--) {
                    motor_y = 1 << i;
//                    rotina_velocidade_eixo();
                    rotina_deslocamento_eixo();
                    pulsos_Y+=1;
                    wait(velocidade);

                }
//            }
//            tempo_motor.reset();
//            tempo_motor.stop();
        }

//============== Movimenta Eixo X ======================================

        if (J_XYZ > 34100 & deslocamento_X > 0  & pulsos_X > 0) {
//            tempo_motor.start();
//            while(deslocamento_X > 0) {
                for(i = 0; i < 4; i++) {
                    motor_x = 1 << i;
//                    rotina_velocidade_eixo();
                    rotina_deslocamento_eixo();
                    pulsos_X-=1;
                    wait(velocidade);

                }
//            }
//            tempo_motor.reset();
//            tempo_motor.stop();
        }

        if(J_XYZ < 32400 & deslocamento_max_X > deslocamento_X & pulsos_X < 2048) {
//            tempo_motor.start();
//            while(deslocamento_max_X > deslocamento_X) {
                for(i = 3; i > -1; i--) {
                    motor_x = 1 << i;
//                    rotina_velocidade_eixo();
                    rotina_deslocamento_eixo();
                    pulsos_X+=1;
                    wait(velocidade);

                }
//            }
//            tempo_motor.reset();
//            tempo_motor.stop();
        }


//============== Movimenta Eixo Z ======================================

        if (J_XYZ > 34100 & deslocamento_Z > 0  & pulsos_Z > 0) {
//            tempo_motor.start();
//            while(deslocamento_Z > 0) {
                for(i = 0; i < 4; i++) {
                    motor_z = 1 << i;
//                    rotina_velocidade_eixo();
                    rotina_deslocamento_eixo();
                    pulsos_Z-=1;
                    wait(velocidade);

                }
//            }
//            tempo_motor.reset();
//            tempo_motor.stop();
        }

        if(J_XYZ < 32400 & deslocamento_max_Z > deslocamento_Z & pulsos_Z < 2048) {
//            tempo_motor.start();
//            while(deslocamento_max_Z > deslocamento_Z) {
                for(i = 3; i > -1; i--) {
                    motor_z = 1 << i;
//                    rotina_velocidade_eixo();
                    rotina_deslocamento_eixo();
                    pulsos_Z+=1;
                    wait(velocidade);

                }
//            }
//            tempo_motor.reset();
//            tempo_motor.stop();
        }


    }
}


void Rotina_Pega()
{
    sinal = 0; // saída nível lógico "baixo"
    
    pc.printf("posZ_atual: %f, posZ_origem: %d \r\n",posZ_atual, posZ_origem);
    //if(posZ_atual > posZ_origem) {
        while (deslocamento_Z != posZ_origem) {
            //While para verificar que o motor funcione até a a posição desejada ser alcançada
            for(i = 0; i < 4; i++) {
                motor_z = 1 << i;
                pulsos_Z -= 1;
                wait(velocidade);
                rotina_deslocamento_eixo();
                pc.printf("Pos atualZ H: %f \r\n", deslocamento_Z);
            }
        }
    //}
//    else { //não faz nada
//        while (deslocamento_Z < posZ0_desejada) {
//            //While para verificar que o motor funcione até a a posição desejada ser alcançada
//            for(i = 0; i < 4; i++) {
//                motor_z = 1 << i;
//                pulsos_Z -= 1;
//                wait(velocidade);
//                rotina_deslocamento_eixo();
//                pc.printf("Pos atualZ H: %d\r\n", deslocamento_Z);
//            }
//        }
//    }
    
    
    
    posX_atual = deslocamento_X;
    pc.printf("posX_atual: %f, posX0_desejada:%f \r\n",posX_atual, posX0_desejada);
    if(posX_atual < posX0_desejada) {
        while (deslocamento_X > posX0_desejada) {
            //While para verificar que o motor funcione até a a posição desejada ser alcançada
            for(i = 3; i > -1; i--) {
                motor_x = 1 << i;
                pulsos_X += 1;
                wait(velocidade);
                rotina_deslocamento_eixo();
                pc.printf("Pos atualX AH: %f\r\n", deslocamento_X);
            }
            pc.printf("Entrou no while posX_atual");
        }
    } 
    else {
        while (deslocamento_X < posX0_desejada) {
            //While para verificar que o motor funcione até a a posição desejada ser alcançada
            for(i = 0; i < 4; i++) {
                motor_x = 1 << i;
                pulsos_X -= 1;
                wait(velocidade);
                rotina_deslocamento_eixo();
                pc.printf("Pos atualX H: %f\r\n", deslocamento_X);
            }
        }
    }
    posY_atual = deslocamento_Y;
    pc.printf("posY_atual: %f, posY0_desejada: %f \r\n",posY_atual, posY0_desejada);
    if(posY_atual < posY0_desejada) {
        while (posY_atual > posY0_desejada) {
            for(i = 3; i > -1; i--) {
                motor_y = 1 << i;
                pulsos_Y += 1;
                wait(velocidade);
                rotina_deslocamento_eixo();
            }
        }
    }

    else {
        while (posY_atual < posY0_desejada) {
            for(i = 0; i < 4; i++) {
                motor_y = 1 << i;
                pulsos_Y -= 1;
                wait(velocidade);
                rotina_deslocamento_eixo();
            }
        }
    }

    posZ_atual = deslocamento_Z;
    pc.printf("posZ_atual: %f, posZ0_desejada: %f \r\n",posZ_atual, posZ0_desejada);
    if(posZ_atual < posZ0_desejada) {
        while (deslocamento_Z > posZ0_desejada) {
            //While para verificar que o motor funcione até a a posição desejada ser alcançada
            for(i = 3; i > -1; i--) {
                motor_z = 1 << i;
                pulsos_Z += 1;
                wait(velocidade);
                rotina_deslocamento_eixo();
                pc.printf("Pos atualZ AH: %f\r\n", deslocamento_X);
            }
        }
    } else {
        while (deslocamento_Z < posZ0_desejada) {
            //While para verificar que o motor funcione até a a posição desejada ser alcançada
            for(i = 0; i < 4; i++) {
                motor_z = 1 << i;
                pulsos_Z -= 1;
                wait(velocidade);
                rotina_deslocamento_eixo();
                pc.printf("Pos atualX H: %f\r\n", deslocamento_X);
            }
        }
    }


    sinal = 1; // saída nível lógico "alto"
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf("Coletando");
    pc.printf("Coletando\r\n");
    wait(tempo);

}

void Rotina_Solta()
{
    sinal = 0; // saída nível lógico "baixo"

    n_pulsosZ = (1/(passo_motor*passo_fuso))*360*deslocamentoZ_partida; // desloca a pipeta no eixo Z para sua posição de partida para evitar colisão

    n_pulsosX = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_X; //movimentação do eixo X para posição de pega
    n_pulsosY = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Y; //movimentação do eixo Y para posição de pega
    n_pulsosZ = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Z; //movimentação do eixo Z para posição de pega

    sinal = 1; // saída nível lógico "alto"
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf("Depositando");
    pc.printf("Depositando\r\n");
    wait(tempo);
}