Diff: main.cpp

Revision:

5:452d20780aac

Parent:

4:4b994227bca2

--- a/main.cpp	Fri Jun 04 21:48:17 2021 +0000
+++ b/main.cpp	Fri Jun 04 22:51:42 2021 +0000
@@ -42,15 +42,15 @@
 int quant_desejada = 3; // quantidade de mL na posição de solta 1
 //int npos_solta = [1,2,3,4];
 int pos_solta_total = 4;
-float posZ_atual = 100;
-float posX_atual = 10;
-float posY_atual = 20;
-float posX0_desejada = 0;
+float posZ_atual = 3;
+float posX_atual = 4;
+float posY_atual = 5;
+float posX0_desejada = 3;
 float posY0_desejada = 5;
-float posZ0_desejada = 10;
-float posX_desejada = 10;
-float posY_desejada = 20;
-float posZ_desejada = 90;
+float posZ0_desejada = 4;
+float posX_desejada = 2;
+float posY_desejada = 6;
+float posZ_desejada = 2;
 
 float deslocamento_X = 0; //Define o deslocamento linear total
 float deslocamento_Y = 0; //Define o deslocamento linear total
@@ -207,7 +207,7 @@
                     lcd.locate(0,0);
                     lcd.printf("Coleta vazia");
                     lcd.locate(0,1);
-                    lcd.printf("Insira mais substrato");
+                    lcd.printf("Insira mais subst");
                     wait(4);
                     pc.printf("Coleta vazia \r\n");
                     exit(0); // obriga o usuário a recomeçar
@@ -218,7 +218,9 @@
                     //break;
                 else {
                     Rotina_Pega();
+                    wait(tempo);
                     Rotina_Solta();
+                    wait(tempo);
                 }
 
                 atual_pega = atual_pega - 1;
@@ -521,36 +523,38 @@
 
 void Rotina_Pega()
 {
-    // ------------ LEIA ----------
-    //Ele ta pulando o while do Z e X e indo direto pro Z -- ele não tá nem entrando nesse while
     sinal = 0; // saída nível lógico "baixo"
     
-    posZ_atual = deslocamento_Z; // posZ_atual = a posição de deslocamento neste momento
-    if(posZ_atual > posZ_origem) {
-        while (deslocamento_Z > posZ_origem) {
+    pc.printf("posZ_atual: %f, posZ_origem: %d \r\n",posZ_atual, posZ_origem);
+    //if(posZ_atual > posZ_origem) {
+        while (deslocamento_Z != posZ_origem) {
             //While para verificar que o motor funcione até a a posição desejada ser alcançada
-            for(i = 3; i > -1; i--) {
+            for(i = 0; i < 4; i++) {
                 motor_z = 1 << i;
                 pulsos_Z -= 1;
                 wait(velocidade);
                 rotina_deslocamento_eixo();
-                pc.printf("Pos atualZ AH: %f\r\n", deslocamento_Z);
+                pc.printf("Pos atualZ H: %f \r\n", deslocamento_Z);
             }
         }
-    }
-    else { //não faz nada
+    //}
+//    else { //não faz nada
 //        while (deslocamento_Z < posZ0_desejada) {
 //            //While para verificar que o motor funcione até a a posição desejada ser alcançada
 //            for(i = 0; i < 4; i++) {
 //                motor_z = 1 << i;
-//                pulsos_Z -= 1
+//                pulsos_Z -= 1;
 //                wait(velocidade);
 //                rotina_deslocamento_eixo();
 //                pc.printf("Pos atualZ H: %d\r\n", deslocamento_Z);
 //            }
 //        }
-    }
+//    }
+    
+    
+    
     posX_atual = deslocamento_X;
+    pc.printf("posX_atual: %f, posX0_desejada:%f \r\n",posX_atual, posX0_desejada);
     if(posX_atual < posX0_desejada) {
         while (deslocamento_X > posX0_desejada) {
             //While para verificar que o motor funcione até a a posição desejada ser alcançada
@@ -561,8 +565,10 @@
                 rotina_deslocamento_eixo();
                 pc.printf("Pos atualX AH: %f\r\n", deslocamento_X);
             }
+            pc.printf("Entrou no while posX_atual");
         }
-    } else {
+    } 
+    else {
         while (deslocamento_X < posX0_desejada) {
             //While para verificar que o motor funcione até a a posição desejada ser alcançada
             for(i = 0; i < 4; i++) {
@@ -575,6 +581,7 @@
         }
     }
     posY_atual = deslocamento_Y;
+    pc.printf("posY_atual: %f, posY0_desejada: %f \r\n",posY_atual, posY0_desejada);
     if(posY_atual < posY0_desejada) {
         while (posY_atual > posY0_desejada) {
             for(i = 3; i > -1; i--) {
@@ -598,6 +605,7 @@
     }
 
     posZ_atual = deslocamento_Z;
+    pc.printf("posZ_atual: %f, posZ0_desejada: %f \r\n",posZ_atual, posZ0_desejada);
     if(posZ_atual < posZ0_desejada) {
         while (deslocamento_Z > posZ0_desejada) {
             //While para verificar que o motor funcione até a a posição desejada ser alcançada