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 }