File content as of revision 4:5dd6e95b5706:

#include "TextLCD.h"
#include "L6474.h"
#include "TCS3200.h"
#include "Servo.h"


// Set do display LCD 20x4 com o módulo I2C

I2C i2c_lcd(D14,D15);
TextLCD_I2C lcd(&i2c_lcd, 0x7E,TextLCD::LCD20x4);

/*-----Declaração dos pinos e suas funções-----*/

//PINOS DE INTERRUPÇÃO.

InterruptIn jog_button_pos(PC_12); // Botão para jog+ e navegação no menu
InterruptIn jog_button_neg(PA_15); // Botão para jog- e navegação no menu

InterruptIn btnX(PC_3); // Botão para selecionar o eixo X
InterruptIn btnY(PC_2); // Botão para selecionar o eixo Y
InterruptIn btnZ(PC_0); // Botão para selecionar o eixo Z

InterruptIn confirma(PC_13); // Botão OK do menu (pino do botão do usuario)
InterruptIn back_btn(PB_7); // Botão para a função "voltar" do menu

InterruptIn FDC(PC_5); // Fim de curso para o eixo Z
InterruptIn FDC2(PC_8); // Fim de curso para o eixo X
InterruptIn FDC3(PC_6); // Fim de curso para o eixo Y

/*-----Declaração de variaveis globais do código-----*/

bool referZ = false; // Parametro para o referenciamento do motor 1
bool referX = false; // Parametro para o referenciamento do motor 2
bool referY = false; // Parametro para o referenciamento do motor 3

bool save_pos = false; // Parametro para definir o fim da rotina de save pick\drop

bool dirx = false; // Indicador que o motor ira se movimentar no eixo x
bool diry = false; // Indicador que o motor ira se movimentar no eixo y
bool dirz = false; // Indicador que o motor ira se movimentar no eixo z

bool jog_pos = false; // Indicador para a movimentação do motor no jog (+)
bool jog_neg = false; // Indicador para a movimentação do motor no jog (-)
bool ref_cycle = true; // Indicador para a realização do ciclo Pick/Place

bool enable = false; // Variavel auxiliar para navegação no motor
bool flag = true; // Variavel auxiliar para utilizar mais de uma vez as funções disponiveis no menu

signed char change = 0;  // Numero que varia de 0 a 2, indicando qual set de velocidade será usado
signed char ref_cursor = 0; // Numero de referência para a posição do cursor
unsigned char ref_menu = 0; // Numero para indicar o menu atual

/*Criação de uma struct (basicamente, uma classe) para sets de velocidades e acelerações
  diferentes. Cada objeto dessa struct possui os argumentos de velocidades max e min e
  aceleração e desaceleração.
*/

struct set_velocidades {
    unsigned int maxspeed;
    unsigned int minspeed;
    unsigned int ac;
    unsigned int dc;
};

struct set_velocidades set[3]; /* Cria um objeto da struct set_velocidades com três
                               objetos dentro dele (basicamente, um objeto triplo).
                               */

//Struct para sets de coordenadas, com argumentos de posição em x,y e z.

struct Coordenadas {
    int posx;
    int posy;
    int posz;
};

struct Coordenadas PickPos,DropPos[3]; //Cria objeto unico para posição de pick, e objeto triplo para posição de place.

// Perfil de incialização do motor

L6474_init_t init = {
    160,                              /* Acceleration rate in pps^2. Range: (0..+inf). */
    160,                              /* Deceleration rate in pps^2. Range: (0..+inf). */
    1600,                             /* Maximum speed in pps. Range: (30..10000]. */
    800,                              /* Minimum speed in pps. Range: [30..10000). */
    1000,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
    L6474_OCD_TH_1500mA,               /* Overcurrent threshold (OCD_TH register). */
    L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
    L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
    L6474_STEP_SEL_1_8,               /* Step selection (STEP_SEL field of STEP_MODE register). */
    L6474_SYNC_SEL_1_2,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
    L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
    L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
    3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
    21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
    L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
    L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
    L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
    L6474_ALARM_EN_OVERCURRENT |
    L6474_ALARM_EN_THERMAL_SHUTDOWN |
    L6474_ALARM_EN_THERMAL_WARNING |
    L6474_ALARM_EN_UNDERVOLTAGE |
    L6474_ALARM_EN_SW_TURN_ON |
    L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
};

// Declaração dos motores utilizados

L6474 *motorZ;
L6474 *motorX;
L6474 *motorY;

/*-----Declaração de funções auxiliares----*/

/*  FUNÇÕES "sobe_cursor(void)" E "desce_cursor(void)":
    1-) Adiciona ou subtrai 1 da variavel ref_cursor;
    2-) Atribui o valor de linha máx ou mín se o ref_cursor atravessar algum
        extremo da tela LCD;
    3-) O cursor se posiciona de acordo com a posição designada;
*/
void sobe_cursor(void)
{
    ref_cursor += 1;
    if (ref_cursor > 3) {
        ref_cursor = 0;
    }
}

void desce_cursor(void)
{
    ref_cursor -= 1;
    if (ref_cursor < 0) {
        ref_cursor = 3;
    }
}

/* FUNÇÃO "conclusao(char n)":
    1-)Desliga o cursor;
    2-)Apresenta uma mensagem de conclusão do salvamento das posições:
    -> Se n = 0, apresenta a mensagem de conclusão do PICK;
    -> Se n != 0, apresenta a mensagem de alguma das posições de DROP (depende do valor de n);
*/

void conclusao(char n)
{
    lcd.setCursor(TextLCD::CurOff_BlkOff);
    if (n == 0) {
        lcd.cls();
        lcd.locate(4,0);
        lcd.printf("PICK POSITION");
        lcd.locate(7,1);
        lcd.printf("SALVA!");
        wait(2);
        lcd.cls();
    }
    if (n > 0) {
        lcd.cls();
        lcd.locate(3,0);
        lcd.printf("DROP POSITION %d", n);
        lcd.locate(7,1);
        lcd.printf("SALVA!");
        wait(2);
        lcd.cls();
    }
}

/*FUNÇÃO "menu_jog(void)":
    -> Prepara a estrutura para printar no LCD as posições atuais de cada eixo.
*/

void menu_jog(void)
{
    lcd.cls();
    lcd.locate(0,1);
    lcd.printf("X: ");
    lcd.locate(0,2);
    lcd.printf("Y: ");
    lcd.locate(0,3);
    lcd.printf("Z: ");
}

/*FUNÇÃO "menu_passivo(char n)":
    -> A função apresenta um menu estático diferente de acordo com o valor n recebido.
       Os menus aqui disponiveis são estáticos pois são temporários e não possibilitam
       seleção de funções ou movimento do cursor. 
    -> Os menus estáticos são chamados para avisar ao usuário da realização de alguma 
       função, da inicialização da maquina e seu estado de operação.
*/
 
void menu_passivo(char n)
{
    switch(n) {
        
        // Menu estático 1 é o menu de abertura (ao ligar a máquina)
        case 1: {
            lcd.cls();
            lcd.locate(6,0);
            lcd.printf("Maquina");
            lcd.locate(3,1);
            lcd.printf("Pick-and-Place");
            lcd.locate(5,2);
            lcd.printf("RAWCAMBOLE");
            wait(3);
            lcd.cls();
            break;
        }
        // Menu estático 2 é o menu chamado durante a ação de referenciamento da máquina
        case 2: {
            lcd.cls();
            lcd.printf("Setando ponto de");
            lcd.locate(5,1);
            lcd.printf("origem!");
            lcd.locate(5,3);
            lcd.printf("AGUARDE...");
            break;
        }
        // Menu estático 3 é chamado durante o ciclo infinito
        case 3: {
            lcd.cls();
            lcd.printf("Maquina operando");
            lcd.locate(2,1);
            lcd.printf("    em ciclo");
            lcd.locate(2,3);
            lcd.printf("STOP-> Press Back");
            break;
        }
        // Menu estático 4 é chamado durante o ciclo único
        case 4: {
            lcd.cls();
            lcd.printf("Maquina operando");
            lcd.locate(2,1);
            lcd.printf("apenas um ciclo");
            break;
        }
            
    }
}

/* FUNÇÕES "jog_(...)":
    -> As funções de jog vem em pares de "on" e "off", um par para cada direção.
    1-) A função "on" apenas seta uma variavel em TRUE, possibilitando a movimentação
        em determinada direção;
    2-) A função "off" seta o valor mesma variavel em FALSE e para a movimentação do motor,
        terminando com a operação de jog em determinada direção;
*/

void jog_pos_on(void)
{
    jog_pos = true;
}

void jog_pos_off(void)
{
    jog_pos = false;
    motorZ->hard_stop();
    motorX->hard_stop();
    motorY->hard_stop();
}

void jog_neg_on(void)
{
    jog_neg = true;
}

void jog_neg_off(void)
{
    jog_neg = false;
    motorZ->hard_stop();
    motorX->hard_stop();
    motorY->hard_stop();
}

/* FUNÇÕES "motorN_off(void)":
    1-) Para a movimentação do motor;
    2-) Seta uma variavel booleana em TRUE para indicar que o referenciamento de tal
        eixo está pronto;
*/

void motorZ_off(void)
{
    motorZ->hard_stop();
    referZ = true;
}

void motorX_off(void)
{
    motorX->hard_stop();
    referX = true;
}

void motorY_off(void)
{
    motorY->hard_stop();
    referY = true;
}

/*FUNÇÕES "set_aceleracoesN(...)":
    -> Função recebe os seguintes parâmetros:
    1-) Velocidade máxima;
    2-) Velocidade mínima;
    3-) Aceleração;
    4-) Desaceleração;
    -> Função seta esses valores no motor N
*/
void set_aceleracoesZ(int maxspeed,int minspeed,int ac,int dc)
{
    motorZ->set_max_speed(maxspeed)/2;
    motorZ->set_min_speed(minspeed)/2;
    motorZ->set_acceleration(ac)/2;
    motorZ->set_deceleration(dc)/2;
}

void set_aceleracoesX(int maxspeed,int minspeed,int ac,int dc)
{
    motorX->set_max_speed(maxspeed);
    motorX->set_min_speed(minspeed);
    motorX->set_acceleration(ac);
    motorX->set_deceleration(dc);
}

void set_aceleracoesY(int maxspeed,int minspeed,int ac,int dc)
{
    motorY->set_max_speed(maxspeed);
    motorY->set_min_speed(minspeed);
    motorY->set_acceleration(ac);
    motorY->set_deceleration(dc);
}

/*FUNÇÕES "save_pick_pos()" E "save_dropN":
    1-) Função captura as posições atuais dos 3 motores e armazenam seus valores
        nos atributos respectivos de seu objeto respectivo.
    2-) Altera o valor de uma variavel booleana para TRUE, indicando o fim da 
        operação de jog.
*/

void save_pick_pos(void)
{
    PickPos.posx = motorX->get_position();
    PickPos.posy = motorY->get_position();
    PickPos.posz = motorZ->get_position();
    save_pos = true;
}

void save_drop1()
{
    DropPos[0].posx = motorX->get_position();
    DropPos[0].posy = motorY->get_position();
    DropPos[0].posz = motorZ->get_position();
    save_pos = true;
}

void save_drop2()
{
    DropPos[1].posx = motorX->get_position();
    DropPos[1].posy = motorY->get_position();
    DropPos[1].posz = motorZ->get_position();
    save_pos = true;
}

void save_drop3()
{
    DropPos[2].posx = motorX->get_position();
    DropPos[2].posy = motorY->get_position();
    DropPos[2].posz = motorZ->get_position();
    save_pos = true;
}

/* FUNÇÃO "reconhecimento_peca()":
    1-) Função primeiro usa o sensor de cor para identificar a cor predominante:
        verde, vermelho ou azul e também o indutivo para identificar o material;
        -> De acordo com a leitura do sensor, definiram-se apenas duas opções: ver-
           de ou não verde (se não verde, ele retorna vermelho);
    2-) Um reconhecimento é feito a cada 0.1 segundos por 1 segundo, e cada um dos três resultados
        possiveis recebe um tag:
        -> Tag 0 = etiqueta verde e metal;
        -> Tag 1 = etiqueta verde e polimero;
        -> Tag 2 = etiqueta vermelha, qualquer material;
        O valor do tag de cada medição é armazenado em uma array.
    3-) Em um par de loops, avalia-se qual foi o tag que mais apareceu. O que for
        a maioria, será considerado como o tag (ou cor) verdadeiro, que é o valor
        retornado pela função;
    4-) Esse tag servirá como o indice da struct de structs "DropPos", indicando 
        para qual posição de DROP ele deveria ir
*/
   
int reconhecimento_peca(void)
{   
    TCS3200 color(PC_4, PB_14, PB_1, PB_13, PB_15); // Declara os pinos para o sensor RGB
    DigitalIn sensor(PB_2); // Declara pino para o sensor indutivo
    long red, green, blue, clear; // Valores de leitura para cada cor
    int tagy, i = 0, j = 0, tag[10]; 
    int contador = 0, contador_max = 0, elemento;
    
    // Modo de operação do sensor
    color.SetMode(TCS3200::SCALE_20);

    while(i <= 9) {

        red = color.ReadRed();
        green = color.ReadGreen();
        blue = color.ReadBlue();
        clear = color.ReadClear();
        
        if(green > 30) {
            tagy = 2;
        }
        if((green <= 30)&&(sensor)) {
            tagy = 0;
        }
        if((green <= 30) && (!sensor)) {
            
            tagy = 1;
        }

        tag[i] = tagy;
        i = i + 1;
        wait(0.1);
        
    }
    
    // Loops para avaliar qual tag aparece mais
    
    for (i = 0; i <= 9; i++) {
        for (j = 0; j <= 9; j++) {
            if (tag[i] == tag[j]) {
                contador += 1;
            }
        }
        if (contador > contador_max) {
            contador_max = contador;
            elemento = tag[i];
        }
        contador = 0;
    }
   
    return elemento;
}

/* FUNÇÃO "sefta_origem()":
    -> A função realiza o referenciamento completo para os 3 eixos, um de cada vez;
    -> Por ser uma função mais longa, o processo é detalhado dentro da própria função;
*/

void seta_origem()
{
    menu_passivo(2);
    
    //Seta uma alta velocidade para o referenciamento
    
    set_aceleracoesZ(3500/2,1200,100/2,100/2); 
    set_aceleracoesX(4000,3000,100,100); 
    set_aceleracoesY(4000,3000,100,100); 
    
    InterruptIn confirma(PC_13);
    confirma.mode(PullUp);
    
    while(1) {
        
        
        InterruptIn FDC(PC_5);
        FDC.fall(&motorZ_off); 
        FDC.mode(PullUp);
        
        confirma.fall(&motorZ_off);
        
        //Chamada do fim de curso para a função de interrupção
        
        
        if (referZ == false) {
            motorZ->run(StepperMotor::BWD);
            motorZ->wait_while_active();
        }

        // Se a interrupção for chamada, a variavel referZ se torna TRUE, acionando
        // os comandos a seguir.

        else {
            motorZ->move(StepperMotor::FWD,1000); //Leve recuo
            motorZ->wait_while_active();
            motorZ->set_home(); // Seta posição de Home
            int HomePosition = motorZ->get_position();
            printf("Posicao Home = %d\r\n",HomePosition); //Verificar que HomePosition = 0
            referZ = false;
            break;   //Quebra do loop while, pois referenciamento do motor foi feito
        }

    }
    
    while(1) {
            
        // Motor continua andando em uma só direção enquanto variavel referX estiver
        // em FALSE
        
        InterruptIn FDC(PC_5);
        FDC.fall(&motorX_off); 
        FDC.mode(PullUp);
        
        if (referX == false) {
            motorX->run(StepperMotor::BWD);
            motorX->wait_while_active();
        }
        

        // Se a interrupção for chamada, a variavel referX se torna TRUE, acionando
        // os comandos a seguir.

        else {
            motorX->move(StepperMotor::FWD,1000); //Leve recuo
            motorX->wait_while_active();
            motorX->set_home(); // Seta posição de Home
            int HomePosition = motorX->get_position();
            referX = false;
            break;   //Quebra do loop while, pois referenciamento do motor foi feito
        }

    }
    
    referY = false; //Caso os botôes de fim de curso sejam apertados sem querer antes de seus zeramentos, tem certeza que vai entrar no loop, ja que são interrupçõs
    
    while(1) {
        
        
        InterruptIn FDC(PC_5);
        FDC.fall(&motorY_off); 
        FDC.mode(PullUp);
        //Motor continua andando em uma só direção enquanto variavel referX estiver
        // em FALSE

        if (referY == false) {
            motorY->run(StepperMotor::BWD);
            motorY->wait_while_active();
        }

        // Se a interrupção for chamada, a variavel referX se torna TRUE, acionando
        // os comandos a seguir.

        else {
            motorY->move(StepperMotor::FWD,1000); //Leve recuo
            motorY->wait_while_active();
            motorY->set_home(); // Seta posição de Home
            int HomePosition = motorY->get_position();
            printf("Posicao Home = %d\r\n", HomePosition); //Verificar que HomePosition = 0
            referY = false;
            break;   //Quebra do loop while, pois referenciamento do motor foi feito
        }

    }
}

/* FUNÇÃO "muda_velocidade(void)":
    1-) Acrescenta-se 1 à variavel "change", que indica qual dos sets será sele-
        cionado.
    2-) De acordo com o valor da variável "change", um dos sets é ativado para 
        todos os motores. 
    3-) De acordo com o valor da variáve l "change", um dos três leds é aceso
        para indicar a velocidade atual.
*/

void muda_velocidade(void)
{ 
    change += 1;
    
    // O valor máximo de "change" é 2, então ao chegar em 3, é zerado novamente 
    if (change == 3) {
        change = 0;
    }
    set_aceleracoesZ(set[change].maxspeed/2,set[change].minspeed/2,set[change].ac/2,set[change].dc/2);
    set_aceleracoesX(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
    set_aceleracoesY(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
    
    switch(change) {
        case 0: {
            DigitalOut LED1(PC_1);
            LED1 = 1;
            DigitalOut LED2(PA_1);
            LED2 = 0;
            DigitalOut LED3(PA_4);
            LED3 = 0; 
            break; 
        }
        case 1: {
            DigitalOut LED1(PC_1);
            LED1 = 0;
            DigitalOut LED2(PA_1);
            LED2 = 1;
            DigitalOut LED3(PA_4);
            LED3 = 0;     
            break;
        }
        case 2: {
            DigitalOut LED1(PC_1);
            LED1 = 0;
            DigitalOut LED2(PA_1);
            LED2 = 0; 
            DigitalOut LED3(PA_4);
            LED3 = 1;  
            break;
        }
    }

}

/* FUNÇÃO "ativa_eixoN(void)":
    -> Desativa a variável referente aos outros eixos, setando-as como "false", 
       e ativa a do eixo N, setando-a como TRUE
*/

void ativa_eixoX(void)
{
    dirx = true;
    dirz = false;
    diry = false;
    muda_velocidade();
}

void ativa_eixoY(void)
{   
    dirx = false;
    dirz = false;
    diry = true;
    muda_velocidade();
}

void ativa_eixoZ(void)
{
    dirx = false;
    dirz = true;
    diry = false;
    muda_velocidade();
}

/* FUNÇÃO "selecao_funcao(void)":
    -> A função apenas coloca o valor de "enable" em TRUE. No loop principal do
       programa, o enable em TRUE permite a seleção de alguma das funções do menu
       por parte do usuário.  
*/

void selecao_funcao(void)
{
    enable = true;
}

/* FUNÇÃO "disco_disco(void)":
    -> Função completamente inútil, feita apenas para divertir o usuário;
*/

void disco_disco(void) {
    unsigned char i = 0;
    while (i < 50) {
        lcd.cls();
        lcd.setBacklight(TextLCD::LightOn);
        lcd.printf(" DI$CO DI$CO DI$CO");
        lcd.locate(0,1);
        lcd.printf(" DI$CO DI$CO DI$CO");
        lcd.locate(0,2);
        lcd.printf(" DI$CO DI$CO DI$CO");
        lcd.locate(0,3);
        lcd.printf(" DI$CO DI$CO DI$CO");
        wait(0.05);
        lcd.setBacklight(TextLCD::LightOff);
        wait(0.05);
        i += 1;
    }
    lcd.setBacklight(TextLCD::LightOn);
}

/* FUNÇÃO "menu_dinamico(char n)":
    -> Um "menu_dinamico" é considerado, neste programa, como um menu em que o 
       usuario consegue movimentar o cursor e selecionar funções (ou seja, é a
       parte interativa do menu). 
    1-) Os botões "Confirma","Jog_button_pos" e "Jog_button_neg" são setados para 
        chamar certas funções relacionadas à navegação no menu ao detectar uma 
        borda de subida (comando "rise");
    2-) Há dois menus dinâmicos no projeto:
        A-) A tela inicial, contendo as opções:
            ->Cycle Start (inicia a operação ciclica de Pick/Place);
            ->Set positions (transição para o próximo menu dinâmico);
            ->Referenciamento (realiza operação de zeramento dos eixos novamente);
            ->Disco Disco! (executa a função "disco_disco"); 
        B-) A tela referente às posições à serem salvas:
            ->Set Pick (inicia o jog para salvar a posição de Pick);
            ->Set Drop 1 (inicia o jog para salvar a posição de Drop 1);
            ->Set Drop 2 (inicia o jog para salvar a posição de Drop 2);
            ->Set Drop 3 (inicia o jog para salvar a posição de Drop 3);
    3-) A variável "n" que entra como input da função define qual dos menus é 
        chamado.
*/
    
void menu_dinamico(char n)
{

    confirma.fall(&selecao_funcao);
    jog_button_pos.fall(&sobe_cursor);
    jog_button_neg.fall(&desce_cursor);

    switch(n) {
        case 0: {
            lcd.setCursor(TextLCD::CurOn_BlkOn);
            lcd.cls();
            lcd.locate(1,0);
            lcd.printf("->Cycle start");
            lcd.locate(1,1);
            lcd.printf("->Set positions");
            lcd.locate(1,2);
            lcd.printf("->Referenciamento");
            lcd.setAddress(0,ref_cursor);
            lcd.locate(1,3);
            lcd.printf("->DISCO DISCO!");
            lcd.setAddress(0,ref_cursor);
            break;
        }
        case 1: {
            lcd.setCursor(TextLCD::CurOn_BlkOn);
            lcd.cls();
            lcd.locate(1,0);
            lcd.printf("->Set Pick");
            lcd.locate(1,1);
            lcd.printf("->Set Drop 1");
            lcd.locate(1,2);
            lcd.printf("->Set Drop 2");
            lcd.locate(1,3);
            lcd.printf("->Set Drop 3");
            lcd.setAddress(0,ref_cursor);
            break;
        }
        case 2: {
            lcd.setCursor(TextLCD::CurOn_BlkOn);
            lcd.cls();
            lcd.locate(1,0);
            lcd.printf("->Ciclo infinito");
            lcd.locate(1,1);
            lcd.printf("->Ciclo unico");
            lcd.setAddress(0,ref_cursor);
            break;
        }
    }
}

/*FUNÇÕES "cycle_stop(void)" E "cycle(char n)":
    A-) "cycle_stop(void)": função seta a variável "ref_cycle" como FALSE, parâ-
         metro que irá finalizar o ciclo ao final da rotina;
    B-) "cycle":
        -> Seta o botão "back" para chamar a função "cycle_stop" e finalizar a 
           operação em ciclo;
        1-) Leva o motor para a posição de HOME (apenas uma vez);
        2-) Motor vai até a posição de PICK;
        3-) Ativa a função "reconhecimento_peca()" para determinar aonde a peça
            será levada (posição de DROP);
        4-) Vai para a posição de DROP e solta a peça;
        5-) Retorna para o PICK, reiniciando o ciclo;
        -> OBS: A função "cycle" recebe um parâmetro n. Se esse parâmetro for 1, o
           ciclo é realizado apenas uma vez. Se for 0, o ciclo se repetirá até o u-
           suário pressionar o botão de "back".
*/

void cycle_stop(void)
{   
    ref_cycle = false;
}
    
void cycle(char n)
{
    Servo garra(PA_11); // Declaração do servo
    garra.calibrate(0.001,90); // Calibração de sua abertura
    
    back_btn.fall(&cycle_stop);
    
    if (n == 0) {
        menu_passivo(3); // Ciclo infinito
    }
    
    if (n == 1) {
        menu_passivo(4); // Ciclo único
    }
    
    int tag; // Tag que sairá como resultado da função de reconhecimento da peça
    
    printf("Comeco do ciclo!\r\n");
    
    //Alta velocidade para retornar para a posição de home
    
    set_aceleracoesZ(set[2].maxspeed/2,set[2].minspeed/2,set[2].ac/2,set[2].dc/2);
    set_aceleracoesX(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
    set_aceleracoesY(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
    
    motorZ->go_home();
    motorZ->wait_while_active();
    
    motorX->go_home();
    motorX->wait_while_active();
    
    motorY->go_home();
    motorY->wait_while_active();
    
    
    // Seta velocidaes/acelerações para o ciclo
    
    set_aceleracoesZ(set[0].maxspeed,set[0].minspeed,set[0].ac,set[0].dc);
    set_aceleracoesX(set[1].maxspeed,set[1].minspeed,set[1].ac,set[1].dc);
    set_aceleracoesY(set[1].maxspeed,set[1].minspeed,set[1].ac,set[1].dc);

    ref_cycle = true;
    
    garra = 1;
    
    while(1) {
        
        // Vai para a posição de PICK
        motorX->go_to(PickPos.posx);
        motorX->wait_while_active();
        motorY->go_to(PickPos.posy);
        motorY->wait_while_active();
        motorZ->go_to(PickPos.posz - 1000);
        motorZ->wait_while_active();
        
        tag = reconhecimento_peca(); // Reconhece a peça e qual posição de Drop irá
        garra = 0.7; // Fecha a garra
        wait(1);
        
        // Vai para a posição de DROP
        motorZ->move(StepperMotor::BWD,3000);
        motorZ->wait_while_active();
        motorY->go_to(DropPos[tag].posy);
        motorY->wait_while_active();
        motorX->go_to(DropPos[tag].posx);
        motorX->wait_while_active();
        motorZ->go_to(DropPos[tag].posz);
        motorZ->wait_while_active();
        wait(1);
        garra = 1; // Garra abre e deixa o objeto
        
        /*Se a chamada para finalizar o ciclo foi chamada, volta-se para o menu 
          inicial e quebra o loop while; */
          
        if ((ref_cycle == false) || (n == 1)) {
            enable = false;
            menu_dinamico(ref_menu);
            break;
        }
        
    } 
}

/* FUNÇÕES DE "jog_(...)":
    -> Chama o layout para o menu do jog;
    -> O botão "Confirma" é setado para salvar a posição;
    -> Basicamente, cada botão de jog, ao ser segurado (borda de descida) faz o motor
       seguir em movimento. Ao ser solto (borda de subida) faz o motor parar;
    -> Habilitam-se os botões dos eixos para que o usuário possa mover um eixo de
       cada vez. Quando o usuario apertar um desses botões, a velocidade é alterada
       também;
*/

void paradaZ(void) 
{
    motorZ->hard_stop();
    wait(1);
    int posicao = motorZ->get_position();
    if (posicao < 0) {
        motorZ->move(StepperMotor::FWD,2000);
    }
    else {
        motorZ->move(StepperMotor::BWD,2000);
    }
}

void paradaX(void) 
{
    motorX->hard_stop();
    wait(1);
    int posicao = motorX->get_position();
    if (posicao < 0) {
        motorX->move(StepperMotor::FWD,3000);
    }
    else {
        motorX->move(StepperMotor::BWD,3000);
    }
}
void paradaY(void) 
{
    motorY->hard_stop();
    wait(1);
    int posicao = motorY->get_position();
    if (posicao < 0) {
        motorY->move(StepperMotor::FWD,3000);
    }
    else {
        motorY->move(StepperMotor::BWD,3000);
    }
}

void jog(char n) {
        menu_jog();
        lcd.locate(3,0);
    /* 
       De acordo com o valor de entrada "n", seta-se o botão "Confirma" para
       salvar a posição de Save,Drop1, Drop2 ou Drop3;
    */
    InterruptIn confirma(PC_13); 
    confirma.mode(PullUp);
    
    switch(n) {
        case 0: {
            printf("Posicao de Save\r\n");
            lcd.printf("PICK POSITION");
            confirma.fall(&save_pick_pos);
            break;
        }
        case 1: {
            lcd.locate(3,0);
            lcd.printf("DROP POSITION %d",n);
            confirma.fall(&save_drop1);
            break;
        }
        case 2: {
            lcd.locate(3,0);
            lcd.printf("DROP POSITION %d",n);
            confirma.fall(&save_drop2);
            break;
        }
        case 3: {
            lcd.locate(3,0);
            lcd.printf("DROP POSITION %d",n);
            confirma.fall(&save_drop3);
            break;
        }
    }
    
    InterruptIn jog_button_pos(PC_12);
    jog_button_pos.mode(PullUp);
    InterruptIn jog_button_neg(PA_15);
    jog_button_neg.mode(PullUp);
    
    jog_button_pos.fall(&jog_pos_on);
    jog_button_neg.fall(&jog_neg_on);

    jog_button_pos.rise(&jog_pos_off);
    jog_button_neg.rise(&jog_neg_off);

    btnX.fall(&ativa_eixoX);
    btnY.fall(&ativa_eixoY);
    btnZ.fall(&ativa_eixoZ);

    while(save_pos == false) {

        float posz = motorZ->get_position();
        float posx = motorX->get_position();
        float posy = motorY->get_position();
        lcd.locate(3,1);
        lcd.printf("%.2f mm",posx*3/800);
        lcd.locate(3,2);
        lcd.printf("%.2f mm",posy*3/800);
        lcd.locate(3,3);
        lcd.printf("%.2f mm",posz*5/800);

        if (jog_pos == true) {
            if (dirx == true) {
                InterruptIn FDC(PC_5);
                FDC.mode(PullUp);
                FDC.fall(&paradaX);
                motorX->run(StepperMotor::FWD);
                motorX->wait_while_active();
            }
            if (diry == true) {
                InterruptIn FDC(PC_5);
                FDC.mode(PullUp);
                FDC.fall(&paradaY);
                motorY->run(StepperMotor::FWD);
                motorY->wait_while_active();
            }
            if (dirz == true) {
                InterruptIn FDC(PC_5);
                FDC.mode(PullUp);
                FDC.fall(&paradaZ);
                motorZ->run(StepperMotor::FWD);
                motorZ->wait_while_active();
            }
        }

        if (jog_neg == true) {
            if (dirx == true) {
                InterruptIn FDC(PC_5);
                FDC.mode(PullUp);
                FDC.fall(&paradaX);
                motorX->run(StepperMotor::BWD);
                motorX->wait_while_active();
            }
            if (diry == true) {
                InterruptIn FDC(PC_5);
                FDC.mode(PullUp);
                FDC.fall(&paradaY);
                motorY->run(StepperMotor::BWD);
                motorY->wait_while_active();
            }
            if (dirz == true) {
                InterruptIn FDC(PC_5);
                FDC.mode(PullUp);
                FDC.fall(&paradaZ);
                motorZ->run(StepperMotor::BWD);
                motorZ->wait_while_active();
            }
        }
    }
    save_pos = false;
    ref_menu = 1;
    flag = true;
    conclusao(n);
    menu_dinamico(ref_menu);
}

/* FUNÇÃO "back_op(void)":
    -> Se a tela for referente a um sub-menu, a função irá fazer retornar para o
       menu principal. Caso contrário, nada ocorre;
*/
  
void back_op(void)
{
    ref_cursor = 4; // O valor de ref_cursor em 4 chama um case que chama a primeira tela dinâmica;
    enable = true;
    printf("BACK\r\n");
}

int main()
{
    //Prepara os 3 sets de velocidade, com velocidaes e acelerações variadas.

    set[0].maxspeed = 2000;
    set[0].minspeed = 1000;
    set[0].ac = 100;
    set[0].dc = 100;

    set[1].maxspeed = 4000;
    set[1].minspeed = 3000;
    set[1].ac = 100;
    set[1].dc = 100;

    set[2].maxspeed = 4200;
    set[2].minspeed = 3500;
    set[2].ac = 100;
    set[2].dc = 100;

    
    //Seta comunicação SPI
    DevSPI dev_spi(D11, D12, D13);

    //Inicialização dos componentes dos motores
    motorZ = new L6474(D2, D8, D7, D9, D10, dev_spi);
    motorX = new L6474(D2, D8, D4, D3, D10, dev_spi);
    motorY = new L6474(D2, D8, D5, D6, D10, dev_spi);

    if (motorZ->init(&init) != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }
    if (motorX->init(&init) != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }
    if (motorY->init(&init) != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }

    // Seta todos os motores para que trabalhem com microstep de 8
    motorZ->set_step_mode(StepperMotor::STEP_MODE_1_4);
    motorX->set_step_mode(StepperMotor::STEP_MODE_1_4);
    motorY->set_step_mode(StepperMotor::STEP_MODE_1_4);
    
    //Seta velocidades e acelerações inciciais
    set_aceleracoesZ(set[0].maxspeed,set[0].minspeed,set[0].ac,set[0].dc);
    set_aceleracoesX(set[0].maxspeed,set[0].minspeed,set[0].ac,set[0].dc);
    set_aceleracoesY(set[0].maxspeed,set[0].minspeed,set[0].ac,set[0].dc);

    lcd.setCursor(TextLCD::CurOn_BlkOn); // Liga o cursor
    
    lcd.setBacklight(TextLCD::LightOn); // Liga o backlight do LCD
    lcd.setAddress(0,0);
    lcd.setCursor(TextLCD::CurOff_BlkOff); // Desliga o cursor para o menu estático
    menu_passivo(1);

    lcd.setCursor(TextLCD::CurOn_BlkOn); // Liga o cursor novamente pro usuario poder mexe-lo
    menu_dinamico(0);


    /* Loops principais do funcionamento do motor:
    
    -> Os dois loops aninhados (while e do_while) definem o funcionamento conti-
       nuo da máquina.
    -> O funcionamento geral é o seguinte: 
        1-) O do_while depende de um parametro booleano chamado "flag". O flag
            permanece em TRUE até alguma função do menu ser chamada. Qualquer fun-
            ção chamada no Menu que resulte em uma ação seta o flag como FALSE.
            Por exemplo, cliquar em alguma opção do menu que leve para outro sub
            menu não setaria o "flag" como FALSE, pois nenhuma ação real foi feita.
            Agora, entrar em qualquer operação de jog ou de ciclo, setaria a variavel
            em FALSE, finalizando o do_while após o término da ação.
        2-) Para evitar que a máquina pare de funcionar depois do término de uma ação,
            aninha-se o do_while em um loop infinito while(1). Ao sair do do_while, o
            flag retorna para TRUE, permitindo que o usuario possa escolher alguma ação
            novamente. 
    -> Variaveis importantes:
        a) "ref_menu": Seu valor indica em qual dos menus o usuário está no momento;
        b) "ref_cursor": Seu valor indica em qual das linhas o cursor está localizado;
        c) "enable": A variavel "enable" possui FALSE como estado padrão. Quando o botão
            de "confirma" for pressionado, ele comuta para TRUE. Quando isso acontece, 
            avalia-se o valor de "ref_menu" e "ref_cursor" para indicar que ação deve
            ocorrer naquele momento (ir para outro submenu, referenciar, jog, etc.).
            Seu valor sempre retorna para FALSE após qualquer operação ou ao fim do 
            do_while;
    -> Cada menu dinâmico disponivel tem seu aninhamento com switch-case para avaliar
       as funções que podem ser chamadas dentro dele; 
    -> OBS: O ref_cursor só vai de 0 a 3 (linha 1 a 4 do LCD). Entretanto, coloca-se nos 
       dois sub-menus um case em que ref_cursor vale 4. No uso normal do cursor, isso nunca
       vai ocorrer. Esse valor é SETADO para o ref_cursor forçadamente quando o usuario apertar
       o botão de BACK. Isso faz com que acione-se uma operação unica para este botão, retornando
       para o menu principal. 
        */
    while(1) {
        
        /* Redeclaração da maioria dos pinos. Isso é feito para evitar qualquer
           confito entre as chamadas de cada pino. Muitos são desativados quando
           o sensor RGB é declarado, então esse processo faz com que nenhuma função
           seja perdida no meio do uso da máquina */
           
        InterruptIn confirma(PC_13);
        confirma.mode(PullUp);
        InterruptIn jog_button_pos(PC_12);
        jog_button_pos.mode(PullUp);
        InterruptIn jog_button_neg(PA_15);
        jog_button_neg.mode(PullUp);
        InterruptIn btnX(PC_3); // Botão para selecionar o eixo X
        btnX.mode(PullUp);
        InterruptIn btnY(PC_2); // Botão para selecionar o eixo Y
        btnY.mode(PullUp);
        InterruptIn btnZ(PC_0); // Botão para selecionar o eixo Z
        btnZ.mode(PullUp);
        
        InterruptIn back_btn(PB_7);
        back_btn.mode(PullUp);
        
        confirma.fall(&selecao_funcao);
        
        jog_button_pos.fall(&sobe_cursor);
        jog_button_neg.fall(&desce_cursor);
        
        btnX.fall(&ativa_eixoX);
        btnY.fall(&ativa_eixoY);
        btnZ.fall(&ativa_eixoZ);
        
        do {
            wait(0.1);
            lcd.locate(0,ref_cursor); // Movimentação do cursor
            
            // Menu prinicipal
            if (ref_menu == 0) {
                if (enable == true) {
                    switch(ref_cursor) {
                        case 0: {
                            // Mudança para o submenu com as opções de ciclo
                            ref_cursor = 0;
                            lcd.locate(0,ref_cursor);
                            ref_menu = 2;
                            menu_dinamico(ref_menu);
                            enable = false;
                            break;
                        }
                        case 1: {
                            // Mudança para o submenu com as 4 posições a serem salvas
                            ref_cursor = 0;
                            lcd.locate(0,ref_cursor);
                            ref_menu = 1;
                            menu_dinamico(ref_menu);
                            enable = false;
                            break;
                        }
                        case 2: {
                            // Operação de referenciamento
                            //int tag = reconhecimento_peca();
                            seta_origem();
                            flag = false;
                            break;
                        }
                        case 3: {
                            // Operação disco-disco
                            disco_disco();
                            flag = false;
                            break;
                        }
                    }
                }
            }
    
            // Sub-menu para as posições a serem salvas
            
            if(ref_menu == 1) {
                back_btn.mode(PullUp);
                back_btn.fall(&back_op);
                if (enable == true) {
                    switch(ref_cursor) {
                        case 0: {
                            // Posição de Pick
                            jog(0);
                            flag = false;
                            InterruptIn back_btn(PB_7);
                            back_btn.mode(PullUp);
                            back_btn.fall(&back_op);
                            break;
                        }
                        case 1: {
                            // Posição de Drop 1
                            jog(1);
                            flag = false;
                            InterruptIn back_btn(PB_7);
                            back_btn.mode(PullUp);
                            back_btn.fall(&back_op);
                            break;
                        }
                        case 2: {
                            // Posição de Drop 2
                            jog(2);
                            flag = false;
                            InterruptIn back_btn(PB_7);
                            back_btn.mode(PullUp);
                            back_btn.fall(&back_op);
                            break;
                        }
                        case 3: {
                            // Posição de Drop 3
                            jog(3);
                            flag = false;
                            InterruptIn back_btn(PB_7);
                            back_btn.mode(PullUp);
                            back_btn.fall(&back_op);
                            break;
                        }
                        case 4: {
                            // Case para quando botão BACK for pressionado. Retorna para o menu inicial
                            ref_cursor = 0;
                            lcd.locate(0,ref_cursor);
                            ref_menu = 0;
                            menu_dinamico(ref_menu);
                            enable = false;
                            break;
                        }
                    }
                }
            }
            
            if (ref_menu == 2) {
                back_btn.fall(&back_op);
                back_btn.mode(PullUp);
                if (enable == true) {
                    switch(ref_cursor) {
                        case 0: {
                            // Opção para ciclo infinito
                            cycle(0);
                            flag = false;
                            break;
                        }
                        case 1: {
                            // Opção para ciclo único
                            cycle(1);
                            flag = false;
                            break;
                        }
                        case 4: {
                            // Case para quando botão BACK for pressionado. Retorna para o menu inicial
                            ref_cursor = 0;
                            lcd.locate(0,ref_cursor);
                            ref_menu = 0;
                            menu_dinamico(ref_menu);
                            enable = false;
                            break;
                        }
                    }
                }
            }
            
        } while(flag == true);
        
        flag = true;
        enable = false;
        menu_dinamico(ref_menu);
    }
}