Como o projeto está até agora....
Dependencies: Servo TCS3200 X_NUCLEO_IHM01A1 mbed TextLCD2
Fork of QEI_HelloWorld by
main.cpp
- Committer:
- Victor_Mirkhan
- Date:
- 2017-05-29
- Revision:
- 3:77d4b108568c
- Parent:
- 2:666ad168bacb
- Child:
- 4:5dd6e95b5706
File content as of revision 3:77d4b108568c:
#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_11); // 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 float conversao = 3/1600; // A variavel é definida para converter a posição de steps para mm (depende do micro step); /*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; } /* Opção para o menu com os dois tipos de ciclo. Apenas para impedir que o usuario coloque o cursor em um campo vazio. */ if ((ref_cursor > 1) && (ref_menu == 2)) { ref_cursor = 0; } } void desce_cursor(void) { ref_cursor -= 1; if (ref_cursor < 0) { ref_cursor = 3; } /* Opção para o menu com os dois tipos de ciclo. Apenas para impedir que o usuario coloque o cursor em um campo vazio. */ if ((ref_cursor < 0) && (ref_menu == 2)) { ref_cursor = 1; } } /* 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_estatico(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_estatico(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); motorZ->set_min_speed(minspeed); motorZ->set_acceleration(ac); motorZ->set_deceleration(dc); } 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((red<green)&&(red<blue)) { tagy = 2; } if((green<red)&&(green<blue)&&(sensor)) { tagy = 0; } if((green<red)&&(green<blue)&&(!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 "seta_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_estatico(2); //Seta uma alta velocidade para o referenciamento set_aceleracoesZ(3500,1000,100,100); set_aceleracoesX(3500,1000,100,100); set_aceleracoesY(3500,1000,100,100); while(1) { /* Motor continua andando em uma só direção enquanto variavel referZ estiver em FALSE */ //Chamada do fim de curso para a função de interrupção FDC.fall(&motorZ_off); FDC2.fall(&motorX_off); FDC3.fall(&motorY_off); 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 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(); printf("Posicao Home = %d\r\n", HomePosition); //Verificar que HomePosition = 0 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) { //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; } 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; } } set_aceleracoesZ(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc); 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); } /* 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; printf("EIXO X\r\n"); muda_velocidade(); } void ativa_eixoY(void) { dirx = false; dirz = false; diry = true; printf("EIXO Y\r\n"); muda_velocidade(); } void ativa_eixoZ(void) { dirx = false; dirz = true; diry = false; printf("EIXO Z\r\n"); 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_estatico(3); // Ciclo infinito } if (n == 1) { menu_estatico(4); // Ciclo único } unsigned char 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,set[2].minspeed,set[2].ac,set[2].dc); 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; 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); motorZ->wait_while_active(); tag = reconhecimento_peca(); // Reconhece a peça e qual posição de Drop irá garra = 1; // Abre a garra wait(1); garra = 0.7; // Fecha a garra wait(1); // Vai para a posição de DROP motorZ->go_to(DropPos[tag].posx); motorZ->wait_while_active(); motorX->go_to(DropPos[tag].posy); motorX->wait_while_active(); motorY->go_to(DropPos[tag].posx); motorY->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 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*conversao); lcd.locate(3,2); lcd.printf("%.2f mm",posy*conversao); lcd.locate(3,3); lcd.printf("%.2f mm",posz*conversao); if (jog_pos == true) { if (dirx == true) { motorX->run(StepperMotor::FWD); motorX->wait_while_active(); } if (diry == true) { motorY->run(StepperMotor::FWD); motorY->wait_while_active(); } if (dirz == true) { motorZ->run(StepperMotor::FWD); motorZ->wait_while_active(); } } if (jog_neg == true) { if (dirx == true) { motorX->run(StepperMotor::BWD); motorX->wait_while_active(); } if (diry == true) { motorY->run(StepperMotor::BWD); motorY->wait_while_active(); } if (dirz == true) { 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) { if ((ref_menu == 1) || (ref_menu == 2)) { ref_cursor = 4; // O valor de ref_cursor em 4 chama um case que chama a primeira tela dinâmica; enable = true; } } 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_estatico(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); 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 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.fall(&back_op); if (enable == true) { switch(ref_cursor) { case 0: { // Posição de Pick jog(0); flag = false; break; } case 1: { // Posição de Drop 1 jog(1); flag = false; break; } case 2: { // Posição de Drop 2 jog(2); flag = false; break; } case 3: { // Posição de Drop 3 jog(3); 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; } } } } if (ref_menu == 2) { back_btn.fall(&back_op); 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); } }