Projeto Mecatrônico
codigo fonte
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "TextLCD.h" 00003 00004 TextLCD lcd(D8, D9, D4, D5, D6, D7); //Comunicação com LCD 00005 BusOut motor_x(PC_8, PC_6, PC_5, PA_12); //Pinos para acionamento do motor do eixo X 00006 BusOut motor_y(PA_11, PB_12, PB_11, PB_2); //Pinos para acionamento do motor do eixo Y 00007 BusOut motor_z(PB_1, PB_15, PB_14, PB_13); //Pinos para acionamento do motor do eixo Z 00008 DigitalOut sinal(PC_10); // Pino para o acionamento da pipeta 00009 //DigitalIn botao_seleciona(PC_3); //botao de SELECT geral 00010 00011 Serial pc (D1, D0); //Comunicação com USB 00012 AnalogIn joystick(PC_2); //Joystick potenciometro 00013 //AnalogIn botao_SELECT(A0); //Botão SELECT da IHM 00014 //Teste 00015 InterruptIn botao_seleciona(PA_3); //Botão de salva posição Joystick 00016 00017 InterruptIn botao_emergencia_in(PC_4); //Botão de emergência para detectar acionamento 00018 InterruptIn botao_fim_curso_rise(PC_12); //Chave de fim de curso 00019 00020 00021 Timer debounce1; //Correção para botão de posição de salva 00022 Timer debounce2; //Correção para botão de emergência 00023 Timer debounce3; //Correção para chave de fim de curso 00024 00025 int pulsos = 0; //Contagem dos pulsos para o motor 00026 00027 int i; //Variável para contar o incremento no motor 00028 int cont_fim_curso = 0; //Variável para contar o incremento do fim de curso 00029 int J_XYZ; //Variável para ler o potenciometro do Joystick 00030 int passo_fuso = 5; //Declara o passo do fuso 00031 int pegaZ; //Variável que receberá o valor da posição de salva 00032 int SELECT; //Variável para leitura do botão SELECT da IHM 00033 int contador_SELECT = 0; //Define o contator do SELECT da IHM 00034 int contador_emergencia = 0; //Define o contator do botão de emergência 00035 00036 int contador_rise = 0; 00037 int seguranca_fdc = 0; 00038 00039 // ------------------Acionamento da Pipeta------------------ 00040 // Valores de Teste 00041 int total_pega = 6; // quantidade de mL na posição de pega disponível 00042 int quant_desejada = 3; // quantidade de mL na posição de solta 1 00043 //int npos_solta = [1,2,3,4]; 00044 int pos_solta_total = 4; 00045 float posZ_atual = 3; 00046 float posX_atual = 4; 00047 float posY_atual = 5; 00048 float posX0_desejada = 3; 00049 float posY0_desejada = 5; 00050 float posZ0_desejada = 4; 00051 float posX_desejada = 2; 00052 float posY_desejada = 6; 00053 float posZ_desejada = 2; 00054 00055 float deslocamento_X = 0; //Define o deslocamento linear total 00056 float deslocamento_Y = 0; //Define o deslocamento linear total 00057 float deslocamento_Z = 0; //Define o deslocamento linear total 00058 00059 float deslocamento_max_X = 0; //Define o deslocamento linear total 00060 float deslocamento_max_Y = 0; //Define o deslocamento linear total 00061 float deslocamento_max_Z = 0; //Define o deslocamento linear total 00062 00063 float velocidade = 0.003; //Armazena a velocidade dos motores 00064 00065 int atual_pega = total_pega; // "total_pega" é a quantidade inserida pelo usuário na posição de pega 00066 //int quant_desejada; // "quant desejada" é a quantidade inserida pelo usuário na posição de solta 00067 int quant_depositada = 0; 00068 int n_pulsosX; 00069 int n_pulsosY; 00070 int n_pulsosZ; 00071 int posZ_origem = 0; 00072 int deslocamentoZ_partida = posZ_origem - deslocamento_Z; // deslocamento do eixo Z até a posição de partida (origem) 00073 int deslocamento_X0 = posX0_desejada - posX_atual; // deslocamento do eixo X até a posição de pega desejada 00074 int deslocamento_Y0 = posY0_desejada - posY_atual; // deslocamento do eixo Y até a posição de pega desejada 00075 int deslocamento_Z0 = posZ0_desejada - posZ_atual; // deslocamento do eixo Z até a posição de pega desejada 00076 int deslocamento_solta_X = posX_desejada - posX_atual; // deslocamento do eixo X até a posição de solta desejada 00077 int deslocamento_solta_Y = posY_desejada - posY_atual; // deslocamento do eixo Y até a posição de solta desejada 00078 int deslocamento_solta_Z = posZ_desejada - posZ_atual; // deslocamento do eixo Z até a posição de solta desejada 00079 00080 // ------------------Acionamento da Pipeta------------------ 00081 00082 float tempo_horario, tempo_anti_horario; //Define os tempos de acionamento do motor 00083 float passo_motor = 5.625/32; //Declara o passo angular do motor 00084 float tempo = 1.5; // Define o tempo que a pipeta fica acionada 00085 00086 int pulsos_Z = 0; //Contagem dos pulsos para o motor_Z 00087 int pulsos_X = 0; //Contagem dos pulsos para o motor_X 00088 int pulsos_Y = 0; //Contagem dos pulsos para o motor_Y 00089 00090 int contador_botao_seleciona = 0; //Boleana para a lógica do botão SELECT do joystick 00091 00092 bool valor_SELECT = false; //Boleana para lógica do botão SELECT da IHM 00093 bool estado_referenciamento = false; //Boleana para identificar a necessidade do referênciamento 00094 bool estado_botao_emergencia_in = false; //Boleana para identificar a necessidade do referênciamentosatisfazer a lógica da rotina_emergencia_in 00095 bool motor_referenciamento = false; //Boleana para permitir o movimento automático do motot no referenciamento 00096 00097 // Rotinas do programa 00098 void rotina_posicao_salva (void); 00099 void rotina_emergencia_in (void); 00100 void rotina_velocidade_eixo_Z (void); 00101 void rotina_deslocamento_eixo_Z (void); 00102 void rotina_deslocamento_eixo_menos_Z (void); 00103 void rotina_JOG (void); 00104 void rotina_botoes_IHM (void); 00105 void Rotina_Pega (void); 00106 void Rotina_Solta (void); 00107 void teste2dora (void); 00108 void reposiciona_pipeta_origem (void); 00109 00110 void rotina_fdc_rise (void); 00111 void referenciamento (void); 00112 00113 void rotina_estado_botao_seleciona (void); 00114 00115 int main() 00116 { 00117 pc.baud(115200); //Define a velocidade da porta USB 00118 motor_x = 0x00; //Estabelece a condição inicial do motor do eixo X 00119 motor_y = 0x00; //Estabelece a condição inicial do motor do eixo Y 00120 motor_z = 0x00; //Estabelece a condição inicial do motor do eixo Z 00121 00122 //Declara os pinos de interrupção com suas funções 00123 botao_seleciona.rise(&rotina_estado_botao_seleciona); 00124 00125 botao_emergencia_in.rise(&rotina_emergencia_in); 00126 botao_fim_curso_rise.rise(&rotina_fdc_rise); 00127 00128 00129 //Inicializa os timers 00130 debounce1.start(); 00131 debounce2.start(); 00132 debounce3.start(); 00133 00134 //Inicia o LCD 00135 wait_ms(500); 00136 lcd.locate(2,0); 00137 lcd.printf("SELECT para"); 00138 lcd.locate(0,1); 00139 lcd.printf("referenciamento"); 00140 00141 while(1) { 00142 //Leitura de variáveis 00143 J_XYZ = joystick.read_u16(); 00144 //SELECT = botao_SELECT.read_u16(); 00145 //pc.printf("estado_referenciamento=%4d, estado_botao_emergencia_in=%d valor_SELECT=%d\r\n", estado_referenciamento, estado_botao_emergencia_in, valor_SELECT); 00146 //pc.printf("deslocamento_Z=%4f, pulsos=%d\r\n", deslocamento_Z, pulsos); 00147 //pc.printf("J_XYZ= %d\r\n", J_XYZ); 00148 if(estado_referenciamento == true & estado_botao_emergencia_in == false & contador_botao_seleciona == 2) { 00149 //Condição para poder disponibilizar a função JOG 00150 //Chama a rotina do JOG 00151 rotina_JOG(); 00152 //LCD 00153 00154 lcd.locate(2,0); 00155 lcd.printf("PosicaoZ:"); 00156 lcd.locate(0,1); 00157 lcd.printf("%4fmm",deslocamento_Z); 00158 //pc.printf("J_XYZ= %d\r\n", J_XYZ); 00159 } 00160 00161 else { 00162 //Chama a rotina dos botões da IHM 00163 //rotina_botoes_IHM(); 00164 00165 if(valor_SELECT == false) { //se estiver 0, mudar para 1 -- no PC da dora fica 0 pela LCD dela 00166 00167 lcd.cls(); 00168 lcd.locate(2,0); 00169 lcd.printf("Iniciando o"); 00170 lcd.locate(0,1); 00171 lcd.printf("referenciamento"); 00172 00173 pc.printf("Referenciamento \r\n"); 00174 00175 contador_SELECT = 1; 00176 estado_referenciamento = false; 00177 //Permite o movimento automático do motor até atingir o fim de curso 00178 referenciamento(); 00179 00180 pc.printf("Referenciamento feito\r\n"); 00181 lcd.cls(); 00182 lcd.locate(0,0); 00183 lcd.printf("Pressione SELECT"); 00184 lcd.locate(0,1); 00185 lcd.printf("para continuar"); 00186 00187 } 00188 } 00189 00190 //======================================================================================== 00191 00192 //while (npos_solta < pos_solta_total) { 00193 //checa se existem mais posições de solta 00194 00195 if(botao_seleciona == 1) { 00196 //pressiona para entrar nesse if 00197 // o usuario nao quer alterar nenhum parametro 00198 pc.printf("pressionado\r\n"); 00199 00200 quant_depositada = 0; 00201 00202 while (quant_depositada < quant_desejada) { 00203 // "quant_desejada" definido pelo usuário na IHM 00204 00205 if (atual_pega == 0) { 00206 lcd.cls(); 00207 lcd.locate(0,0); 00208 lcd.printf("Coleta vazia"); 00209 lcd.locate(0,1); 00210 lcd.printf("Insira mais subst"); 00211 wait(4); 00212 pc.printf("Coleta vazia \r\n"); 00213 exit(0); // obriga o usuário a recomeçar 00214 //levanta_pipeta(); //void para levantar a pipeta até o zero 00215 //ALTERAR: PIPETA VAI PARA A ORIGEM NO EIXO Z 00216 } 00217 //referenciamento(); 00218 //break; 00219 else { 00220 Rotina_Pega(); 00221 wait(tempo); 00222 Rotina_Solta(); 00223 wait(tempo); 00224 } 00225 00226 atual_pega = atual_pega - 1; 00227 quant_depositada = quant_depositada + 1; 00228 00229 pc.printf("atual_pega: %d \r\n",atual_pega); 00230 pc.printf("quant_depositada: %d \r\n",quant_depositada); 00231 } 00232 } 00233 pc.printf("fora do if\r\n"); 00234 //referenciamento(); 00235 } //while(1) 00236 } //int main() 00237 00238 void rotina_estado_botao_seleciona() 00239 { 00240 if(debounce1.read_ms() >= 10 & estado_referenciamento == true & estado_botao_emergencia_in == false) { 00241 contador_botao_seleciona += 1; 00242 pc.printf("contador_botao_seleciona= %d\r\n", contador_botao_seleciona); 00243 lcd.cls(); 00244 } 00245 debounce1.reset(); 00246 } 00247 00248 void rotina_emergencia_in() 00249 { 00250 if(debounce2.read_ms() >= 10) { 00251 00252 motor_x = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo X 00253 motor_y = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Y 00254 motor_z = 0x00; //Não permite que pulsos sejam enviados para o motor do eixo Z 00255 00256 estado_referenciamento = false; //Solicita um novo referenciamento 00257 estado_botao_emergencia_in =! estado_botao_emergencia_in; 00258 contador_emergencia += 1; 00259 00260 lcd.cls(); 00261 wait_ms(500); 00262 lcd.locate(0,0); 00263 lcd.printf("Emergencia ON"); 00264 lcd.locate(0,1); 00265 lcd.printf("Pressione RST"); 00266 00267 exit(0); //Encerra o programa e obriga o usuário a resetar o sistema 00268 00269 debounce2.reset(); 00270 } 00271 } 00272 00273 void rotina_velocidade_eixo_Z() 00274 { 00275 //Converte a leitura do Joystick para tempo de acionamento do motor 00276 J_XYZ = joystick.read_u16(); 00277 tempo_horario = -1*J_XYZ*0.0000028 + 0.1875; 00278 tempo_anti_horario = J_XYZ*0.000002785 + 0.0025; 00279 } 00280 00281 void rotina_velocidade_referenciamento_eixo_Z() 00282 { 00283 //Converte a leitura do Joystick para tempo de acionamento do motor 00284 J_XYZ = joystick.read_u16(); 00285 tempo_horario = (-1*J_XYZ*0.0000028 + 0.1875)*2; 00286 tempo_anti_horario = (J_XYZ*0.000002785 + 0.0025)*2; 00287 } 00288 00289 void rotina_deslocamento_eixo() 00290 { 00291 //Calcula os deslocamentos no eixo Z 00292 00293 deslocamento_X = (passo_fuso*pulsos_X*passo_motor)/360; 00294 deslocamento_Y = (passo_fuso*pulsos_Y*passo_motor)/360; 00295 deslocamento_Z = (passo_fuso*pulsos_Z*passo_motor)/360; 00296 00297 // recebe_arduino.printf("X%3.2f\n", deslocamento_X); 00298 // recebe_arduino.printf("Y%3.2f\n", deslocamento_Y); 00299 // recebe_arduino.printf("Z%3.2f\n", deslocamento_Z); 00300 00301 pc.printf("X%3.2f, Y%3.2f, Z%3.2f, J_XYZ=%d\n", deslocamento_X, deslocamento_Y, deslocamento_Z, J_XYZ); 00302 00303 } 00304 00305 //void rotina_botoes_IHM() 00306 //{ 00307 // if(SELECT > 60000 & SELECT < 65000) valor_SELECT =! valor_SELECT; 00308 // else valor_SELECT = 0; 00309 //} 00310 00311 void referenciamento() 00312 { 00313 00314 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_SELECT == 1) {//W1 00315 00316 for(i = 0; i < 4; i++) { 00317 motor_x = 1 << i; 00318 motor_y = 1 << i; 00319 motor_z = 1 << i; 00320 wait(0.003); 00321 } 00322 00323 if(contador_rise == 1) {//if1 00324 00325 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 1) {//W2 00326 00327 for(i = 3; i > -1; i--) { 00328 motor_x = 1 << i; 00329 motor_y = 1 << i; 00330 motor_z = 1 << i; 00331 wait(0.003); 00332 00333 } 00334 00335 if(contador_rise == 2) {//if2 00336 00337 while(estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 2) {//W3 00338 00339 for(i = 0; i < 4; i++) { 00340 motor_x = 1 << i; 00341 motor_y = 1 << i; 00342 motor_z = 1 << i; 00343 wait(0.003); 00344 } 00345 00346 if(contador_rise == 3) {//if3 00347 00348 while(seguranca_fdc != 8 & estado_referenciamento == false & estado_botao_emergencia_in == false & contador_rise == 3) {//W4 00349 for(i = 3; i > -1; i--) { 00350 motor_x = 1 << i; 00351 motor_y = 1 << i; 00352 motor_z = 1 << i; 00353 wait(0.003); 00354 00355 } 00356 seguranca_fdc += 1; 00357 00358 }//Fecha W4 00359 00360 estado_referenciamento = true; 00361 contador_SELECT = 0; 00362 contador_rise = 0; 00363 seguranca_fdc = 0; 00364 00365 valor_SELECT = true; 00366 contador_botao_seleciona = 0; 00367 deslocamento_X = 0; //Define o deslocamento linear total 00368 deslocamento_Y = 0; //Define o deslocamento linear total 00369 deslocamento_Z = 0; //Define o deslocamento linear total 00370 pulsos_X = 0; 00371 pulsos_Y = 0; 00372 pulsos_Z = 0; 00373 deslocamento_max_X = 5; //Define o deslocamento linear total 00374 deslocamento_max_Y = 5; //Define o deslocamento linear total 00375 deslocamento_max_Z = 5; //Define o deslocamento linear total 00376 // velocidade = 0.1; 00377 00378 // recebe_arduino.printf("D\n"); //Done referenciamento 00379 00380 }//Fecha if3 00381 }//Fecha W3 00382 00383 }//Fecha if2 00384 }//Fecha W2 00385 }//Fecha if1 00386 }//Fecha W1 00387 motor_x = 0x00; 00388 motor_y = 0x00; 00389 motor_z = 0x00; 00390 }//Fecha funcao 00391 00392 00393 void rotina_fdc_rise() 00394 { 00395 if(debounce3.read_ms() >= 50 & estado_botao_emergencia_in == false & estado_referenciamento == false & contador_SELECT == 1) { 00396 contador_rise += 1; 00397 pc.printf("contador_rise=%d\n", contador_rise); 00398 debounce3.reset(); 00399 } 00400 } 00401 00402 00403 void rotina_JOG() 00404 { 00405 J_XYZ = joystick.read_u16(); 00406 00407 if(32400 <= J_XYZ & J_XYZ <= 34100) { 00408 motor_x = 0x00; 00409 motor_y = 0x00; 00410 motor_z = 0x00; 00411 } 00412 00413 else { 00414 //============== Movimenta Eixo Y ====================================== 00415 00416 if (J_XYZ > 34100 & deslocamento_Y > 0 & pulsos_Y > 0) { 00417 // tempo_motor.start(); 00418 // while(deslocamento_Y > 0) { 00419 J_XYZ = joystick.read_u16(); 00420 for(i = 0; i < 4; i++) { 00421 motor_y = 1 << i; 00422 // rotina_velocidade_eixo(); 00423 rotina_deslocamento_eixo(); 00424 pulsos_Y-=1; 00425 wait(velocidade); 00426 00427 } 00428 // } 00429 // tempo_motor.reset(); 00430 // tempo_motor.stop(); 00431 } 00432 00433 if(J_XYZ < 32400 & deslocamento_max_Y > deslocamento_Y & pulsos_Y < 2048) { 00434 // tempo_motor.start(); 00435 // while(deslocamento_max_Y > deslocamento_Y) { 00436 J_XYZ = joystick.read_u16(); 00437 for(i = 3; i > -1; i--) { 00438 motor_y = 1 << i; 00439 // rotina_velocidade_eixo(); 00440 rotina_deslocamento_eixo(); 00441 pulsos_Y+=1; 00442 wait(velocidade); 00443 00444 } 00445 // } 00446 // tempo_motor.reset(); 00447 // tempo_motor.stop(); 00448 } 00449 00450 //============== Movimenta Eixo X ====================================== 00451 00452 if (J_XYZ > 34100 & deslocamento_X > 0 & pulsos_X > 0) { 00453 // tempo_motor.start(); 00454 // while(deslocamento_X > 0) { 00455 for(i = 0; i < 4; i++) { 00456 motor_x = 1 << i; 00457 // rotina_velocidade_eixo(); 00458 rotina_deslocamento_eixo(); 00459 pulsos_X-=1; 00460 wait(velocidade); 00461 00462 } 00463 // } 00464 // tempo_motor.reset(); 00465 // tempo_motor.stop(); 00466 } 00467 00468 if(J_XYZ < 32400 & deslocamento_max_X > deslocamento_X & pulsos_X < 2048) { 00469 // tempo_motor.start(); 00470 // while(deslocamento_max_X > deslocamento_X) { 00471 for(i = 3; i > -1; i--) { 00472 motor_x = 1 << i; 00473 // rotina_velocidade_eixo(); 00474 rotina_deslocamento_eixo(); 00475 pulsos_X+=1; 00476 wait(velocidade); 00477 00478 } 00479 // } 00480 // tempo_motor.reset(); 00481 // tempo_motor.stop(); 00482 } 00483 00484 00485 //============== Movimenta Eixo Z ====================================== 00486 00487 if (J_XYZ > 34100 & deslocamento_Z > 0 & pulsos_Z > 0) { 00488 // tempo_motor.start(); 00489 // while(deslocamento_Z > 0) { 00490 for(i = 0; i < 4; i++) { 00491 motor_z = 1 << i; 00492 // rotina_velocidade_eixo(); 00493 rotina_deslocamento_eixo(); 00494 pulsos_Z-=1; 00495 wait(velocidade); 00496 00497 } 00498 // } 00499 // tempo_motor.reset(); 00500 // tempo_motor.stop(); 00501 } 00502 00503 if(J_XYZ < 32400 & deslocamento_max_Z > deslocamento_Z & pulsos_Z < 2048) { 00504 // tempo_motor.start(); 00505 // while(deslocamento_max_Z > deslocamento_Z) { 00506 for(i = 3; i > -1; i--) { 00507 motor_z = 1 << i; 00508 // rotina_velocidade_eixo(); 00509 rotina_deslocamento_eixo(); 00510 pulsos_Z+=1; 00511 wait(velocidade); 00512 00513 } 00514 // } 00515 // tempo_motor.reset(); 00516 // tempo_motor.stop(); 00517 } 00518 00519 00520 } 00521 } 00522 00523 00524 void Rotina_Pega() 00525 { 00526 sinal = 0; // saída nível lógico "baixo" 00527 00528 pc.printf("posZ_atual: %f, posZ_origem: %d \r\n",posZ_atual, posZ_origem); 00529 //if(posZ_atual > posZ_origem) { 00530 while (deslocamento_Z != posZ_origem) { 00531 //While para verificar que o motor funcione até a a posição desejada ser alcançada 00532 for(i = 0; i < 4; i++) { 00533 motor_z = 1 << i; 00534 pulsos_Z -= 1; 00535 wait(velocidade); 00536 rotina_deslocamento_eixo(); 00537 pc.printf("Pos atualZ H: %f \r\n", deslocamento_Z); 00538 } 00539 } 00540 //} 00541 // else { //não faz nada 00542 // while (deslocamento_Z < posZ0_desejada) { 00543 // //While para verificar que o motor funcione até a a posição desejada ser alcançada 00544 // for(i = 0; i < 4; i++) { 00545 // motor_z = 1 << i; 00546 // pulsos_Z -= 1; 00547 // wait(velocidade); 00548 // rotina_deslocamento_eixo(); 00549 // pc.printf("Pos atualZ H: %d\r\n", deslocamento_Z); 00550 // } 00551 // } 00552 // } 00553 00554 00555 00556 posX_atual = deslocamento_X; 00557 pc.printf("posX_atual: %f, posX0_desejada:%f \r\n",posX_atual, posX0_desejada); 00558 if(posX_atual < posX0_desejada) { 00559 while (deslocamento_X > posX0_desejada) { 00560 //While para verificar que o motor funcione até a a posição desejada ser alcançada 00561 for(i = 3; i > -1; i--) { 00562 motor_x = 1 << i; 00563 pulsos_X += 1; 00564 wait(velocidade); 00565 rotina_deslocamento_eixo(); 00566 pc.printf("Pos atualX AH: %f\r\n", deslocamento_X); 00567 } 00568 pc.printf("Entrou no while posX_atual"); 00569 } 00570 } 00571 else { 00572 while (deslocamento_X < posX0_desejada) { 00573 //While para verificar que o motor funcione até a a posição desejada ser alcançada 00574 for(i = 0; i < 4; i++) { 00575 motor_x = 1 << i; 00576 pulsos_X -= 1; 00577 wait(velocidade); 00578 rotina_deslocamento_eixo(); 00579 pc.printf("Pos atualX H: %f\r\n", deslocamento_X); 00580 } 00581 } 00582 } 00583 posY_atual = deslocamento_Y; 00584 pc.printf("posY_atual: %f, posY0_desejada: %f \r\n",posY_atual, posY0_desejada); 00585 if(posY_atual < posY0_desejada) { 00586 while (posY_atual > posY0_desejada) { 00587 for(i = 3; i > -1; i--) { 00588 motor_y = 1 << i; 00589 pulsos_Y += 1; 00590 wait(velocidade); 00591 rotina_deslocamento_eixo(); 00592 } 00593 } 00594 } 00595 00596 else { 00597 while (posY_atual < posY0_desejada) { 00598 for(i = 0; i < 4; i++) { 00599 motor_y = 1 << i; 00600 pulsos_Y -= 1; 00601 wait(velocidade); 00602 rotina_deslocamento_eixo(); 00603 } 00604 } 00605 } 00606 00607 posZ_atual = deslocamento_Z; 00608 pc.printf("posZ_atual: %f, posZ0_desejada: %f \r\n",posZ_atual, posZ0_desejada); 00609 if(posZ_atual < posZ0_desejada) { 00610 while (deslocamento_Z > posZ0_desejada) { 00611 //While para verificar que o motor funcione até a a posição desejada ser alcançada 00612 for(i = 3; i > -1; i--) { 00613 motor_z = 1 << i; 00614 pulsos_Z += 1; 00615 wait(velocidade); 00616 rotina_deslocamento_eixo(); 00617 pc.printf("Pos atualZ AH: %f\r\n", deslocamento_X); 00618 } 00619 } 00620 } else { 00621 while (deslocamento_Z < posZ0_desejada) { 00622 //While para verificar que o motor funcione até a a posição desejada ser alcançada 00623 for(i = 0; i < 4; i++) { 00624 motor_z = 1 << i; 00625 pulsos_Z -= 1; 00626 wait(velocidade); 00627 rotina_deslocamento_eixo(); 00628 pc.printf("Pos atualX H: %f\r\n", deslocamento_X); 00629 } 00630 } 00631 } 00632 00633 00634 sinal = 1; // saída nível lógico "alto" 00635 lcd.cls(); 00636 lcd.locate(0,0); 00637 lcd.printf("Coletando"); 00638 pc.printf("Coletando\r\n"); 00639 wait(tempo); 00640 00641 } 00642 00643 void Rotina_Solta() 00644 { 00645 sinal = 0; // saída nível lógico "baixo" 00646 00647 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 00648 00649 n_pulsosX = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_X; //movimentação do eixo X para posição de pega 00650 n_pulsosY = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Y; //movimentação do eixo Y para posição de pega 00651 n_pulsosZ = (1/(passo_motor*passo_fuso))*360*deslocamento_solta_Z; //movimentação do eixo Z para posição de pega 00652 00653 sinal = 1; // saída nível lógico "alto" 00654 lcd.cls(); 00655 lcd.locate(0,0); 00656 lcd.printf("Depositando"); 00657 pc.printf("Depositando\r\n"); 00658 wait(tempo); 00659 }
Generated on Sat Jul 23 2022 07:47:15 by
1.7.2