Gabriel Ligeiro
Projeto Mecatronico Grupo: Pingos nos I's 2019
Dependencies: mbed Projeto_Mecatronico Projeto_Mecat_Pingos
Diff: main.cpp
- Revision:
- 0:61ea5c9ad2de
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Jun 13 19:46:46 2019 +0000
@@ -0,0 +1,1717 @@
+#include "mbed.h"
+#include "TextLCD.h"
+//#include "LinkedList.h"
+#define espera 100
+#define vel_1 1000
+#define vel_2 1000
+//bibliteca segmento: TextLCD
+/*
+O QUE FAZER
+1-aarrumar o print do COLA? qnd ponto modificado
+2-arrrumar a listadecola quando ponto modificado
+*/
+
+
+//---------------------------------------------------------------------------------ENTRADAS E SAIDAS ANALOGICAS--------------------------------------------------------------------------------------------/
+//DigitalIn botaoC(PC_1);// //pino entrada botaoC
+//DigitalIn botaoA(PC_2); // //pino entrada botaoA colar pontos
+int botaoA=1;
+int botaoC=1;
+DigitalIn botao_analog(PC_12);
+//int botao_analog = 1;
+AnalogIn eixo_x(A0);
+AnalogIn eixo_y(A2);
+
+DigitalOut enable_x(PA_10);//motor x
+DigitalOut dir_x(PB_5);//motor x
+DigitalOut step_x(PB_3);//motor x
+DigitalIn FimCursoX(PA_6); //entrada fdc eixo x novo
+
+DigitalOut enable_y(PB_4);//motor y
+DigitalOut dir_y(PA_8);//motor y
+DigitalOut step_y(PB_10);//motor y
+DigitalIn FimCursoY(PC_8); //entrada fdc eixo y novo
+
+DigitalOut enable_z(PA_9);//motor z
+DigitalOut dir_z(PB_6);//motor z
+DigitalOut step_z(PC_7);//motor z
+
+DigitalIn FimCursoZ(PB_7); //entrada fdc eixo z novo
+
+DigitalIn botao_emergencia(PA_13); //entrada B.E. nova
+DigitalOut aciona_pneu(PC_3);//aciona pneumatica da cola PRESSAO
+DigitalOut aciona_pneu2(PC_2);//aciona pneumatica da cola VALVULA
+//----------------------------------IHM
+TextLCD lcd(PC_13, PA_15, PA_12, PA_11, PB_12, PB_11, TextLCD::LCD20x4); // rs, e , d4-d7
+DigitalIn botao_cima(PB_2);
+DigitalIn botao_enter(PB_1);
+DigitalIn botao_baixo(PB_15);
+
+int menu = 1; // Aqui será a seleção dos menus do IHM
+int menu_enter = 0;//referenciando menu se refe =1
+int menu_enter_2 = 0;//menu 2 ADICONAR PONTOS/ MODIFICAR PONTOS COLAR PONTOS
+int flag = 1;
+char strings_x[5]={0,0,0,0,0};
+char ident_eixo;
+int i = 1; //contador de chars
+int linha = 0;//para descer o " >"
+int ponto = 0;//valor de um ponto
+int pontos[10];//numero de pontos
+int cola[10];//lista para saber se ira aplicar cola entre os pontos
+int i_c = 0;// indice da lista cola
+int i_p = 0; //loop i pontos
+int ultimo_p = 0;
+int lista_vel[]={1000,700,500};//lista velocidade dos motores
+int i_v = 0;//indice lista velocidade
+int vel = 0;//velocidade dos motores
+int mover_motores = 0;//permite permitir mover os motores
+int flag_mod = 0;//flag de modificar pontos para add pontos
+int flag_mod2 = 0;//flag de modificar pontos para add COLA
+int flag_emergencia = 0;// flag botao de emergencia
+int flag_emergencia2 = 0;// flag botao de emergencia 2
+int vai_emergencia = 0;// variavel para ser usado a funcao " go to"
+
+Serial pc(USBTX , USBRX);// band rate é de 1152000 na Serial Port do Tera Term
+//------------------------------------------------------------------------------------------VARIÁVEIS-------------------------------------------------------------------------------------------------------/
+
+float passo = 1.8f; //1.8 graus por passo
+float x ; //valor do INPUT analog x
+float y ;//valor do INPUT analog Y
+bool refe; //selecionar a rotina de referenciamento
+bool troca_yz;//troca o eixo y/z
+int posicao_x; //Posicao do passo atual EIXO X
+int posicao_y;//Posicao do passo atual EIXO Y
+int posicao_z;//Posicao do passo atual EIXO Y
+int i_x = -1; // contador lista x
+int i_y = -1; // contador lista y
+int i_z = -1; // contador lista z
+int pontos_x[10]; //lista de pontos X
+int pontos_y[10];//lista de pontos Y
+int pontos_z[10];//lista de pontos Z
+int add;
+int pos;
+int guardar_pontos = 0;//flag para ir ao condicional "guardar pontos"
+int colar_pontos = 0;//flag para ir ao condicional "ir aos pontos"
+//LinkedList<float>pontos;
+
+//------------------------------------------------------------------------------------------DEFINICAO DAS FUNÇÕES--------------------------------------------------------------------------------------------------------/
+
+float passo_mm_xy (int passo_atual)
+ {
+ float conversao = (passo_atual*3)/200;
+ return conversao;
+ }
+float passo_mm_z (int passo_atual)
+ {
+ float conversao = (passo_atual*10)/200;
+ return conversao;
+ }
+
+
+void move_inicial(bool dir, float vel, bool fimcurso, int qual_eixo) //dir 0 ou 1 vel em [passos/s] fimcurso x/y qual_eixo x/y/z = 0/1/2
+ {
+ //vel = (2*1000) / (vel);
+ vel = 1000000 / (vel);
+ vel = vel - espera;
+ if (qual_eixo == 0)
+ {
+ while(FimCursoX == 0)
+ {
+
+
+ step_x = 1;
+ wait_us(espera);
+ step_x = 0;
+ wait_us(vel);
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ }
+ }//qual eixo == 0 = X
+ if (qual_eixo == 1)
+ {
+ while(FimCursoY == 0)
+ {
+ step_y = 1;
+ wait_us(espera);
+ step_y = 0;
+ wait_us(vel);
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ }
+ }//qual eixo == 0 = Y
+ if (qual_eixo == 2)
+ {
+ while(FimCursoZ == 0)
+ {
+ step_z = 1;
+ wait_us(espera);
+ step_z = 0;
+ wait_us(vel);
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ }
+ }//qual eixo == 0 = Z
+ }
+
+
+int move(bool dir, float vel,int posicao_xy, int qual_eixo) //dir 0 ou 1 vel em [passos/s] posicao, devolve os passos andados qual_eixo 0/1/2 x/y/z
+ {
+ int passos = 0;
+ int ultimo_passo =0;
+ int parada;
+ //vel = (2*1000) / (vel);
+ vel = 1000000 / (vel);
+ vel = vel - espera;
+ if (enable_x == 0 and qual_eixo == 0 ) //PARA EIXO X
+ {
+ ultimo_passo = posicao_xy;
+ //while ((eixo_x.read() > 0.7 or eixo_x.read() < 0.3f) and posicao_x > -1 )
+ while ((eixo_x > 0.7 or eixo_x < 0.3f) and posicao_x > -1 )
+ { step_x = 1;
+ wait_us(espera);
+ step_x = 0;
+ wait_us(vel);
+ passos++;
+ if (dir_x == 0)
+ {
+ parada = ultimo_passo + passos;
+ // pc.printf("dir_x1 \r\n");
+ }
+ if (dir_x == 1)
+ {
+ parada = ultimo_passo - passos;
+ // pc.printf("dir_x0 \r\n");
+ }
+ if (parada == 0 or parada == 25000)
+ {
+ break;
+ }
+
+ }
+ }
+ if (enable_y == 0 and qual_eixo == 1 ) //PARA EIXO Y
+ {
+ ultimo_passo = posicao_xy;
+ //while ((eixo_y.read() > 0.7 or eixo_y.read() < 0.3f) and posicao_y > -1 )
+ while ((eixo_y > 0.7 or eixo_y < 0.3f) and posicao_y > -1 )
+ {
+ step_y = 1;
+ wait_us(espera);
+ step_y = 0;
+ wait_us(vel);
+ passos++;
+ //pc.printf("%d \r\n",passos_y);
+ if (dir_y == 0)
+ {
+ parada = ultimo_passo + passos;
+ // pc.printf("dir_y1 \r\n");
+ }
+ if (dir_y == 1)
+ {
+ parada = ultimo_passo - passos;
+ // pc.printf("dir_y0 \r\n");
+ }
+ if (parada == 0 or parada == 26000)
+ {
+ break;
+ }
+ //pc.printf("ultimo passo:%d i: %d \r\n",parada,passos);
+ }
+ }
+ if (enable_z == 0 and qual_eixo == 2 ) //PARA EIXO Z
+ {
+ ultimo_passo = posicao_xy;
+ // while ((eixo_y.read() > 0.7 or eixo_y.read() < 0.3f) )
+ while ((eixo_y > 0.7 or eixo_y < 0.3f) )
+ {
+ step_z = 1;
+ wait_us(espera);
+ step_z = 0;
+ wait_us(vel);
+ passos++;
+ //pc.printf("%d \r\n",passos_y);
+ if (dir_z == 0)
+ {
+ parada = ultimo_passo + passos;
+ // pc.printf("dir_y1 \r\n");
+ }
+ if (dir_z == 1)
+ {
+ parada = ultimo_passo - passos;
+ // pc.printf("dir_y0 \r\n");
+ }
+ if (parada == 0 or parada == 2000)
+ {
+ break;
+ }
+ //pc.printf("ultimo passo:%d i: %d \r\n",parada,passos);
+ }
+ }
+ return passos;
+ }
+void para()//funçao para desativar o motor
+ {
+ enable_x = 1;
+ }
+
+void para_()//funçao para desativar o motor
+ {
+ enable_y = 1;
+ enable_z = 1;
+ }
+
+int movepara(int dir, float vel, int passo_,int qual_eixo )
+{
+ vel = 1000000 / (vel);
+ vel = vel - espera;
+ int passo_atual = 0;
+ // pc.printf("Passo_local %i, %i ", passo_atual, passo_ );
+
+ if (enable_x == 0 and qual_eixo == 0 ) //PARA EIXO X
+ {
+ while (passo_atual != abs(passo_))
+ { step_x = 1;
+ wait_us(espera);
+ step_x = 0;
+ wait_us(vel);
+ passo_atual++ ;
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ // pc.printf("Passo_local %i, %i ", passo_atual, passo_ );
+ }
+ }
+ if (enable_y == 0 and qual_eixo == 1 ) //PARA EIXO Y
+ {
+ while (passo_atual != abs(passo_))
+ { step_y = 1;
+ wait_us(espera);
+ step_y = 0;
+ wait_us(vel);
+ passo_atual++ ;
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ // pc.printf("Passo_local %i, %i ", passo_atual, passo_ );
+ }
+ }
+ if (enable_z == 0 and qual_eixo == 2 ) //PARA EIXO Z
+ {
+ while (passo_atual != abs(passo_))
+ { step_z = 1;
+ wait_us(espera);
+ step_z = 0;
+ wait_us(vel);
+ passo_atual++ ;
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ // pc.printf("Passo_local %i, %i ", passo_atual, passo_ );
+ }
+ }
+ return passo_atual;
+}
+
+//----------------------------------------------------------------------------------------------MAIN------------------------------------------------------------------------------------------------------------/
+int main()
+{
+ lcd.setCursor(TextLCD::CurOff_BlkOff);//desliga o cursor
+ posicao_x = 0;
+ posicao_y = 0;
+ posicao_z = 0;
+ refe = 0 ; //0 ou 1 1= REFERENCIAMENTO 0 = MOVIMENTO DOS EIXOS
+ troca_yz = 0; // 0 ou 1 0 = EIXO Y 1 = EIXO Z
+ pos = 0;
+ pc.baud(115200);
+ pc.printf("hello WORLD!\r\n");
+ enable_x = 1;//desliga os drivers
+ enable_y = 1;//desliga os drivers
+ enable_z = 1;//desliga os drivers
+ aciona_pneu2 = 1 ; //desliga pneu
+ aciona_pneu = 1 ; //desliga pneu
+ vel = lista_vel[0];//velocidade inicial
+ wait(0.25);
+ while(1)
+ {
+
+ if (botao_emergencia == 1)
+ {
+ if (flag_emergencia == 1)
+ {
+ lcd.setCursor(TextLCD::CurOff_BlkOff);//desliga o cursor
+ lcd.cls();
+ menu = 0;
+ menu = 1;
+ menu_enter_2 = 0 ;
+ flag_emergencia = 0;
+ }
+ if (menu == 1 )
+ {
+ lcd.cls();
+ lcd.printf(" -Referenciamento\n");
+ lcd.printf(" -Limpar Bico Ejetor");
+ lcd.printf(" -Aplicar Cola\n");
+ lcd.printf(" -Configuracoes\n");
+ lcd.locate(0,linha);
+ lcd.putc('~');
+
+ menu = 0;
+ }// if menu = 1
+ if (botao_cima == 1 and menu ==0)
+ {
+ pc.printf("menu %d \r\n",menu);
+ pc.printf("botao_cima \r\n");
+ lcd.cls();
+ lcd.printf(" -Referenciamento\n");
+ lcd.printf(" -Limpar Bico Ejetor");
+ lcd.printf(" -Aplicar Cola\n");
+ lcd.printf(" -Configuracoes\n");
+ linha--;
+ pc.printf(" %d\r\n",linha);
+ if (linha < 0)
+ {
+ linha = 3;
+ }
+ lcd.locate(0,linha);
+ lcd.putc('~');
+
+ wait(0.2);
+ }//IF botao_cima
+ //-------------------------------------------------------------------------------------BOTAO BAIXO MENU = 1 -----------------------------------------------------------------
+ if (botao_baixo == 1 and menu ==0)
+ {
+ pc.printf("menu %d\r\n",menu);
+ pc.printf("botao_baixo \r\n");
+ lcd.cls();
+ lcd.printf(" -Referenciamento\n");
+ lcd.printf(" -Limpar Bico Ejetor");
+ lcd.printf(" -Aplicar Cola\n");
+ lcd.printf(" -Configuracoes\n");
+ if (linha > 2)
+ {
+ linha = -1;
+ }
+ linha++;
+ pc.printf(" %d\r\n",linha);
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ wait(0.2);
+ }// if botao_baixo
+
+
+ if (botao_enter == 1 and menu==0)
+ {
+ pc.printf("botao_enter \r\n");
+ pc.printf(" %d\r\n",linha);
+
+ if (linha ==0)
+ {
+ menu_enter =1; // REFERERENCIAMENTO
+ refe = 1;
+ wait(0.2);
+ }
+ if (linha == 1)
+ {
+ menu_enter = 2;//LIMPAR BI EJETOR
+ }
+ if (linha == 2)
+ {
+ menu_enter = 3;//Aplicar COla
+ linha =0;
+ }
+ if (linha == 3)
+ {
+ menu_enter = 5;//Aplicar COla
+ linha =0;
+ }
+
+ pc.printf(" %d\r\n",linha);
+ wait(0.2);
+ }// if bbotao_enter\
+ //-------------------------------------------------------------------------------------LIMPAR BICO EJETOR -----------------------------------------------------------------
+ if (menu_enter == 2)
+ {
+ lcd.cls();
+ lcd.printf("\n");
+ aciona_pneu = 0;// liga pneumatica pressao
+ lcd.printf(" Limpando.\n");
+ aciona_pneu2 = 0;// liga pneumatica pressao
+ wait(0.5);
+ lcd.cls();
+ lcd.printf("\n");
+ lcd.printf(" Limpando..\n");
+ aciona_pneu2 = 1;// liga pneumatica pressao
+ wait(0.5);
+ lcd.cls();
+ lcd.printf("\n");
+ lcd.printf(" Limpando...\n");
+ aciona_pneu2 = 0;// liga pneumatica pressao
+ wait(1);
+ lcd.cls();
+ aciona_pneu2 = 1;// liga pneumatica pressao
+ lcd.printf("\n");
+ lcd.printf(" Bico Limpo!\n");
+ wait(2);
+ lcd.cls();
+ menu = 1;
+ menu_enter = 0;
+
+ }
+
+ //-------------------------------------------------------------------------------------APLICAR COLA MENU_ENTER = 3 -----------------------------------------------------------------
+ if (menu_enter == 3)
+ {
+ linha = 0;
+ lcd.cls();
+ lcd.printf(" -Adicionar Pontos \n");
+ lcd.printf(" -Mod./Ver Pontos \n");
+ lcd.printf("\n");
+ lcd.printf(" Voltar Colar\n");
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ menu = 2;
+ menu_enter = 4;//para o refresh rate
+ pc.printf("menu %d\r\n",menu);
+ }// Aplicar Cola
+ //-------------------------------------------------------------------------------------BOTAO CIMA -------------------------------------------------------------
+ if (botao_cima == 1)
+ {
+ if (menu == 2)
+ {
+ pc.printf("menu %d\r\n",menu);
+ pc.printf("botao_cima menu 2\r\n");
+ lcd.cls();
+ lcd.printf(" -Adicionar Pontos \n");
+ lcd.printf(" -Mod./Ver Pontos \n");
+ lcd.printf("\n");
+ lcd.printf(" Voltar Colar\n");
+ menu = 2;
+ linha--;
+ pc.printf(" %d\r\n",linha);
+ if (linha < 0)
+ {
+ linha = 4;
+ lcd.locate(0,0);
+ lcd.putc(' ');
+ lcd.locate(14,linha);
+ lcd.putc('~');
+ }
+ if(linha == 2)
+ {
+ linha = 1;
+ }
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ if (linha == 4)
+ {
+ lcd.locate(0,3);
+ lcd.putc(' ');
+ lcd.locate(14,linha);
+ lcd.putc('~');
+ }
+ wait(0.2);
+
+ }
+ if (menu == 3 or menu == 4)//-----------------------------------------------------------------add pontos-BB-------------------------------------------------------------
+ {
+
+ menu_enter_2 = 0;//para o refresh rate
+ linha--;
+ if (linha == 1)
+ {
+ linha = 3;
+ }
+ if (linha == 2)
+ {
+ lcd.locate(0,linha + 1);
+ lcd.putc(' ');
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ }
+ if (linha == 3)
+ {
+ lcd.locate(0,linha - 1);
+ lcd.putc(' ');
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ }
+
+ wait(0.2);
+ }
+ if (menu == 5)//-----------------------------------------------------------------Modificando Pontos---BC-----------------------------------------------------------
+ {
+ if (i_p == 0)
+ {
+
+
+ pc.printf(" %d\r\n",linha);
+ linha--;
+ if (linha == 1)
+ {
+ lcd.locate(10,linha+1);
+ lcd.putc(' ');
+ lcd.locate(0,3);
+ lcd.putc('~');
+ linha = 4;
+ }
+ if (linha == 2)
+ {
+ lcd.locate(10,linha + 1);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (linha == 3)
+ {
+ lcd.locate(0,linha);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+
+ wait(0.2);
+ }
+ if (i_p != 0 and i_p != ultimo_p -1)
+ {
+
+
+
+
+
+ linha--;
+ printf("linha %d\r\n",linha);
+ if (linha == 1)//
+ {
+ lcd.locate(10,linha+1);
+ lcd.putc(' ');
+ lcd.locate(0,2);
+ lcd.putc('~');
+ linha = 5;
+ }
+ if (linha == 2)
+ {
+ lcd.locate(10,linha + 1);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (linha == 3)
+ {
+ lcd.locate(0,linha);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (linha == 4)
+ {
+ lcd.locate(0,2);
+ lcd.putc(' ');
+ lcd.locate(0,3);
+ lcd.putc('~');
+ }
+
+ wait(0.2);
+ }
+ if (i_p != 0 and i_p == ultimo_p -1)
+ {
+
+ linha--;
+ pc.printf("linha %d\r\n",linha);
+
+ if (linha == 2)
+ {
+ lcd.locate(10,linha + 1);
+ lcd.putc(' ');
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ linha = 5;
+ }
+ if (linha == 3)
+ {
+ lcd.locate(0,linha);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (linha == 4)
+ {
+ lcd.locate(0,2);
+ lcd.putc(' ');
+ lcd.locate(0,3);
+ lcd.putc('~');
+ }
+
+ wait(0.2);
+ }
+ }
+ if(menu == 6)//-----------------------------------------------------------------Configuracoes--------------------------------------------------------------
+ {
+ pc.printf(" %d\r\n",linha);
+
+
+
+ if (linha == 3)
+ {
+ linha = 1;
+ }
+ linha--;
+
+ if (linha == -1)
+ {
+ lcd.locate(0,0);
+ lcd.putc(' ');
+ lcd.locate(0,3);
+ lcd.putc('~');
+ linha = 3;
+ }
+ if (linha == 0)
+ {
+ lcd.locate(0,3);
+ lcd.putc(' ');
+ lcd.locate(0,0);
+ lcd.putc('~');
+ linha = 0;
+ }
+ wait(0.2);
+ }
+ }//IF botao_cima
+ //-------------------------------------------------------------------------------------BOTAO BAIXO MENU = 2 ----BB-------------------------------------------------------------
+ if (botao_baixo == 1)
+ {
+ if (menu == 2)
+ {
+ pc.printf("menu %d\r\n",menu);
+ pc.printf("botao_baixo menu 2\r\n");
+ lcd.cls();
+ lcd.printf(" -Adicionar Pontos \n");
+ lcd.printf(" -Mod./Ver Pontos \n");
+ lcd.printf("\n");
+ lcd.printf(" Voltar Colar\n");
+ menu = 2;
+
+ if (linha > 3)
+ {
+ linha = -1;
+ }
+
+ pc.printf(" %d\r\n",linha);
+ linha++;
+ if(linha == 2)
+ {
+ linha = 3;
+ }
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ if (linha == 4)
+ {
+ lcd.locate(0,linha-1);
+ lcd.putc(' ');
+ lcd.locate(14,linha);
+ lcd.putc('~');
+ }
+ wait(0.2);
+ }
+ if (menu == 3 or menu == 4)//-----------------------------------------------------------------add pontos-BB-------------------------------------------------------------
+ {
+ if (linha == 3)
+ {
+ linha = 1;
+ }
+ menu_enter_2 = 0;//para o refresh rate
+ linha++;
+ pc.printf("Botao Baixo\r\n");
+ pc.printf("menu%d linha=%d\r\n",menu,linha);
+ if (linha == 3)
+ {
+ lcd.locate(0,linha - 1);
+ lcd.putc(' ');
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ }
+ if (linha == 2)
+ {
+ lcd.locate(0,linha +1);
+ lcd.putc(' ');
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ }
+ wait(0.2);
+ }
+ if (menu == 5)//-----------------------------------------------------------------Modificando Pontos---BB-----------------------------------------------------------
+ {
+ if (i_p == 0)
+ {
+ if (linha > 3)
+ {
+ lcd.locate(0,3);
+ lcd.putc(' ');
+ lcd.locate(10,2);
+ lcd.putc('~');
+ linha = 1;
+ }
+
+ pc.printf(" %d\r\n",linha);
+ linha++;
+ if (linha == 3)
+ {
+ lcd.locate(10,linha-1);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (linha == 4)
+ {
+ lcd.locate(10,linha-1);
+ lcd.putc(' ');
+ lcd.locate(0,linha-1);
+ lcd.putc('~');
+ }
+ wait(0.2);
+ }
+ if (i_p != 0 and i_p != ultimo_p -1)
+ {
+ if (linha > 4)//"proximo"
+ {
+ lcd.locate(0,2);
+ lcd.putc(' ');
+ lcd.locate(10,2);
+ lcd.putc('~');
+ linha = 1;
+ }
+
+ pc.printf(" %d\r\n",linha);
+ linha++;
+ if (linha == 3)//"Modificar"
+ {
+ lcd.locate(10,linha-1);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (linha == 4)//"voltarr"
+ {
+ lcd.locate(10,3);
+ lcd.putc(' ');
+ lcd.locate(0,3);
+ lcd.putc('~');
+ }
+ if (linha == 5)//"Anterior"
+ {
+ lcd.locate(0,3);
+ lcd.putc(' ');
+ lcd.locate(0,2);
+ lcd.putc('~');
+ }
+ wait(0.2);
+ }
+ if (i_p != 0 and i_p == ultimo_p -1)
+ {
+ if (linha == 5)
+ {
+ linha = 2;
+ }
+
+ linha++;
+ if (linha == 3)//"Modificar"
+ {
+ lcd.locate(0,linha-1);
+ lcd.putc(' ');
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (linha == 4)//"voltarr"
+ {
+ lcd.locate(10,3);
+ lcd.putc(' ');
+ lcd.locate(0,3);
+ lcd.putc('~');
+ }
+
+ if (linha == 5)//"Anterior"
+ {
+ lcd.locate(0,3);
+ lcd.putc(' ');
+ lcd.locate(0,2);
+ lcd.putc('~');
+ }
+
+ pc.printf(" %d\r\n",linha);
+ wait(0.2);
+ }
+ }
+ if(menu == 6)//-----------------------------------------------------------------Configuracoes--BB------------------------------------------------------------
+ {
+ pc.printf(" %d\r\n",linha);
+
+
+ if (linha == 0)
+ {
+ linha = 2;
+ }
+ if (linha == 3)
+ {
+ linha = 0;
+ }
+ linha++;
+
+ if (linha == 3)
+ {
+ lcd.locate(0,0);
+ lcd.putc(' ');
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ }
+ if (linha == 1)
+ {
+ lcd.locate(0,3);
+ lcd.putc(' ');
+ lcd.locate(0,0);
+ lcd.putc('~');
+ linha = 0;
+ }
+ wait(0.2);
+ }
+ }// if botao_baixo
+ //-------------------------------------------------------------------------------------BOTAO ENTER MENU = 2 --BE---------------------------------------------------------------
+ if (botao_enter == 1 )
+ {
+ pc.printf("menu%d linha %d\r\n",menu,linha);
+
+ if (menu == 2)
+ {
+ pc.printf(" %d\r\n",linha);
+ if (linha ==0)
+ {
+ //pc.printf("vai para PONTOS\r\n");
+ menu_enter_2 =1; // ADICIONAR PONTOS
+ menu = 0;
+ mover_motores = 1;
+ }
+ if (linha ==1)
+ {
+ menu_enter_2 = 4; // MODIFICAR PONTOS
+ i_p = 0;//zerar
+ }
+ if (linha == 4)//COLAR PONTOS
+ {
+ pc.printf("Vai para funcao COlar Pontos\r\n");
+ menu = 2;//tela de "aplicar pontas"
+ colar_pontos = 1;//menu colar pontos
+ wait(0.2);
+ }
+ if (linha == 3)
+ {
+ menu = 1 ;//VOltar
+ linha = 1;
+ }
+ pc.printf(" %d\r\n",linha);
+ wait(0.2);
+ }
+ if (menu == 3)//Adicionado os pontos
+ {
+ if (linha == 2)//confirmar ponto
+ {
+
+ if ( flag_mod == 0)
+ {
+ pc.printf("flag_mod = 0 \r\n");
+ i_p++;
+ guardar_pontos = 1;// vai para o condicional "guardar Pontos"
+ wait(0.1);
+ }
+
+
+ ultimo_p = i_p;
+ menu_enter_2 = 1 ;//Ponto Adicionado!
+
+ linha = 0;
+ ponto = ponto + 10;
+ wait(0.2);
+ if( i_p > 1)
+ {
+ menu_enter_2 = 2;
+ }
+ if ( flag_mod == 1)
+ {
+ pc.printf("flag_mod = 1 \r\n");
+ pontos_x[i_p] = posicao_x;
+ pontos_y[i_p] = posicao_y;
+ pontos_z[i_p] = posicao_z;
+ pc.printf(" Pontos X[%i]=%i Pontos Y[%i]=%i Pontos Z[%i]=%i \r\n",i_p, pontos_x[i_p],i_p, pontos_y[i_p ],i_p, pontos_z[i_p]);
+ flag_mod = 0;//tira flag de modificar pontos
+ menu_enter_2 = 2;
+ wait(0.2);
+
+ }
+
+ wait(0.2);
+ }
+ if (linha == 3)
+ {
+ menu_enter = 3 ;//VOltar
+ linha = 0;
+ wait(0.2);
+ }
+ }//menu = 3
+ if (menu == 4)//------------------COla-BE
+ {
+ pc.printf(" -->COLA-BE: flag_mod2 = %d\r\n",flag_mod2);
+ if (flag_mod2 == 0)
+ {
+ if (linha == 2)
+ {
+ pc.printf(" COla[%d] = 1\r\n",i_c);
+ menu_enter_2 = 1;
+ cola[i_c] = 1;
+ }
+ if (linha == 3)
+ {
+ pc.printf(" COla[%d] = 0\r\n",i_c);
+ menu_enter_2 = 1 ;//VOltar
+
+ cola[i_c] = 0;
+ }
+ }
+ if (flag_mod2 == 1)
+ {
+ pc.printf(" menu_enter = 3\r\n");
+ if (linha == 2)
+ {
+ pc.printf(" COla[%d] = 1\r\n",i_p-1);
+ menu_enter_2 = 1;
+ cola[i_p-1] = 1;
+ }
+ if (linha == 3)
+ {
+ pc.printf(" COla[%d] = 0\r\n",i_p-1);
+ menu_enter_2 = 1 ;//VOltar
+
+ cola[i_p-1] = 0;
+ }
+ menu_enter = 3;//volta para a tela de "add pontos"
+ wait(0.1);
+ }
+ flag_mod2 = 0;//tira flag de modificar pontos
+ wait(0.2);
+ flag_mod = 0;
+ i_c++;
+ }//menu = 4
+ if (menu == 5) //MODIFICANDO OS PONTOS-BE
+ {
+
+ if (i_p != 0)
+ {
+ if ( linha == 2)
+ {
+ i_p++;
+ menu_enter_2 = 4;
+ wait(0.2);
+ }
+ if ( linha == 3)
+ {
+ menu_enter_2 = 1;
+ flag_mod = 1;
+ flag_mod2= 1;
+ pc.printf("Mod!");
+ wait(0.2);
+ }
+ if ( linha == 4)
+ {
+ pc.printf("voltar!");
+ menu_enter = 3 ;//VOltar
+ linha = 1;
+ wait(0.2);
+ }
+ if ( linha == 5)
+ {
+ i_p--;
+ menu_enter_2 = 4;
+ wait(0.2);
+ }
+ }
+ if (i_p == 0)
+ {
+ if ( linha == 2)
+ {
+ i_p++;
+ menu_enter_2 = 4;
+ wait(0.2);
+ }
+ if ( linha == 3)
+ {
+ menu_enter_2 = 1;
+ pc.printf("Mod!");
+ wait(0.2);
+ }
+ if ( linha == 4)
+ {
+ pc.printf("Mod!");
+ menu_enter = 3 ;//VOltar
+ linha = 1;
+ wait(0.2);
+ }
+ }
+ }
+ if (menu == 6)//-----------------------------------------------------------------Configuracoes--------------------------------------------------------------
+ {
+ if ( linha == 0)
+ {
+ i_v++;
+ if (i_v == 3)
+ {
+ i_v = 0;
+ }
+ vel = lista_vel[i_v];
+ wait(0.2);
+ menu_enter = 5;
+
+
+ }
+ if ( linha == 2)
+ {
+ lcd.locate(2,2);
+ lcd.putc(' ');
+ lcd.locate(3,2);
+ lcd.putc(' ');
+ lcd.locate(4,2);
+ lcd.putc(' ');
+ lcd.locate(2,2);
+ lcd.putc('N');
+ lcd.locate(3,2);
+ lcd.putc('a');
+ lcd.locate(4,2);
+ lcd.putc('o');
+ }
+ if ( linha == 3)
+ {
+ pc.printf("Mod!");
+ menu = 1 ;//VOltar
+ linha = 1;
+ wait(0.2);
+ }
+
+ }
+
+ }// if bbotao_enter\
+ //-------------------------------------------------------------------------------------ADICIONAR PONTOS -----------------------------------------------------------------
+ if (menu_enter_2 == 1)
+ {
+ pc.printf("ADICIONAR PONTOS\r\n");
+ pc.printf("i_p = %d ", i_p);
+ lcd.cls();
+ linha = 2;
+ pontos[i_p] = ponto;
+ pc.printf("Ponto %d \r\n",i_p);
+ pc.printf("X:%d Y:%d Z:%d \r\n",posicao_x,posicao_y,posicao_z);
+ float posicao_x_f = passo_mm_xy(posicao_x);
+ float posicao_y_f = passo_mm_xy(posicao_y);
+ float posicao_z_f = passo_mm_z(posicao_z);
+ lcd.printf("Pontos %d \n",i_p);
+ lcd.printf("X:%.0f Y:%.0f Z:%.0f\n",posicao_x_f,posicao_y_f,posicao_z_f);
+ lcd.printf(" Confirmar\n");
+ lcd.printf(" Voltar\n");
+ lcd.locate(0,linha);
+ lcd.putc('~');
+
+
+
+ menu = 3;
+ menu_enter_2 = 0;//para o refresh rate
+ }// Adiciona Pontos
+ //-------------------------------------------------------------------------------------Cola nos pontos? -----------------------------------------------------------------
+ if (menu_enter_2 == 2)
+ {
+ pc.printf("Cola pontos?\r\n");
+ lcd.cls();
+ linha = 2;
+ lcd.printf("Colar entre?\n");
+ pc.printf("flag_mod2 = %d\r\n",flag_mod2);
+ if ( flag_mod2 == 0)
+ {
+ lcd.printf(" P%i e P%i\n",i_p - 2,i_p - 1);
+ }
+ if ( flag_mod2 == 1)
+ {
+ lcd.printf(" P%i e P%i\n",i_p - 1,i_p);
+ }
+ lcd.printf(" Sim\n");
+ lcd.printf(" Nao\n");
+ lcd.locate(0,linha);
+ lcd.putc('~');
+
+
+ menu = 4;
+ menu_enter_2 = 0;//para o refresh rate
+ }// Adiciona Pontos
+ //-------------------------------------------------------------------------------------MODIFICAR PONTOS -----------------------------------------------------------------
+ if (menu_enter_2 == 4)
+ {
+
+ pc.printf("Modificar Pontos\r\n");
+ pc.printf("i_p = %d ", i_p);
+ lcd.cls();
+ linha = 2;
+ pontos[i_p] = ponto;
+ pc.printf("Pontos %d\r\n",i_p);
+ pc.printf("X:%d Y:%d Z:%d \r\n",pontos_x[i_p],pontos_y[i_p],pontos_z[i_p]);
+ float posicao_x_f = passo_mm_xy(pontos_x[i_p]);
+ float posicao_y_f = passo_mm_xy(pontos_y[i_p]);
+ float posicao_z_f = passo_mm_z(pontos_z[i_p]);
+ if (i_p != 0)
+ {
+ pc.printf("i_c = %d ", i_c);
+ if ( cola[i_p - 1] == 1)
+ {
+ lcd.printf("Ponto %d P%d->P%d=S",i_p,i_p-1 ,i_p);
+ lcd.printf("\n");
+ lcd.printf("X:%.0f Y:%.0f Z:%.0f\n",posicao_x_f,posicao_y_f,posicao_z_f);
+ }
+ if ( cola[i_p - 1] == 0)
+ {
+ lcd.printf("Ponto %d P%d->P%d=N",i_p,i_p-1 ,i_p);
+ lcd.printf("\n");
+ lcd.printf("X:%.0f Y:%.0f Z:%.0f\n",posicao_x_f,posicao_y_f,posicao_z_f);
+ }
+ }
+
+ if ( i_p == 0)
+ {
+
+ lcd.printf("Ponto 0 ");
+ lcd.printf("\n");
+ lcd.printf("X:%.0f Y:%.0f Z:%.0f\n",posicao_x_f,posicao_y_f,posicao_z_f);
+ lcd.printf(" Proximo\n");
+ lcd.printf(" Voltar Modificar\n");
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ if (i_p != 0 and i_p != ultimo_p -1)
+ {
+ lcd.printf(" Anterior Proximo\n");
+ lcd.printf(" Voltar Modificar\n");
+ lcd.locate(10,linha);
+ lcd.putc('~');
+ }
+ pc.printf("ultimo_p = %d \r\n",ultimo_p);
+ if (i_p != 0 and i_p == ultimo_p -1)
+ {
+ lcd.printf(" Anterior \n");
+ lcd.printf(" Voltar Modificar\n");
+ lcd.locate(0,linha);
+ lcd.putc('~');
+ linha = 5;
+ }
+
+ menu = 5;
+ menu_enter_2 = 0;//para o refresh rate
+ }// Adiciona Pontos
+ //----------------------------------------------------------------------------------------------Configuracoes--------------------------------------------------------------
+ if ( menu_enter == 5)
+ {
+ pc.printf("Configuracoes\r\n");
+ lcd.cls();
+ linha = 0;
+ lcd.printf(" -Vel:%d[mm/s]\n",vel);
+ //lcd.printf(" -Cola nos Pontos?\n");
+ //lcd.printf(" Sim \n");
+ lcd.printf(" \n");
+ lcd.printf(" \n");
+ lcd.printf(" Voltar\n");
+ lcd.locate(0,linha);
+ lcd.putc('~');
+
+
+ menu = 6;
+ menu_enter = 0;//para o refresh rate
+ wait(0.1);
+ }
+
+ //-----------------------------------REFERENCIANDO-----------------------------------------------------------/
+ if(refe== 1) //comeca referenciamento roda uma vez
+ {
+
+ //pc.printf("refe == 1 \r\n");
+ enable_x = 0;
+ enable_y = 1;
+ enable_z = 1;
+ dir_x = 1;
+ //---------------IHM----------
+ lcd.cls();
+ lcd.printf(" Referenciando...\n");
+ wait(0.25);
+ //-----------------------------
+
+ move_inicial(dir_x,600,FimCursoX,0);
+ enable_x = 1;
+ // wait(2);
+ if (FimCursoX == 1)
+ {
+ pc.printf("Fim de Curso 1 X \r\n");
+ // refe X
+ enable_x = 0;
+ dir_x = !dir_x ;
+ movepara(dir_x,600,200,0);
+ enable_x = 1;
+ posicao_x = 1;
+ pc.printf("Fim de Curso 2 X \r\n");
+ }
+ enable_x = 1;
+ enable_y = 0;
+ enable_z = 1;
+ dir_y =1;
+ move_inicial(dir_y,600,FimCursoY,1);
+ enable_y = 1;
+ if (FimCursoY == 1)
+ {
+ pc.printf("Fim de Curso 1 Y \r\n");
+ // refe Y
+ enable_y = 0;
+ dir_y = !dir_y ;
+ movepara(dir_y,vel,200,1);
+ enable_y = 1;
+ refe= 0;
+ posicao_y = 1;
+ pc.printf("Fim de Curso 2 Y \r\n");
+ }
+
+ dir_z =1;
+ enable_x = 1;
+ enable_y = 1;
+ enable_z = 0;
+ move_inicial(dir_z,450,FimCursoZ,2);
+ enable_z = 1;
+
+ if (FimCursoZ == 1)
+ {
+ pc.printf("Fim de Curso 1 Z \r\n");
+ // refe Y
+ enable_z = 0;
+ dir_z = !dir_z ;
+ movepara(dir_z,450,200,2);
+ enable_z = 1;
+ refe= 0;
+ posicao_z = 1;
+ pc.printf("Fim de Curso 2 Z \r\n");
+ lcd.cls();
+ //---------------IHM----------
+ lcd.printf("Referenciamento\n");
+ lcd.printf("Concluido!\n");
+ wait(1);
+ lcd.cls();
+ menu = 1;
+ //-----------------------------
+ }
+ }
+
+ //---------------------------------------------------------------------------Drivers-------------------------------------------------------------------------------------------------------/
+ //---------------------------------------------------------------------------LOOP DA MAQUINA-------------------------------------------------------------------------------------------------------/
+ if (mover_motores == 1 and menu == 3 and flag_emergencia2 == 0) //fica preso no loop
+ {
+
+ //pc.printf("refe == 0 \r\n");
+ //Lendo JoyStick Analogico
+ x = eixo_x.read();
+ y = eixo_y.read();
+ //pc.printf("Posicao/Vel X: %f Y: %f.\r\n", x, y);
+ //Acionamento motor de Passo via Joystick
+ //-----------------------------------------JOYSTICK X------------------------------------------------------/
+
+ while (x < 0.3f and flag_emergencia2 == 0)// X+
+ { enable_x = 0;
+ dir_x = 0;
+ //wait(espera);
+ x = eixo_x.read();
+ //pc.printf("x> \r\n");
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ if (dir_x == 0 and posicao_x < 25000)
+ {
+ posicao_x = posicao_x + move(dir_x, vel,posicao_x,0) ;
+ // menu_enter_2 = 1;
+
+ }
+ pc.printf("Posicao Passo X: %d. \r\n",posicao_x);
+ menu_enter_2 = 1;
+ }
+ while ( x > 0.7f and flag_emergencia2 == 0) //X-
+ { enable_x = 0;
+ dir_x = 1;
+ x = eixo_x.read();
+ // pc.printf("x < \r\n");
+ // pc.printf("dir_x = %B \r\n",dir_x);
+ if (botao_emergencia == 0 )
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ if (dir_x == 1 and posicao_x > 0)
+ {
+ posicao_x = posicao_x - move(dir_x, vel, posicao_x,0) ;
+ }
+ pc.printf("Posicao Passo X: %d. \r\n",posicao_x);
+ menu_enter_2 = 1;
+ }
+ if ( x > 0.3f and x < 0.73f)
+ {
+ para();
+ // menu_enter_2 = 1;
+ }
+ //-----------------------------------------JOYSTICK Y-------------------------------------------------------/
+ if (troca_yz == 0)
+ {
+ while (y < 0.3f and flag_emergencia2 == 0) //Y+
+ {
+ enable_y = 0;
+ dir_y = 0;
+ y = eixo_y.read();
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ if (dir_y == 0 and posicao_y <26000)
+ {
+ posicao_y = posicao_y + move(dir_y, vel,posicao_y,1) ;
+ }
+ pc.printf("Posicao Passo Y: %d. \r\n",posicao_y);
+ menu_enter_2 = 1;
+ }
+ while ( y > 0.7f and flag_emergencia2 == 0)//Y-
+ {
+ enable_y = 0;
+ dir_y = 1;
+ y = eixo_y.read();
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ if (dir_y == 1 and posicao_y > 0)
+ {
+ posicao_y = posicao_y - move(dir_y, vel, posicao_y,1) ;
+ }
+ pc.printf("Posicao Passo Y: %d. \r\n",posicao_y);
+ menu_enter_2 = 1;
+
+ }
+ if ( y > 0.3f and y < 0.7f)
+ {
+ para_();
+ // menu_enter_2 = 1;
+ }
+ }
+ //-----------------------------------------JOYSTICK Y PARA MEXER O Z------------------------------------------------------/
+ if ( troca_yz == 1)
+ {
+ while (y < 0.3f and flag_emergencia2 == 0) //Z-
+ {
+ enable_z = 0;
+ dir_z = 1;
+ y = eixo_y.read();
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ if (dir_z == 1 and posicao_z > 0)
+ {
+ posicao_z = posicao_z - move(dir_z, 500,posicao_z,2) ;
+ }
+ pc.printf("Posicao Passo Z: %d. \r\n",posicao_z);
+ menu_enter_2 = 1;
+ }
+ while ( y > 0.7f and flag_emergencia2 == 0)//Z+
+ {
+ enable_z = 0;
+ dir_z = 0;
+ y = eixo_y.read();
+ if (botao_emergencia == 0)
+ {
+ flag_emergencia2 = 1;
+ break;
+ }
+ if (dir_z == 0 and posicao_z < 2000)
+ {
+ posicao_z = posicao_z + move(dir_z, 500, posicao_z,2) ;
+ }
+ pc.printf("Posicao Passo Z: %d. \r\n",posicao_z);
+ menu_enter_2 = 1;
+
+ }
+ if ( y > 0.3f and y < 0.7f)
+ {
+ para_();
+ //menu_enter_2 = 1;
+ }
+ }//fim troca_yz
+ //-----------------------------------------BOTAO ANALOGICO PARA TROCAR O EIXO Z/Y --------------------------------------------/
+
+ if (botao_analog == 0)
+ { troca_yz = !troca_yz; //troca Y/Z
+
+ pc.printf("ZY");
+ lcd.cls();
+ if (troca_yz == 1)
+ {
+ lcd.printf(" Eixo Z Selecionado \n");
+ }
+ if (troca_yz == 0)
+ {
+ lcd.printf(" Eixo Y Selecionado \n");
+ }
+ wait(0.8);
+ menu_enter_2 = 1;
+ }
+
+ //-----------------------------------------GUARDAR PONTOS (BOTAO) --------------------------------------------/
+ if ((guardar_pontos == 1 or botaoC == 0) and posicao_x >= 0 and posicao_y >= 0 and posicao_z >= 0 )
+ { //-------------------------------------------------X--------------------------------/
+
+ pc.printf("GUARDAR PONTOS\r\n");
+ if ( i_x != 0 )
+ {
+ pontos_x[i_x] = posicao_x;
+ //pc.printf(" Ponto[%i] Passo: %i \r\n",i_x,pontos_x[i_x]);
+ i_x++;
+ }
+ if ( i_x == 0 )
+ {
+ pontos_x[i_x] = posicao_x;
+ //pc.printf(" Ponto[%i] Passo: %i \r\n",i_x,pontos_x[i_x]);
+ i_x++;
+ }
+
+ /*if (posicao_x != pontos_x[i_x-1])
+ {
+ pontos_x[i_x] = posicao_x;
+ // pc.printf(" Ponto[%i] Passo: %i \r\n",i_x,pontos_x[i_x]);
+ i_x++;
+ }*/
+
+ //-------------------------------------------------Y--------------------------------/
+
+ if ( i_y != 0 )
+ {
+ pontos_y[i_y] = posicao_y;
+ //pc.printf(" Ponto[%i] Passo: %i \r\n",i_y,pontos_y[i_y]);
+ i_y++;
+ }
+ if ( i_y == 0 )
+ {
+ pontos_y[i_y] = posicao_y;
+ //pc.printf(" Ponto[%i] Passo: %i \r\n",i_y,pontos_y[i_y]);
+ i_y++;
+ }
+ /*if (posicao_y != pontos_y[i_y-1])
+ {
+ pontos_y[i_y] = posicao_y;
+ //pc.printf(" Ponto[%i] Passo: %i \r\n",i_y,pontos_y[i_y]);
+ i_y++;
+ }*/
+ // pc.printf(" Pontos X[%i] Y[%i] Passo X: %i Passo Y: %i\r\n",i_x,i_y, pontos_x[i_x -1],pontos_y[i_y -1]);
+
+ //-------------------------------------------------Z--------------------------------/
+
+ if ( i_z != 0 )
+ {
+ pontos_z[i_z] = posicao_z;
+ //pc.printf(" Ponto[%i] Passo: %i \r\n",i_z,pontos_y[i_z]);
+ i_z++;
+ }
+ if ( i_z == 0 )
+ {
+ pontos_z[i_z] = posicao_z;
+ // pc.printf(" Ponto[%i] Passo: %i \r\n",i_z,pontos_z[i_z]);
+ i_z++;
+ }
+ /* if (posicao_z != pontos_z[i_z-1])
+ {
+ pontos_y[i_z] = posicao_z;
+ //pc.printf(" Ponto[%i] Passo: %i \r\n",i_z,pontos_y[i_z]);
+ i_z++;
+ }*/
+ pc.printf(" Pontos X[%i]=%i Pontos Y[%i]=%i Pontos Z[%i]=%i \r\n",i_x, pontos_x[i_x -1],i_y, pontos_y[i_y -1],i_z, pontos_z[i_z -1]);
+ guardar_pontos = 0;
+ wait(0.5);
+ }
+ }//end mover_motores == 1 and menu == 3
+
+ if (mover_motores == 1 and menu == 2)
+ {
+ //-----------------------------------------IR PARA OS PONTOS GUARDADOS (BOTAO) -----------------------------------------/
+
+ if (colar_pontos == 1 or botaoA == 0)
+ {
+
+ lcd.cls();
+ lcd.printf("Aplicando Cola!\n");
+ pc.printf("colar pontos funcao! \r\n");
+ aciona_pneu = 0;//liga a pressao da valvula
+ for(int h = 0; h < (i_z); h++)
+ {
+
+ //-----------------------------------------Aciona a pneumatica -----------------------------------------/
+ if (cola[h - 1] == 1)
+ {
+ aciona_pneu2 = 0;//abre valvula
+
+ }
+
+ wait(1);
+ pc.printf(" Indo para os Pontos \r\n");
+ //-----------------------------------------pontos X -----------------------------------------/
+ pc.printf(" Pontos X \r\n");
+
+ int set_point_x = pontos_x[h] - posicao_x; //o quanto de passos que o motor ira dar para chegar ao ponto desejado
+ //pc.printf("SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point, pontos_x[h],h);
+ if (set_point_x != 0) //o ponto atual é menor do que o desejado
+ {
+ enable_x = 0;
+
+ }
+
+ if (set_point_x == 0) //o ponto atual é menor do que o desejado
+ {
+ enable_x = 1;
+ enable_y = 1;
+ enable_z = 1;
+ }
+ if (set_point_x > 0) //o ponto atual é menor do que o desejado
+ { // pc.printf("maior q 0 \r\n");
+ // pc.printf(" SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point, pontos_x[h],h);
+ dir_x = 0;
+ posicao_x = posicao_x + movepara(dir_x,vel,set_point_x,0);
+ }
+
+ if (set_point_x < 0) // o ponto atual esta a frente do desejado
+ { // pc.printf("menor q 0 \r\n");
+ // pc.printf(" SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point, pontos_x[h],h);
+ dir_x = 1;
+ posicao_x = posicao_x - movepara(dir_x,vel,set_point_x,0);
+ }
+ //enable_x = 1 ;//desligar o motor na espera
+ //pc.printf("Posicao Passo: %d. \r\n",posicao_x);
+ // wait(1); //espera para ir ao proximo ponto
+
+ //-----------------------------------------pontos y -----------------------------------------/
+ pc.printf(" Pontos Y \r\n");
+ int set_point_y = pontos_y[h] - posicao_y; //o quanto de passos que o motor ira dar para chegar ao ponto desejado
+
+ //pc.printf("SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point_y, pontos_y[h],h);
+ if (set_point_y != 0) //o ponto atual é menor do que o desejado
+ {
+ enable_x = 1;
+ enable_y = 0;
+ enable_z = 1;
+ }
+ if (set_point_y == 0) //o ponto atual é menor do que o desejado
+ {
+ enable_x = 1;
+ enable_y = 1;
+ enable_z = 1;
+ }
+ if (set_point_y > 0) //o ponto atual é menor do que o desejado
+ { //pc.printf("maior q 0 \r\n");
+ // pc.printf(" SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point_y, pontos_y[h],h);
+ dir_y = 0;
+ posicao_y = posicao_y + movepara(dir_y,vel,set_point_y,1);
+ }
+
+ if (set_point_y < 0) // o ponto atual esta a frente do desejado
+ { // pc.printf("menor q 0 \r\n");
+ //pc.printf(" SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point_y, pontos_y[h],h);
+ dir_y = 1;
+ posicao_y = posicao_y - movepara(dir_y,vel,set_point_y,1);
+ }
+ //enable_y = 1 ;//desligar o motor na espera
+ // pc.printf("Posicao Passo X: %d Passo Y: %d . \r\n",posicao_x,posicao_y);
+ // wait(1); //espera 2 segundos para ir ao proximo ponto
+ //-----------------------------------------pontos z -----------------------------------------/
+ pc.printf(" Pontos Z \r\n");
+
+ int set_point_z = pontos_z[h] - posicao_z; //o quanto de passos que o motor ira dar para chegar ao ponto desejado
+ //pc.printf("SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point_y, pontos_y[h],h);
+ if (set_point_z != 0) //o ponto atual é menor do que o desejado
+ {
+ enable_x = 1;
+ enable_y = 1;
+ enable_z = 0;
+ }
+ if (set_point_z == 0) //o ponto atual é menor do que o desejado
+ {
+ enable_x = 1;
+ enable_y = 1;
+ enable_z = 1;
+ }
+ if (set_point_z > 0) //o ponto atual é menor do que o desejado
+ { //pc.printf("maior q 0 \r\n");
+ // pc.printf(" SetPoint:%i pontos_z[h]:%i h:%i \r\n",set_point_z, pontos_z[h],h);
+ dir_z = 0; //vai para baixo
+ posicao_z = posicao_z + movepara(dir_z,vel,set_point_z,2);
+ }
+
+ if (set_point_z < 0) // o ponto atual esta a frente do desejado
+ { // pc.printf("menor q 0 \r\n");
+ //pc.printf(" SetPoint:%i pontos_x[h]:%i h:%i \r\n",set_point_y, pontos_y[h],h);
+
+ dir_z = 1; //vai para cima
+ posicao_z = posicao_z - movepara(dir_z,vel,set_point_z,2);
+ }
+ //enable_y = 1 ;//desligar o motor na espera
+ pc.printf("Posicao[%d] Passo X: %d Passo Y: %d Passo Z: %d. \r\n",h,posicao_x,posicao_y,posicao_z);
+ colar_pontos = 0;
+
+ // wait(1); //espera 2 segundos para ir ao proximo ponto
+
+ aciona_pneu2 = 1;//fecha valvula
+
+
+ }//loop for
+ enable_x = 1;
+ enable_y = 1;
+ enable_z = 1;
+ aciona_pneu = 1;//desliga a pressao da valvula
+ pc.printf("Acabou rotina aplicador de cola\n");
+ if (botao_emergencia == 1)
+ {
+ lcd.cls();
+ lcd.printf("Finalizado!\n");
+ wait(1);
+ menu_enter = 3;//volta para o menu
+ linha = 3;
+ }
+ }//if botaoA
+
+ }// end mover_motores == 1 and menu == 2
+
+ }//if botao emergencia == 1
+ if ((botao_emergencia == 0 and flag_emergencia == 0) or flag_emergencia2 == 1)
+ {
+ lcd.cls();
+ lcd.printf("Botao Emergencia!\n");
+ lcd.setCursor(TextLCD::CurOff_BlkOff);//desliga o cursor
+ refe = 0;
+ flag_emergencia = 1;
+ flag_emergencia2 = 0;
+ linha = 1;
+ }
+ }//while 1
+}//main
\ No newline at end of file