Diff: JOG_EIXO_Z.cpp

Revision:

4:c3007ca446ec

Parent:

3:ce5c55c100b1

Child:

5:bfedd53bfd09

--- a/JOG_EIXO_Z.cpp	Thu May 13 12:19:41 2021 +0000
+++ b/JOG_EIXO_Z.cpp	Thu May 13 12:24:42 2021 +0000
@@ -4,22 +4,24 @@
 
 AnalogIn eixo_X(A0);
 AnalogIn eixo_Y(A1);
-InterruptIn botao_in(PA_8);
+AnalogIn eixo_Z(A2); //Precisa ser verificado
+InterruptIn posicao_salva(PA_8);
+InterruptIn botao_emergencia(PB_10);
 
 BusOut motor_x(PA_9, PC_7, PB_6, PA_7);
 BusOut motor_y(PA_11, PB_12, PB_11, PB_2);
+BusOut motor_z(PB_1, PB_15, PB_14, PB_13);
 
 double frequencia; //Hz
 double velocidade = 4; //RPM
 double tempo; //Segundos
 int i;
-int X;
-int Y;
-int botao_estado;
-bool rotacao;
+int X, Y, Z;
+int pegaX, pegaY, pegaZ;
+int posicao_salva_estado, botao_emergencia_estado;
 
-void posicao_de_pega(void);
-
+void rotina_posicao_salva(void);
+void rotina_emergencia(void);
 
 
 
@@ -27,9 +29,10 @@
 {
     motor_x = 0x00;
     motor_y = 0x00;
+    motor_z = 0x00;
     pc.baud(115200);
-    rotacao = 0;
-    botao_in.rise(&posicao_de_pega);
+    posicao_salva.rise(&rotina_posicao_salva);
+    botao_emergencia.rise(&rotina_emergencia);
     frequencia = (2048*velocidade)/60;
     tempo = (1/frequencia);
 
@@ -37,9 +40,11 @@
 
         X = eixo_X.read_u16();
         Y = eixo_Y.read_u16();
-        botao_estado = botao_in.read();
+        Z = eixo_Z.read_u16();
+        posicao_salva_estado = posicao_salva.read();
+        botao_emergencia_estado = botao_emergencia.read();
 
-        pc.printf("X=%4d, Y=%4d, botao_estado=%d, rotacao_estado=%d \r\n", X, Y, botao_estado, rotacao);
+        pc.printf("X=%4d, Y=%4d, Z=%4d, PegaX=%4d, PegaY=%4d, PegaZ=%4d, Posicao_salva_estado=%d, Botao_emergencia_estado=%d \r\n", X, Y, Z, pegaX, pegaY, pegaZ, posicao_salva_estado, botao_emergencia_estado);
 
         if(31000 <= X &  X <= 35000) {
 
@@ -66,25 +71,68 @@
         }
 
         else {
-            if (rotacao == 0) {
+            if (Y > 31000) {
                 for(i = 0; i < 4; i++) {
                     motor_y = 1 << i;
                     wait(tempo);
                 }
             }
 
-            if(rotacao == 1) {
+            if(Y < 35000) {
                 for(i = 3; i > -1; i--) {
                     motor_y = 1 << i;
                     wait(tempo);
                 }
             }
         }
+
+        if(31000 <= X &  X <= 35000) {
+
+            motor_z = 0x00;
+        } else {
+            if (X > 31000) {
+                for(i = 0; i < 4; i++) {
+                    motor_z = 1 << i;
+                    wait(tempo);
+                }
+            }
+
+            if(X<35000) {
+                for(i = 3; i > -1; i--) {
+                    motor_z = 1 << i;
+                    wait(tempo);
+                }
+            }
+        }
+
     }
 }
 
 
-void posicao_de_pega()
+void rotina_posicao_salva()
 {
-    if(botao_estado == 0) rotacao = !rotacao;
+    if(posicao_salva_estado == 0) {
+
+        pegaX = X;
+        pegaY = Y;
+        pegaZ = Z;
+
+    } else {
+
+        pegaX = 0;
+        pegaY = 0;
+        pegaZ = 0;
+
+    }
+}
+
+void rotina_emergencia()
+{
+    if(botao_emergencia_estado == 0) {
+
+        motor_x = 0x00;
+        motor_y = 0x00;
+        motor_z = 0x00;
+
+    }
 }
\ No newline at end of file