Projeto Mecatrônico / Mbed 2 deprecated CODIGO_FONTE

codigo fonte

Dependencies:   mbed TextLCD

main.cpp@4:4b994227bca2, 2021-06-04 (annotated)

Committer:
doradalberto
Date:
Fri Jun 04 21:48:17 2021 +0000
Revision:
4:4b994227bca2
Parent:
3:3aaa5905936e
Child:
5:452d20780aac
Ta funfando - Dora

Who changed what in which revision?

UserRevisionLine numberNew contents of line
diogonac 0:99c31d1bf4d3 1 #include "mbed.h"
diogonac 0:99c31d1bf4d3 2 #include "TextLCD.h"
diogonac 0:99c31d1bf4d3 3
diogonac 0:99c31d1bf4d3 4 TextLCD lcd(D8, D9, D4, D5, D6, D7); //Comunicação com LCD
diogonac 0:99c31d1bf4d3 5 BusOut motor_x(PC_8, PC_6, PC_5, PA_12); //Pinos para acionamento do motor do eixo X
diogonac 0:99c31d1bf4d3 6 BusOut motor_y(PA_11, PB_12, PB_11, PB_2); //Pinos para acionamento do motor do eixo Y
diogonac 0:99c31d1bf4d3 7 BusOut motor_z(PB_1, PB_15, PB_14, PB_13); //Pinos para acionamento do motor do eixo Z
diogonac 0:99c31d1bf4d3 8 DigitalOut sinal(PC_10); // Pino para o acionamento da pipeta
diogonac 0:99c31d1bf4d3 9 //DigitalIn botao_seleciona(PC_3); //botao de SELECT geral
diogonac 0:99c31d1bf4d3 10
diogonac 0:99c31d1bf4d3 11 Serial pc (D1, D0); //Comunicação com USB
diogonac 0:99c31d1bf4d3 12 AnalogIn joystick(PC_2); //Joystick potenciometro
diogonac 0:99c31d1bf4d3 13 //AnalogIn botao_SELECT(A0); //Botão SELECT da IHM
diogonac 0:99c31d1bf4d3 14 //Teste
diogonac 0:99c31d1bf4d3 15 InterruptIn botao_seleciona(PA_3); //Botão de salva posição Joystick
diogonac 0:99c31d1bf4d3 16
diogonac 0:99c31d1bf4d3 17 InterruptIn botao_emergencia_in(PC_4); //Botão de emergência para detectar acionamento
diogonac 0:99c31d1bf4d3 18 InterruptIn botao_fim_curso_rise(PC_12); //Chave de fim de curso
diogonac 0:99c31d1bf4d3 19
diogonac 0:99c31d1bf4d3 20
diogonac 0:99c31d1bf4d3 21 Timer debounce1; //Correção para botão de posição de salva
diogonac 0:99c31d1bf4d3 22 Timer debounce2; //Correção para botão de emergência
diogonac 0:99c31d1bf4d3 23 Timer debounce3; //Correção para chave de fim de curso
diogonac 0:99c31d1bf4d3 24
diogonac 0:99c31d1bf4d3 25 int pulsos = 0; //Contagem dos pulsos para o motor
diogonac 0:99c31d1bf4d3 26
diogonac 0:99c31d1bf4d3 27 int i; //Variável para contar o incremento no motor
diogonac 0:99c31d1bf4d3 28 int cont_fim_curso = 0; //Variável para contar o incremento do fim de curso
diogonac 0:99c31d1bf4d3 29 int J_XYZ; //Variável para ler o potenciometro do Joystick
diogonac 0:99c31d1bf4d3 30 int passo_fuso = 5; //Declara o passo do fuso
diogonac 0:99c31d1bf4d3 31 int pegaZ; //Variável que receberá o valor da posição de salva
diogonac 0:99c31d1bf4d3 32 int SELECT; //Variável para leitura do botão SELECT da IHM
diogonac 0:99c31d1bf4d3 33 int contador_SELECT = 0; //Define o contator do SELECT da IHM
diogonac 0:99c31d1bf4d3 34 int contador_emergencia = 0; //Define o contator do botão de emergência
diogonac 0:99c31d1bf4d3 35
diogonac 0:99c31d1bf4d3 36 int contador_rise = 0;
diogonac 0:99c31d1bf4d3 37 int seguranca_fdc = 0;
diogonac 0:99c31d1bf4d3 38
diogonac 0:99c31d1bf4d3 39 // ------------------Acionamento da Pipeta------------------
diogonac 0:99c31d1bf4d3 40 // Valores de Teste
diogonac 0:99c31d1bf4d3 41 int total_pega = 6; // quantidade de mL na posição de pega disponível
diogonac 0:99c31d1bf4d3 42 int quant_desejada = 3; // quantidade de mL na posição de solta 1
diogonac 0:99c31d1bf4d3 43 //int npos_solta = [1,2,3,4];
doradalberto 1:7ed1c2479638 44 int pos_solta_total = 4;
doradalberto 1:7ed1c2479638 45 float posZ_atual = 100;
doradalberto 1:7ed1c2479638 46 float posX_atual = 10;
doradalberto 1:7ed1c2479638 47 float posY_atual = 20;
diogonac 0:99c31d1bf4d3 48 float posX0_desejada = 0;
diogonac 0:99c31d1bf4d3 49 float posY0_desejada = 5;
diogonac 0:99c31d1bf4d3 50 float posZ0_desejada = 10;
diogonac 0:99c31d1bf4d3 51 float posX_desejada = 10;
diogonac 0:99c31d1bf4d3 52 float posY_desejada = 20;
diogonac 0:99c31d1bf4d3 53 float posZ_desejada = 90;
diogonac 0:99c31d1bf4d3 54
doradalberto 1:7ed1c2479638 55 float deslocamento_X = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 56 float deslocamento_Y = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 57 float deslocamento_Z = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 58
doradalberto 1:7ed1c2479638 59 float deslocamento_max_X = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 60 float deslocamento_max_Y = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 61 float deslocamento_max_Z = 0; //Define o deslocamento linear total
doradalberto 1:7ed1c2479638 62
diogonac 0:99c31d1bf4d3 63 float velocidade = 0.003; //Armazena a velocidade dos motores
diogonac 0:99c31d1bf4d3 64
diogonac 0:99c31d1bf4d3 65 int atual_pega = total_pega; // "total_pega" é a quantidade inserida pelo usuário na posição de pega
diogonac 0:99c31d1bf4d3 66 //int quant_desejada; // "quant desejada" é a quantidade inserida pelo usuário na posição de solta
diogonac 0:99c31d1bf4d3 67 int quant_depositada = 0;
diogonac 0:99c31d1bf4d3 68 int n_pulsosX;
diogonac 0:99c31d1bf4d3 69 int n_pulsosY;
diogonac 0:99c31d1bf4d3 70 int n_pulsosZ;
diogonac 0:99c31d1bf4d3 71 int posZ_origem = 0;
diogonac 0:99c31d1bf4d3 72 int deslocamentoZ_partida = posZ_origem - deslocamento_Z; // deslocamento do eixo Z até a posição de partida (origem)
diogonac 0:99c31d1bf4d3 73 int deslocamento_X0 = posX0_desejada - posX_atual; // deslocamento do eixo X até a posição de pega desejada
diogonac 0:99c31d1bf4d3 74 int deslocamento_Y0 = posY0_desejada - posY_atual; // deslocamento do eixo Y até a posição de pega desejada
diogonac 0:99c31d1bf4d3 75 int deslocamento_Z0 = posZ0_desejada - posZ_atual; // deslocamento do eixo Z até a posição de pega desejada
diogonac 0:99c31d1bf4d3 76 int deslocamento_solta_X = posX_desejada - posX_atual; // deslocamento do eixo X até a posição de solta desejada
diogonac 0:99c31d1bf4d3 77 int deslocamento_solta_Y = posY_desejada - posY_atual; // deslocamento do eixo Y até a posição de solta desejada
diogonac 0:99c31d1bf4d3 78 int deslocamento_solta_Z = posZ_desejada - posZ_atual; // deslocamento do eixo Z até a posição de solta desejada
diogonac 0:99c31d1bf4d3 79
diogonac 0:99c31d1bf4d3 80 // ------------------Acionamento da Pipeta------------------
diogonac 0:99c31d1bf4d3 81
diogonac 0:99c31d1bf4d3 82 float tempo_horario, tempo_anti_horario; //Define os tempos de acionamento do motor
diogonac 0:99c31d1bf4d3 83 float passo_motor = 5.625/32; //Declara o passo angular do motor
diogonac 0:99c31d1bf4d3 84 float tempo = 1.5; // Define o tempo que a pipeta fica acionada
diogonac 0:99c31d1bf4d3 85
diogonac 0:99c31d1bf4d3 86 int pulsos_Z = 0; //Contagem dos pulsos para o motor_Z
diogonac 0:99c31d1bf4d3 87 int pulsos_X = 0; //Contagem dos pulsos para o motor_X
diogonac 0:99c31d1bf4d3 88 int pulsos_Y = 0; //Contagem dos pulsos para o motor_Y
diogonac 0:99c31d1bf4d3 89
diogonac 0:99c31d1bf4d3 90 int contador_botao_seleciona = 0; //Boleana para a lógica do botão SELECT do joystick
diogonac 0:99c31d1bf4d3 91
diogonac 0:99c31d1bf4d3 92 bool valor_SELECT = false; //Boleana para lógica do botão SELECT da IHM
diogonac 0:99c31d1bf4d3 93 bool estado_referenciamento = false; //Boleana para identificar a necessidade do referênciamento
diogonac 0:99c31d1bf4d3 94 bool estado_botao_emergencia_in = false; //Boleana para identificar a necessidade do referênciamentosatisfazer a lógica da rotina_emergencia_in
diogonac 0:99c31d1bf4d3 95 bool motor_referenciamento = false; //Boleana para permitir o movimento automático do motot no referenciamento
diogonac 0:99c31d1bf4d3 96
diogonac 0:99c31d1bf4d3 97 // Rotinas do programa
diogonac 0:99c31d1bf4d3 98 void rotina_posicao_salva (void);
diogonac 0:99c31d1bf4d3 99 void rotina_emergencia_in (void);
diogonac 0:99c31d1bf4d3 100 void rotina_velocidade_eixo_Z (void);
diogonac 0:99c31d1bf4d3 101 void rotina_deslocamento_eixo_Z (void);
diogonac 0:99c31d1bf4d3 102 void rotina_deslocamento_eixo_menos_Z (void);
diogonac 0:99c31d1bf4d3 103 void rotina_JOG (void);
diogonac 0:99c31d1bf4d3 104 void rotina_botoes_IHM (void);
diogonac 0:99c31d1bf4d3 105 void Rotina_Pega (void);
diogonac 0:99c31d1bf4d3 106 void Rotina_Solta (void);
diogonac 0:99c31d1bf4d3 107 void teste2dora (void);
diogonac 0:99c31d1bf4d3 108 void reposiciona_pipeta_origem (void);
diogonac 0:99c31d1bf4d3 109
diogonac 0:99c31d1bf4d3 110 void rotina_fdc_rise (void);
diogonac 0:99c31d1bf4d3 111 void referenciamento (void);
diogonac 0:99c31d1bf4d3 112
diogonac 0:99c31d1bf4d3 113 void rotina_estado_botao_seleciona (void);
diogonac 0:99c31d1bf4d3 114
diogonac 0:99c31d1bf4d3 115 int main()
diogonac 0:99c31d1bf4d3 116 {
diogonac 0:99c31d1bf4d3 117 pc.baud(115200); //Define a velocidade da porta USB
diogonac 0:99c31d1bf4d3 118 motor_x = 0x00; //Estabelece a condição inicial do motor do eixo X
diogonac 0:99c31d1bf4d3 119 motor_y = 0x00; //Estabelece a condição inicial do motor do eixo Y
diogonac 0:99c31d1bf4d3 120 motor_z = 0x00; //Estabelece a condição inicial do motor do eixo Z
diogonac 0:99c31d1bf4d3 121
diogonac 0:99c31d1bf4d3 122 //Declara os pinos de interrupção com suas funções
diogonac 0:99c31d1bf4d3 123 botao_seleciona.rise(&rotina_estado_botao_seleciona);
diogonac 0:99c31d1bf4d3 124
diogonac 0:99c31d1bf4d3 125 botao_emergencia_in.rise(&rotina_emergencia_in);
diogonac 0:99c31d1bf4d3 126 botao_fim_curso_rise.rise(&rotina_fdc_rise);
diogonac 0:99c31d1bf4d3 127
diogonac 0:99c31d1bf4d3 128
diogonac 0:99c31d1bf4d3 129 //Inicializa os timers
diogonac 0:99c31d1bf4d3 130 debounce1.start();
diogonac 0:99c31d1bf4d3 131 debounce2.start();
diogonac 0:99c31d1bf4d3 132 debounce3.start();
diogonac 0:99c31d1bf4d3 133
diogonac 0:99c31d1bf4d3 134 //Inicia o LCD
diogonac 0:99c31d1bf4d3 135 wait_ms(500);
diogonac 0:99c31d1bf4d3 136 lcd.locate(2,0);
diogonac 0:99c31d1bf4d3 137 lcd.printf("SELECT para");
diogonac 0:99c31d1bf4d3 138 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 139 lcd.printf("referenciamento");
diogonac 0:99c31d1bf4d3 140
diogonac 0:99c31d1bf4d3 141 while(1) {
diogonac 0:99c31d1bf4d3 142 //Leitura de variáveis
diogonac 0:99c31d1bf4d3 143 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 144 //SELECT = botao_SELECT.read_u16();
diogonac 0:99c31d1bf4d3 145 //pc.printf("estado_referenciamento=%4d, estado_botao_emergencia_in=%d valor_SELECT=%d\r\n", estado_referenciamento, estado_botao_emergencia_in, valor_SELECT);
diogonac 0:99c31d1bf4d3 146 //pc.printf("deslocamento_Z=%4f, pulsos=%d\r\n", deslocamento_Z, pulsos);
diogonac 0:99c31d1bf4d3 147 //pc.printf("J_XYZ= %d\r\n", J_XYZ);
diogonac 0:99c31d1bf4d3 148 if(estado_referenciamento == true & estado_botao_emergencia_in == false & contador_botao_seleciona == 2) {
diogonac 0:99c31d1bf4d3 149 //Condição para poder disponibilizar a função JOG
diogonac 0:99c31d1bf4d3 150 //Chama a rotina do JOG
diogonac 0:99c31d1bf4d3 151 rotina_JOG();
diogonac 0:99c31d1bf4d3 152 //LCD
diogonac 0:99c31d1bf4d3 153
diogonac 0:99c31d1bf4d3 154 lcd.locate(2,0);
diogonac 0:99c31d1bf4d3 155 lcd.printf("PosicaoZ:");
diogonac 0:99c31d1bf4d3 156 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 157 lcd.printf("%4fmm",deslocamento_Z);
diogonac 0:99c31d1bf4d3 158 //pc.printf("J_XYZ= %d\r\n", J_XYZ);
diogonac 0:99c31d1bf4d3 159 }
diogonac 0:99c31d1bf4d3 160
diogonac 0:99c31d1bf4d3 161 else {
diogonac 0:99c31d1bf4d3 162 //Chama a rotina dos botões da IHM
diogonac 0:99c31d1bf4d3 163 //rotina_botoes_IHM();
diogonac 0:99c31d1bf4d3 164
diogonac 0:99c31d1bf4d3 165 if(valor_SELECT == false) { //se estiver 0, mudar para 1 -- no PC da dora fica 0 pela LCD dela
diogonac 0:99c31d1bf4d3 166
diogonac 0:99c31d1bf4d3 167 lcd.cls();
diogonac 0:99c31d1bf4d3 168 lcd.locate(2,0);
diogonac 0:99c31d1bf4d3 169 lcd.printf("Iniciando o");
diogonac 0:99c31d1bf4d3 170 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 171 lcd.printf("referenciamento");
diogonac 0:99c31d1bf4d3 172
diogonac 0:99c31d1bf4d3 173 pc.printf("Referenciamento \r\n");
diogonac 0:99c31d1bf4d3 174
diogonac 0:99c31d1bf4d3 175 contador_SELECT = 1;
diogonac 0:99c31d1bf4d3 176 estado_referenciamento = false;
diogonac 0:99c31d1bf4d3 177 //Permite o movimento automático do motor até atingir o fim de curso
diogonac 0:99c31d1bf4d3 178 referenciamento();
diogonac 0:99c31d1bf4d3 179
diogonac 0:99c31d1bf4d3 180 pc.printf("Referenciamento feito\r\n");
diogonac 0:99c31d1bf4d3 181 lcd.cls();
diogonac 0:99c31d1bf4d3 182 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 183 lcd.printf("Pressione SELECT");
diogonac 0:99c31d1bf4d3 184 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 185 lcd.printf("para continuar");
diogonac 0:99c31d1bf4d3 186
diogonac 0:99c31d1bf4d3 187 }
diogonac 0:99c31d1bf4d3 188 }
diogonac 0:99c31d1bf4d3 189
diogonac 0:99c31d1bf4d3 190 //========================================================================================
diogonac 0:99c31d1bf4d3 191
diogonac 0:99c31d1bf4d3 192 //while (npos_solta < pos_solta_total) {
diogonac 0:99c31d1bf4d3 193 //checa se existem mais posições de solta
diogonac 0:99c31d1bf4d3 194
doradalberto 4:4b994227bca2 195 if(botao_seleciona == 1) {
juan_ruiz 3:3aaa5905936e 196 //pressiona para entrar nesse if
juan_ruiz 3:3aaa5905936e 197 // o usuario nao quer alterar nenhum parametro
juan_ruiz 3:3aaa5905936e 198 pc.printf("pressionado\r\n");
juan_ruiz 3:3aaa5905936e 199
juan_ruiz 3:3aaa5905936e 200 quant_depositada = 0;
juan_ruiz 3:3aaa5905936e 201
juan_ruiz 3:3aaa5905936e 202 while (quant_depositada < quant_desejada) {
juan_ruiz 3:3aaa5905936e 203 // "quant_desejada" definido pelo usuário na IHM
juan_ruiz 3:3aaa5905936e 204
juan_ruiz 3:3aaa5905936e 205 if (atual_pega == 0) {
juan_ruiz 3:3aaa5905936e 206 lcd.cls();
juan_ruiz 3:3aaa5905936e 207 lcd.locate(0,0);
juan_ruiz 3:3aaa5905936e 208 lcd.printf("Coleta vazia");
juan_ruiz 3:3aaa5905936e 209 lcd.locate(0,1);
doradalberto 4:4b994227bca2 210 lcd.printf("Insira mais substrato");
juan_ruiz 3:3aaa5905936e 211 wait(4);
juan_ruiz 3:3aaa5905936e 212 pc.printf("Coleta vazia \r\n");
juan_ruiz 3:3aaa5905936e 213 exit(0); // obriga o usuário a recomeçar
doradalberto 4:4b994227bca2 214 //levanta_pipeta(); //void para levantar a pipeta até o zero
doradalberto 4:4b994227bca2 215 //ALTERAR: PIPETA VAI PARA A ORIGEM NO EIXO Z
juan_ruiz 3:3aaa5905936e 216 }
doradalberto 4:4b994227bca2 217 //referenciamento();
doradalberto 4:4b994227bca2 218 //break;
juan_ruiz 3:3aaa5905936e 219 else {
juan_ruiz 3:3aaa5905936e 220 Rotina_Pega();
juan_ruiz 3:3aaa5905936e 221 Rotina_Solta();
juan_ruiz 3:3aaa5905936e 222 }
juan_ruiz 3:3aaa5905936e 223
juan_ruiz 3:3aaa5905936e 224 atual_pega = atual_pega - 1;
juan_ruiz 3:3aaa5905936e 225 quant_depositada = quant_depositada + 1;
juan_ruiz 3:3aaa5905936e 226
juan_ruiz 3:3aaa5905936e 227 pc.printf("atual_pega: %d \r\n",atual_pega);
juan_ruiz 3:3aaa5905936e 228 pc.printf("quant_depositada: %d \r\n",quant_depositada);
juan_ruiz 3:3aaa5905936e 229 }
juan_ruiz 3:3aaa5905936e 230 }
juan_ruiz 3:3aaa5905936e 231 pc.printf("fora do if\r\n");
doradalberto 4:4b994227bca2 232 //referenciamento();
diogonac 0:99c31d1bf4d3 233 } //while(1)
diogonac 0:99c31d1bf4d3 234 } //int main()
diogonac 0:99c31d1bf4d3 235
diogonac 0:99c31d1bf4d3 236 void rotina_estado_botao_seleciona()
diogonac 0:99c31d1bf4d3 237 {
diogonac 0:99c31d1bf4d3 238 if(debounce1.read_ms() >= 10 & estado_referenciamento == true & estado_botao_emergencia_in == false) {
diogonac 0:99c31d1bf4d3 239 contador_botao_seleciona += 1;
diogonac 0:99c31d1bf4d3 240 pc.printf("contador_botao_seleciona= %d\r\n", contador_botao_seleciona);
diogonac 0:99c31d1bf4d3 241 lcd.cls();
diogonac 0:99c31d1bf4d3 242 }
diogonac 0:99c31d1bf4d3 243 debounce1.reset();
diogonac 0:99c31d1bf4d3 244 }
diogonac 0:99c31d1bf4d3 245
diogonac 0:99c31d1bf4d3 246 void rotina_emergencia_in()
diogonac 0:99c31d1bf4d3 247 {
diogonac 0:99c31d1bf4d3 248 if(debounce2.read_ms() >= 10) {
diogonac 0:99c31d1bf4d3 249
diogonac 0:99c31d1bf4d3 250 motor_x = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo X
diogonac 0:99c31d1bf4d3 251 motor_y = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Y
diogonac 0:99c31d1bf4d3 252 motor_z = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Z
diogonac 0:99c31d1bf4d3 253
diogonac 0:99c31d1bf4d3 254 estado_referenciamento = false; //Solicita um novo referenciamento
diogonac 0:99c31d1bf4d3 255 estado_botao_emergencia_in =! estado_botao_emergencia_in;
diogonac 0:99c31d1bf4d3 256 contador_emergencia += 1;
diogonac 0:99c31d1bf4d3 257
diogonac 0:99c31d1bf4d3 258 lcd.cls();
diogonac 0:99c31d1bf4d3 259 wait_ms(500);
diogonac 0:99c31d1bf4d3 260 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 261 lcd.printf("Emergencia ON");
diogonac 0:99c31d1bf4d3 262 lcd.locate(0,1);
diogonac 0:99c31d1bf4d3 263 lcd.printf("Pressione RST");
diogonac 0:99c31d1bf4d3 264
diogonac 0:99c31d1bf4d3 265 exit(0); //Encerra o programa e obriga o usuário a resetar o sistema
diogonac 0:99c31d1bf4d3 266
diogonac 0:99c31d1bf4d3 267 debounce2.reset();
diogonac 0:99c31d1bf4d3 268 }
diogonac 0:99c31d1bf4d3 269 }
diogonac 0:99c31d1bf4d3 270
diogonac 0:99c31d1bf4d3 271 void rotina_velocidade_eixo_Z()
diogonac 0:99c31d1bf4d3 272 {
diogonac 0:99c31d1bf4d3 273 //Converte a leitura do Joystick para tempo de acionamento do motor
diogonac 0:99c31d1bf4d3 274 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 275 tempo_horario = -1*J_XYZ*0.0000028 + 0.1875;
diogonac 0:99c31d1bf4d3 276 tempo_anti_horario = J_XYZ*0.000002785 + 0.0025;
diogonac 0:99c31d1bf4d3 277 }
diogonac 0:99c31d1bf4d3 278
diogonac 0:99c31d1bf4d3 279 void rotina_velocidade_referenciamento_eixo_Z()
diogonac 0:99c31d1bf4d3 280 {
diogonac 0:99c31d1bf4d3 281 //Converte a leitura do Joystick para tempo de acionamento do motor
diogonac 0:99c31d1bf4d3 282 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 283 tempo_horario = (-1*J_XYZ*0.0000028 + 0.1875)*2;
diogonac 0:99c31d1bf4d3 284 tempo_anti_horario = (J_XYZ*0.000002785 + 0.0025)*2;
diogonac 0:99c31d1bf4d3 285 }
diogonac 0:99c31d1bf4d3 286
diogonac 0:99c31d1bf4d3 287 void rotina_deslocamento_eixo()
diogonac 0:99c31d1bf4d3 288 {
diogonac 0:99c31d1bf4d3 289 //Calcula os deslocamentos no eixo Z
diogonac 0:99c31d1bf4d3 290
diogonac 0:99c31d1bf4d3 291 deslocamento_X = (passo_fuso*pulsos_X*passo_motor)/360;
diogonac 0:99c31d1bf4d3 292 deslocamento_Y = (passo_fuso*pulsos_Y*passo_motor)/360;
diogonac 0:99c31d1bf4d3 293 deslocamento_Z = (passo_fuso*pulsos_Z*passo_motor)/360;
diogonac 0:99c31d1bf4d3 294
diogonac 0:99c31d1bf4d3 295 // recebe_arduino.printf("X%3.2f\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 296 // recebe_arduino.printf("Y%3.2f\n", deslocamento_Y);
diogonac 0:99c31d1bf4d3 297 // recebe_arduino.printf("Z%3.2f\n", deslocamento_Z);
diogonac 0:99c31d1bf4d3 298
diogonac 0:99c31d1bf4d3 299 pc.printf("X%3.2f, Y%3.2f, Z%3.2f, J_XYZ=%d\n", deslocamento_X, deslocamento_Y, deslocamento_Z, J_XYZ);
diogonac 0:99c31d1bf4d3 300
diogonac 0:99c31d1bf4d3 301 }
diogonac 0:99c31d1bf4d3 302
diogonac 0:99c31d1bf4d3 303 //void rotina_botoes_IHM()
diogonac 0:99c31d1bf4d3 304 //{
diogonac 0:99c31d1bf4d3 305 // if(SELECT > 60000 & SELECT < 65000) valor_SELECT =! valor_SELECT;
diogonac 0:99c31d1bf4d3 306 // else valor_SELECT = 0;
diogonac 0:99c31d1bf4d3 307 //}
diogonac 0:99c31d1bf4d3 308
diogonac 0:99c31d1bf4d3 309 void referenciamento()
diogonac 0:99c31d1bf4d3 310 {
diogonac 0:99c31d1bf4d3 311
diogonac 0:99c31d1bf4d3 312 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_SELECT == 1) {//W1
diogonac 0:99c31d1bf4d3 313
diogonac 0:99c31d1bf4d3 314 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 315 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 316 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 317 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 318 wait(0.003);
diogonac 0:99c31d1bf4d3 319 }
diogonac 0:99c31d1bf4d3 320
diogonac 0:99c31d1bf4d3 321 if(contador_rise == 1) {//if1
diogonac 0:99c31d1bf4d3 322
diogonac 0:99c31d1bf4d3 323 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 1) {//W2
diogonac 0:99c31d1bf4d3 324
diogonac 0:99c31d1bf4d3 325 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 326 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 327 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 328 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 329 wait(0.003);
diogonac 0:99c31d1bf4d3 330
diogonac 0:99c31d1bf4d3 331 }
diogonac 0:99c31d1bf4d3 332
diogonac 0:99c31d1bf4d3 333 if(contador_rise == 2) {//if2
diogonac 0:99c31d1bf4d3 334
diogonac 0:99c31d1bf4d3 335 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 2) {//W3
diogonac 0:99c31d1bf4d3 336
diogonac 0:99c31d1bf4d3 337 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 338 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 339 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 340 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 341 wait(0.003);
diogonac 0:99c31d1bf4d3 342 }
diogonac 0:99c31d1bf4d3 343
diogonac 0:99c31d1bf4d3 344 if(contador_rise == 3) {//if3
diogonac 0:99c31d1bf4d3 345
diogonac 0:99c31d1bf4d3 346 while(seguranca_fdc != 8 & estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 3) {//W4
diogonac 0:99c31d1bf4d3 347 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 348 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 349 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 350 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 351 wait(0.003);
diogonac 0:99c31d1bf4d3 352
diogonac 0:99c31d1bf4d3 353 }
diogonac 0:99c31d1bf4d3 354 seguranca_fdc += 1;
diogonac 0:99c31d1bf4d3 355
diogonac 0:99c31d1bf4d3 356 }//Fecha W4
diogonac 0:99c31d1bf4d3 357
diogonac 0:99c31d1bf4d3 358 estado_referenciamento = true;
diogonac 0:99c31d1bf4d3 359 contador_SELECT = 0;
diogonac 0:99c31d1bf4d3 360 contador_rise = 0;
diogonac 0:99c31d1bf4d3 361 seguranca_fdc = 0;
diogonac 0:99c31d1bf4d3 362
diogonac 0:99c31d1bf4d3 363 valor_SELECT = true;
diogonac 0:99c31d1bf4d3 364 contador_botao_seleciona = 0;
diogonac 0:99c31d1bf4d3 365 deslocamento_X = 0; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 366 deslocamento_Y = 0; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 367 deslocamento_Z = 0; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 368 pulsos_X = 0;
diogonac 0:99c31d1bf4d3 369 pulsos_Y = 0;
diogonac 0:99c31d1bf4d3 370 pulsos_Z = 0;
diogonac 0:99c31d1bf4d3 371 deslocamento_max_X = 5; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 372 deslocamento_max_Y = 5; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 373 deslocamento_max_Z = 5; //Define o deslocamento linear total
diogonac 0:99c31d1bf4d3 374 // velocidade = 0.1;
diogonac 0:99c31d1bf4d3 375
diogonac 0:99c31d1bf4d3 376 // recebe_arduino.printf("D\n"); //Done referenciamento
diogonac 0:99c31d1bf4d3 377
diogonac 0:99c31d1bf4d3 378 }//Fecha if3
diogonac 0:99c31d1bf4d3 379 }//Fecha W3
diogonac 0:99c31d1bf4d3 380
diogonac 0:99c31d1bf4d3 381 }//Fecha if2
diogonac 0:99c31d1bf4d3 382 }//Fecha W2
diogonac 0:99c31d1bf4d3 383 }//Fecha if1
diogonac 0:99c31d1bf4d3 384 }//Fecha W1
diogonac 0:99c31d1bf4d3 385 motor_x = 0x00;
diogonac 0:99c31d1bf4d3 386 motor_y = 0x00;
diogonac 0:99c31d1bf4d3 387 motor_z = 0x00;
diogonac 0:99c31d1bf4d3 388 }//Fecha funcao
diogonac 0:99c31d1bf4d3 389
diogonac 0:99c31d1bf4d3 390
diogonac 0:99c31d1bf4d3 391 void rotina_fdc_rise()
diogonac 0:99c31d1bf4d3 392 {
diogonac 0:99c31d1bf4d3 393 if(debounce3.read_ms() >= 50 & estado_botao_emergencia_in == false & estado_referenciamento == false & contador_SELECT == 1) {
diogonac 0:99c31d1bf4d3 394 contador_rise += 1;
diogonac 0:99c31d1bf4d3 395 pc.printf("contador_rise=%d\n", contador_rise);
diogonac 0:99c31d1bf4d3 396 debounce3.reset();
diogonac 0:99c31d1bf4d3 397 }
diogonac 0:99c31d1bf4d3 398 }
diogonac 0:99c31d1bf4d3 399
diogonac 0:99c31d1bf4d3 400
diogonac 0:99c31d1bf4d3 401 void rotina_JOG()
diogonac 0:99c31d1bf4d3 402 {
diogonac 0:99c31d1bf4d3 403 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 404
diogonac 0:99c31d1bf4d3 405 if(32400 <= J_XYZ & J_XYZ <= 34100) {
diogonac 0:99c31d1bf4d3 406 motor_x = 0x00;
diogonac 0:99c31d1bf4d3 407 motor_y = 0x00;
diogonac 0:99c31d1bf4d3 408 motor_z = 0x00;
diogonac 0:99c31d1bf4d3 409 }
diogonac 0:99c31d1bf4d3 410
diogonac 0:99c31d1bf4d3 411 else {
diogonac 0:99c31d1bf4d3 412 //============== Movimenta Eixo Y ======================================
diogonac 0:99c31d1bf4d3 413
diogonac 0:99c31d1bf4d3 414 if (J_XYZ > 34100 & deslocamento_Y > 0 & pulsos_Y > 0) {
diogonac 0:99c31d1bf4d3 415 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 416 // while(deslocamento_Y > 0) {
diogonac 0:99c31d1bf4d3 417 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 418 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 419 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 420 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 421 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 422 pulsos_Y-=1;
diogonac 0:99c31d1bf4d3 423 wait(velocidade);
diogonac 0:99c31d1bf4d3 424
diogonac 0:99c31d1bf4d3 425 }
diogonac 0:99c31d1bf4d3 426 // }
diogonac 0:99c31d1bf4d3 427 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 428 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 429 }
diogonac 0:99c31d1bf4d3 430
diogonac 0:99c31d1bf4d3 431 if(J_XYZ < 32400 & deslocamento_max_Y > deslocamento_Y & pulsos_Y < 2048) {
diogonac 0:99c31d1bf4d3 432 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 433 // while(deslocamento_max_Y > deslocamento_Y) {
diogonac 0:99c31d1bf4d3 434 J_XYZ = joystick.read_u16();
diogonac 0:99c31d1bf4d3 435 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 436 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 437 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 438 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 439 pulsos_Y+=1;
diogonac 0:99c31d1bf4d3 440 wait(velocidade);
diogonac 0:99c31d1bf4d3 441
diogonac 0:99c31d1bf4d3 442 }
diogonac 0:99c31d1bf4d3 443 // }
diogonac 0:99c31d1bf4d3 444 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 445 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 446 }
diogonac 0:99c31d1bf4d3 447
diogonac 0:99c31d1bf4d3 448 //============== Movimenta Eixo X ======================================
diogonac 0:99c31d1bf4d3 449
diogonac 0:99c31d1bf4d3 450 if (J_XYZ > 34100 & deslocamento_X > 0 & pulsos_X > 0) {
diogonac 0:99c31d1bf4d3 451 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 452 // while(deslocamento_X > 0) {
diogonac 0:99c31d1bf4d3 453 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 454 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 455 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 456 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 457 pulsos_X-=1;
diogonac 0:99c31d1bf4d3 458 wait(velocidade);
diogonac 0:99c31d1bf4d3 459
diogonac 0:99c31d1bf4d3 460 }
diogonac 0:99c31d1bf4d3 461 // }
diogonac 0:99c31d1bf4d3 462 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 463 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 464 }
diogonac 0:99c31d1bf4d3 465
diogonac 0:99c31d1bf4d3 466 if(J_XYZ < 32400 & deslocamento_max_X > deslocamento_X & pulsos_X < 2048) {
diogonac 0:99c31d1bf4d3 467 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 468 // while(deslocamento_max_X > deslocamento_X) {
diogonac 0:99c31d1bf4d3 469 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 470 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 471 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 472 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 473 pulsos_X+=1;
diogonac 0:99c31d1bf4d3 474 wait(velocidade);
diogonac 0:99c31d1bf4d3 475
diogonac 0:99c31d1bf4d3 476 }
diogonac 0:99c31d1bf4d3 477 // }
diogonac 0:99c31d1bf4d3 478 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 479 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 480 }
diogonac 0:99c31d1bf4d3 481
diogonac 0:99c31d1bf4d3 482
diogonac 0:99c31d1bf4d3 483 //============== Movimenta Eixo Z ======================================
diogonac 0:99c31d1bf4d3 484
diogonac 0:99c31d1bf4d3 485 if (J_XYZ > 34100 & deslocamento_Z > 0 & pulsos_Z > 0) {
diogonac 0:99c31d1bf4d3 486 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 487 // while(deslocamento_Z > 0) {
diogonac 0:99c31d1bf4d3 488 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 489 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 490 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 491 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 492 pulsos_Z-=1;
diogonac 0:99c31d1bf4d3 493 wait(velocidade);
diogonac 0:99c31d1bf4d3 494
diogonac 0:99c31d1bf4d3 495 }
diogonac 0:99c31d1bf4d3 496 // }
diogonac 0:99c31d1bf4d3 497 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 498 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 499 }
diogonac 0:99c31d1bf4d3 500
diogonac 0:99c31d1bf4d3 501 if(J_XYZ < 32400 & deslocamento_max_Z > deslocamento_Z & pulsos_Z < 2048) {
diogonac 0:99c31d1bf4d3 502 // tempo_motor.start();
diogonac 0:99c31d1bf4d3 503 // while(deslocamento_max_Z > deslocamento_Z) {
diogonac 0:99c31d1bf4d3 504 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 505 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 506 // rotina_velocidade_eixo();
diogonac 0:99c31d1bf4d3 507 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 508 pulsos_Z+=1;
diogonac 0:99c31d1bf4d3 509 wait(velocidade);
diogonac 0:99c31d1bf4d3 510
diogonac 0:99c31d1bf4d3 511 }
diogonac 0:99c31d1bf4d3 512 // }
diogonac 0:99c31d1bf4d3 513 // tempo_motor.reset();
diogonac 0:99c31d1bf4d3 514 // tempo_motor.stop();
diogonac 0:99c31d1bf4d3 515 }
diogonac 0:99c31d1bf4d3 516
diogonac 0:99c31d1bf4d3 517
diogonac 0:99c31d1bf4d3 518 }
diogonac 0:99c31d1bf4d3 519 }
diogonac 0:99c31d1bf4d3 520
diogonac 0:99c31d1bf4d3 521
diogonac 0:99c31d1bf4d3 522 void Rotina_Pega()
diogonac 0:99c31d1bf4d3 523 {
diogonac 0:99c31d1bf4d3 524 // ------------ LEIA ----------
diogonac 0:99c31d1bf4d3 525 //Ele ta pulando o while do Z e X e indo direto pro Z -- ele não tá nem entrando nesse while
diogonac 0:99c31d1bf4d3 526 sinal = 0; // saída nível lógico "baixo"
diogonac 0:99c31d1bf4d3 527
doradalberto 1:7ed1c2479638 528 posZ_atual = deslocamento_Z; // posZ_atual = a posição de deslocamento neste momento
diogonac 0:99c31d1bf4d3 529 if(posZ_atual > posZ_origem) {
diogonac 0:99c31d1bf4d3 530 while (deslocamento_Z > posZ_origem) {
diogonac 0:99c31d1bf4d3 531 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 532 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 533 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 534 pulsos_Z -= 1;
diogonac 0:99c31d1bf4d3 535 wait(velocidade);
diogonac 0:99c31d1bf4d3 536 rotina_deslocamento_eixo();
doradalberto 1:7ed1c2479638 537 pc.printf("Pos atualZ AH: %f\r\n", deslocamento_Z);
diogonac 0:99c31d1bf4d3 538 }
diogonac 0:99c31d1bf4d3 539 }
diogonac 0:99c31d1bf4d3 540 }
diogonac 0:99c31d1bf4d3 541 else { //não faz nada
diogonac 0:99c31d1bf4d3 542 // while (deslocamento_Z < posZ0_desejada) {
diogonac 0:99c31d1bf4d3 543 // //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 544 // for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 545 // motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 546 // pulsos_Z -= 1
diogonac 0:99c31d1bf4d3 547 // wait(velocidade);
diogonac 0:99c31d1bf4d3 548 // rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 549 // pc.printf("Pos atualZ H: %d\r\n", deslocamento_Z);
doradalberto 1:7ed1c2479638 550 // }
juan_ruiz 2:706603d65abc 551 // }
diogonac 0:99c31d1bf4d3 552 }
doradalberto 1:7ed1c2479638 553 posX_atual = deslocamento_X;
diogonac 0:99c31d1bf4d3 554 if(posX_atual < posX0_desejada) {
diogonac 0:99c31d1bf4d3 555 while (deslocamento_X > posX0_desejada) {
diogonac 0:99c31d1bf4d3 556 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 557 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 558 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 559 pulsos_X += 1;
diogonac 0:99c31d1bf4d3 560 wait(velocidade);
diogonac 0:99c31d1bf4d3 561 rotina_deslocamento_eixo();
juan_ruiz 2:706603d65abc 562 pc.printf("Pos atualX AH: %f\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 563 }
diogonac 0:99c31d1bf4d3 564 }
diogonac 0:99c31d1bf4d3 565 } else {
diogonac 0:99c31d1bf4d3 566 while (deslocamento_X < posX0_desejada) {
diogonac 0:99c31d1bf4d3 567 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 568 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 569 motor_x = 1 << i;
diogonac 0:99c31d1bf4d3 570 pulsos_X -= 1;
diogonac 0:99c31d1bf4d3 571 wait(velocidade);
diogonac 0:99c31d1bf4d3 572 rotina_deslocamento_eixo();
juan_ruiz 2:706603d65abc 573 pc.printf("Pos atualX H: %f\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 574 }
diogonac 0:99c31d1bf4d3 575 }
diogonac 0:99c31d1bf4d3 576 }
doradalberto 1:7ed1c2479638 577 posY_atual = deslocamento_Y;
diogonac 0:99c31d1bf4d3 578 if(posY_atual < posY0_desejada) {
diogonac 0:99c31d1bf4d3 579 while (posY_atual > posY0_desejada) {
diogonac 0:99c31d1bf4d3 580 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 581 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 582 pulsos_Y += 1;
diogonac 0:99c31d1bf4d3 583 wait(velocidade);
diogonac 0:99c31d1bf4d3 584 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 585 }
diogonac 0:99c31d1bf4d3 586 }
diogonac 0:99c31d1bf4d3 587 }
diogonac 0:99c31d1bf4d3 588
diogonac 0:99c31d1bf4d3 589 else {
diogonac 0:99c31d1bf4d3 590 while (posY_atual < posY0_desejada) {
diogonac 0:99c31d1bf4d3 591 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 592 motor_y = 1 << i;
diogonac 0:99c31d1bf4d3 593 pulsos_Y -= 1;
diogonac 0:99c31d1bf4d3 594 wait(velocidade);
diogonac 0:99c31d1bf4d3 595 rotina_deslocamento_eixo();
diogonac 0:99c31d1bf4d3 596 }
diogonac 0:99c31d1bf4d3 597 }
diogonac 0:99c31d1bf4d3 598 }
diogonac 0:99c31d1bf4d3 599
doradalberto 1:7ed1c2479638 600 posZ_atual = deslocamento_Z;
diogonac 0:99c31d1bf4d3 601 if(posZ_atual < posZ0_desejada) {
diogonac 0:99c31d1bf4d3 602 while (deslocamento_Z > posZ0_desejada) {
diogonac 0:99c31d1bf4d3 603 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 604 for(i = 3; i > -1; i--) {
diogonac 0:99c31d1bf4d3 605 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 606 pulsos_Z += 1;
diogonac 0:99c31d1bf4d3 607 wait(velocidade);
diogonac 0:99c31d1bf4d3 608 rotina_deslocamento_eixo();
juan_ruiz 2:706603d65abc 609 pc.printf("Pos atualZ AH: %f\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 610 }
diogonac 0:99c31d1bf4d3 611 }
diogonac 0:99c31d1bf4d3 612 } else {
diogonac 0:99c31d1bf4d3 613 while (deslocamento_Z < posZ0_desejada) {
diogonac 0:99c31d1bf4d3 614 //While para verificar que o motor funcione até a a posição desejada ser alcançada
diogonac 0:99c31d1bf4d3 615 for(i = 0; i < 4; i++) {
diogonac 0:99c31d1bf4d3 616 motor_z = 1 << i;
diogonac 0:99c31d1bf4d3 617 pulsos_Z -= 1;
diogonac 0:99c31d1bf4d3 618 wait(velocidade);
diogonac 0:99c31d1bf4d3 619 rotina_deslocamento_eixo();
juan_ruiz 2:706603d65abc 620 pc.printf("Pos atualX H: %f\r\n", deslocamento_X);
diogonac 0:99c31d1bf4d3 621 }
diogonac 0:99c31d1bf4d3 622 }
diogonac 0:99c31d1bf4d3 623 }
diogonac 0:99c31d1bf4d3 624
diogonac 0:99c31d1bf4d3 625
diogonac 0:99c31d1bf4d3 626 sinal = 1; // saída nível lógico "alto"
diogonac 0:99c31d1bf4d3 627 lcd.cls();
diogonac 0:99c31d1bf4d3 628 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 629 lcd.printf("Coletando");
diogonac 0:99c31d1bf4d3 630 pc.printf("Coletando\r\n");
diogonac 0:99c31d1bf4d3 631 wait(tempo);
diogonac 0:99c31d1bf4d3 632
diogonac 0:99c31d1bf4d3 633 }
diogonac 0:99c31d1bf4d3 634
diogonac 0:99c31d1bf4d3 635 void Rotina_Solta()
diogonac 0:99c31d1bf4d3 636 {
diogonac 0:99c31d1bf4d3 637 sinal = 0; // saída nível lógico "baixo"
diogonac 0:99c31d1bf4d3 638
diogonac 0:99c31d1bf4d3 639 n_pulsosZ = (1/(passo_motor*passo_fuso))*360*deslocamentoZ_partida; // desloca a pipeta no eixo Z para sua posição de partida para evitar colisão
diogonac 0:99c31d1bf4d3 640
diogonac 0:99c31d1bf4d3 641 n_pulsosX = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_X; //movimentação do eixo X para posição de pega
diogonac 0:99c31d1bf4d3 642 n_pulsosY = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Y; //movimentação do eixo Y para posição de pega
diogonac 0:99c31d1bf4d3 643 n_pulsosZ = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Z; //movimentação do eixo Z para posição de pega
diogonac 0:99c31d1bf4d3 644
diogonac 0:99c31d1bf4d3 645 sinal = 1; // saída nível lógico "alto"
diogonac 0:99c31d1bf4d3 646 lcd.cls();
diogonac 0:99c31d1bf4d3 647 lcd.locate(0,0);
diogonac 0:99c31d1bf4d3 648 lcd.printf("Depositando");
diogonac 0:99c31d1bf4d3 649 pc.printf("Depositando\r\n");
diogonac 0:99c31d1bf4d3 650 wait(tempo);
diogonac 0:99c31d1bf4d3 651 }