Como o projeto está até agora....
Dependencies: Servo TCS3200 X_NUCLEO_IHM01A1 mbed TextLCD2
Fork of QEI_HelloWorld by
Diff: main.cpp
- Revision:
- 3:77d4b108568c
- Parent:
- 2:666ad168bacb
- Child:
- 4:5dd6e95b5706
diff -r 666ad168bacb -r 77d4b108568c main.cpp --- a/main.cpp Fri May 12 18:46:45 2017 +0000 +++ b/main.cpp Mon May 29 14:47:22 2017 +0000 @@ -1,256 +1,1215 @@ -#include "mbed.h" -#include "RPG.h" +#include "TextLCD.h" #include "L6474.h" +#include "TCS3200.h" +#include "Servo.h" -// Seta o encoder utilizado. Precisa de duas entradas (D14 e D15) e uma terceira -// que seria o seu botão. Como nosso encoder não tem um botão, declara-se qualquer -// pino, já que nem será utilizado. + +// Set do display LCD 20x4 com o módulo I2C -RPG rpg1(D14,D15,PC_13); +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. -// Pinos D15 e D14 são ligados ao encoder e setados como pinos de interrupção. -InterruptIn encod1(D15); -InterruptIn encod2(D14); +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 -// Botão do usuario é setado para interrupção, especialmente para a fase de -// prototipação. -InterruptIn usuario(PC_13); +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 -//PINOS DE SAIDA DIGITAL. - -// Declara o Led da placa como saída digital. -DigitalOut Led(D13); +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 refer1 = false; // Parametro para o referenciamento do motor 1 -bool refer2 = false; // Parametro para o referenciamento do motor 2 -bool refer3 = false; // Parametro para o referenciamento do motor 3 +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 -int count; // Contagem de pulsos do encoder -int change = 0; // Numero que varia de 0 a 2, indicando qual set de velocidade será usado -int dirt = 0; // Variavel que pega o estado presente do encoder +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. + +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 { - int maxspeed; - int minspeed; - int ac; - int dc; + 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 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. - -// Declaração dos motores utilizados. +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 *motor1; -L6474 *motor2; -L6474 *motor3; +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ção chamada ao atingir o sensor fim de curso. Para o motor e aciona o zera- -// mento ao alterar o valor da variavel refer. +/* 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; + } + + } +} -void motor1_off(void) { - motor1->hard_stop(); - printf("Interrupcao Feita!\r\n"); - refer1 = true; +/* 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 +*/ -// Função chamada na interrupção de um dos sentidos de rotação do encoder. Basicamente, -// desabilita as interrupções enquanto opera ("disable_irq()"), aumenta o contador e -// depois ativa as interrupções novamente ("enable_irq()"). +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)) { -void mais(void){ - encod2.disable_irq(); - encod1.disable_irq(); - count = count + 1 ; - wait(0.002); - encod1.enable_irq(); - encod2.enable_irq(); + 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 chamada na interrupção de um dos sentidos de rotação do encoder. Basicamente, -// desabilita as interrupções enquanto opera ("disable_irq()"), diminui o contador e -// depois ativa as interrupções novamente ("enable_irq()"). +/* 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. -void menos(void){ - encod2.disable_irq(); - encod1.disable_irq(); - count = count - 1; - wait(0.002); - encod1.enable_irq(); - encod2.enable_irq(); + 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(); } -// Função que seta para o motor 1 suas velocidades máxima e minima, aceleração -// e desaceleração. - -void set_aceleracoes(int maxspeed,int minspeed,int ac,int dc) { - motor1->set_max_speed(maxspeed); - motor1->set_min_speed(minspeed); - motor1->set_acceleration(ac); - motor1->set_deceleration(dc); +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); } -void save_pick(void) { - PickPos.posx = motor1->get_position(); - PickPos.posy = motor2->get_position(); - PickPos.posz = motor3->get_position(); - printf("Posicao de Pick:\r\n"); - printf("X = %d, Y = %d, Z = %d\r\n",PickPos.posx,PickPos.posy,PickPos.posz); +/* 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ção que é chamada logo na inicialização do programa para realizar o zera- -// mento das posições dos motores. + +/*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 seta_origem() { +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"); - set_aceleracoes(7000,2000,1000,500); //Seta uma alta aceleração para o referenciamento - usuario.rise(&motor1_off); //Chamada do fim de curso para a função de interrupção + //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) { - //Motor continua andando em uma só direção enquanto variavel refer1 estiver - // em FALSE + // 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(); - if (refer1 == false){ - motor1->run(StepperMotor::BWD); - motor1->wait_while_active(); - } - - // Se a interrupção for chamada, a variavel refer1 se torna TRUE, acionando - // os comandos a seguir. + 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); - else { - motor1->move(StepperMotor::FWD,1000); //Leve recuo - motor1->wait_while_active(); - motor1->set_home(); // Seta posição de Home - int HomePosition = motor1->get_position(); - printf("Posicao Home = %d\r\n",HomePosition); //Verificar que HomePosition = 0 - refer1 = false; - break; //Quebra do loop while, pois referenciamento do motor foi feito - } + // 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ção chamada para alternar entre os sets de velocidade. +/* 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); -void muda_velocidade() { - change = change + 1; - if (change == 3) { - change = 0; + 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(); + } + } } - printf("Velocidade %d\r\n",change + 1); - set_aceleracoes(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc); + save_pos = false; + ref_menu = 1; + flag = true; + conclusao(n); + menu_dinamico(ref_menu); } -int main() { - +/* 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 = 500; - set[0].dc = 500; - + set[0].ac = 100; + set[0].dc = 100; + set[1].maxspeed = 4000; - set[1].minspeed = 2000; - set[1].ac = 750; - set[1].dc = 750; + 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; + - set[2].maxspeed = 7000; - set[2].minspeed = 5000; - set[2].ac = 1000; - set[2].dc = 1000; - - + //Seta comunicação SPI DevSPI dev_spi(D11, D12, D13); //Inicialização dos componentes dos motores - motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi); - motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi); - motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi); - - if (motor1->init() != COMPONENT_OK) { + 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 (motor2->init() != COMPONENT_OK) { + if (motorX->init(&init) != COMPONENT_OK) { exit(EXIT_FAILURE); } - if (motor3->init() != COMPONENT_OK) { + 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); - // Após inicializar tudo, inicia o zeramento. - seta_origem(); + //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: - //Seta a velocidade inicial como a primeira combinação do set - set_aceleracoes(set[0].maxspeed,set[0].minspeed,set[0].ac,set[1].dc); - + -> 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) { - - //Declara as funções a serem chamadas nas bordas se subida de cada entrada. - encod1.rise(&mais); - encod2.rise(&menos); - usuario.rise(&muda_velocidade); - //usuario.rise(&save_pick); - - /*---PAR DE LOOPS PARA O JOG---*/ - - // Esse loop faz com que, caso e enquanto o contador for menor que zero, - // o motor proporcione o movimento BACKWARD. - - if (count < 0) { - while(count < 0) { - motor1->run(StepperMotor::BWD); - wait(0.1); - count = count + 1; //Acrescenta 1 ao contador a cada 0.1seg, aproximando-o de zero. - if (count >= 0) { //Se o contador mudar para 0 ou positivo, motor para e quebra o while. - motor1->hard_stop(); - count = 0; - break; + /* 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; + } + } } } - } - - // Esse loop faz com que, caso e enquanto o contador for maior que zero, - // o motor proporcione o movimento FOWARD. - - if (count > 0) { - while(count > 0) { - motor1->run(StepperMotor::FWD); - wait(0.1); - count = count - 1; //Decrescenta 1 ao contador a cada 0.1seg, aproximando-o de zero. - if (count <= 0) { //Se o contador mudar para 0 ou negativo, motor para e quebra o while. - motor1->hard_stop(); - count = 0; - 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); - count = 0; - + flag = true; + enable = false; + menu_dinamico(ref_menu); } } \ No newline at end of file