Diff: movimentacao.cpp

Revision:

3:a9c1946f0518

Parent:

2:281470e6654a

Child:

4:7594859b6e60

--- a/movimentacao.cpp	Mon Jun 22 22:13:27 2020 +0000
+++ b/movimentacao.cpp	Tue Jun 23 05:31:21 2020 +0000
@@ -1,19 +1,332 @@
 # include "mbed.h"
 
+AnalogIn XAxis(A0);
+AnalogIn YAxis(A1);
+InterruptIn butao(PC_13);
+DigitalIn ZAxis_UP(D13);
+DigitalIn ZAxis_DOWN(D12);
+DigitalOut Fase_M1_A(PB_12);
+DigitalOut Fase_M1_B(PB_15);
+DigitalOut Fase_M1_C(PB_14);
+DigitalOut Fase_M1_D(PB_13);
+DigitalOut Fase_M2_A(PC_12);
+DigitalOut Fase_M2_B(PA_13);
+DigitalOut Fase_M2_C(PA_14);
+DigitalOut Fase_M2_D(PA_15);
+DigitalOut Fase_M3_A(PA_12);
+DigitalOut Fase_M3_B(PC_5);
+DigitalOut Fase_M3_C(PC_6);
+DigitalOut Fase_M3_D(PC_8);
+DigitalOut Emergencia(D14);
+DigitalIn SensorParadaX_1(PA_8);
+DigitalIn SensorParadaY_1(PA_9);
+DigitalIn SensorParadaZ_1(PA_10);
+DigitalIn Fim(PA_11);
+DigitalOut Pipeta(D15);
+Serial pc(USBTX, USBRX);
+Timer debounce;
 
 int PosicaoRecipiente[3]; //Cria um vetor de 3 digítos para representar a posição do recipiente pré-existente na máquina (x, y, z)
 PosicaoRecipiente[0] = 40;
 PosicaoRecipiente[1] = 35;
 PosicaoRecipiente[2] = 10;
 
-void ativa_pipeta() { // Função para fazer a pipeta sugar/soltar o líquido - 1ml
+void Botao_Emergencia(void);
+    
+void Joystick(){
+    
+    int PosicaoUsuario[3];
+    int X,Y,estado;
+    estado = 1;
+    Emergencia = 0;
+    Pipeta = 1;
+    Fase_M1_A = 0;
+    Fase_M1_B = 0;
+    Fase_M1_C = 0;
+    Fase_M1_D = 0;
+    Fase_M2_A = 0;
+    Fase_M2_B = 0;
+    Fase_M2_C = 0;
+    Fase_M2_D = 0;
+    Fase_M3_A = 0;
+    Fase_M3_B = 0;
+    Fase_M3_C = 0;
+    Fase_M3_D = 0;
+    
+    debounce.start();
+    butao.rise(&Botao_Emergencia);
     
+    while(estado == 1) {
+        
+        X = XAxis.read() * 1000;
+        Y = YAxis.read() * 1000;
+        float tempo = 0.01;
+        
+
+        if (ZAxis_UP == 1) { // Enquanto o Botão não estiver pressionado o motor permanece desligado
+            Fase_M3_A = 0;
+            Fase_M3_B = 0;
+            Fase_M3_C = 0;
+            Fase_M3_D = 0;
+            }
+        
+        if (ZAxis_DOWN == 1) { // Enquanto o Botão não estiver pressionado o motor permanece desligado
+            Fase_M3_A = 0;
+            Fase_M3_B = 0;
+            Fase_M3_C = 0;
+            Fase_M3_D = 0;
+            }
+        
+        if (ZAxis_UP == 0 && ZAxis_DOWN == 1) { // Quando o botão UP estiver pressionado e o botão DOWN não - Suba a pipeta - inverte o sentido de rotação do motor
+            Fase_M3_D = 1;
+            wait(tempo);
+            Fase_M3_D = 0;
+            Fase_M3_C = 1;
+            wait(tempo);
+            Fase_M3_C = 0;
+            Fase_M3_B = 1;
+            wait(tempo);
+            Fase_M3_B = 0;
+            Fase_M3_A = 1;
+            wait(tempo);
+            Fase_M3_A = 0;
+            PosicaoUsuario[2] = PosicaoUsuario[2] - 5;
+            
+            }
+        
+        if (ZAxis_DOWN == 0 && ZAxis_UP == 1) { // Quando o botão DOWN estiver pressionado e o botão UP não  - Desce a pipeta - sentido normal de rotação do motor
+            Fase_M3_A = 1;
+            wait(tempo);
+            Fase_M3_A = 0;
+            Fase_M3_B = 1;
+            wait(tempo);
+            Fase_M3_B = 0;
+            Fase_M3_C = 1;
+            wait(tempo);
+            Fase_M3_C = 0;
+            Fase_M3_D = 1;
+            wait(tempo);
+            Fase_M3_D = 0;
+            PosicaoUsuario[2] = PosicaoUsuario[2] + 5;
+            
+            }
+        
+        if (100<X<900) { //delimitando range de atuação do joystick
+            Fase_M1_A = 0;
+            Fase_M1_B = 0;
+            Fase_M1_C = 0;
+            Fase_M1_D = 0;
+            Fase_M2_A = 0;
+            Fase_M2_B = 0;
+            Fase_M2_C = 0;
+            Fase_M2_D = 0;
+            Fase_M3_A = 0;
+            Fase_M3_B = 0;
+            Fase_M3_C = 0;
+            Fase_M3_D = 0;
+            }
+        
+        if (100<Y<900) { //delimitando range de atuação do joystick
+            Fase_M1_A = 0;
+            Fase_M1_B = 0;
+            Fase_M1_C = 0;
+            Fase_M1_D = 0;
+            Fase_M2_A = 0;
+            Fase_M2_B = 0;
+            Fase_M2_C = 0;
+            Fase_M2_D = 0;
+            Fase_M3_A = 0;
+            Fase_M3_B = 0;
+            Fase_M3_C = 0;
+            Fase_M3_D = 0;
+            }  
+
+        if (X<100) { //seta esquerda - Inverte o sentido de rotação
+            Fase_M1_D = 1;
+            wait(tempo);
+            Fase_M1_D = 0;
+            Fase_M1_C = 1;
+            wait(tempo);
+            Fase_M1_C = 0;
+            Fase_M1_B = 1;
+            wait(tempo);
+            Fase_M1_B = 0;
+            Fase_M1_A = 1;
+            wait(tempo);
+            Fase_M1_A = 0;
+            PosicaoUsuario[0] = PosicaoUsuario[0] - 5;
+            
+            }
+        
+        if (X>900) { // seta direita - sentido normal de rotação
+            Fase_M1_A = 1;
+            wait(tempo);
+            Fase_M1_A = 0;
+            Fase_M1_B = 1;
+            wait(tempo);
+            Fase_M1_B = 0;
+            Fase_M1_C = 1;
+            wait(tempo);
+            Fase_M1_C = 0;
+            Fase_M1_D = 1;
+            wait(tempo);
+            Fase_M1_D = 0;
+            PosicaoUsuario[0] = PosicaoUsuario[0] + 5;
+            
+            } 
+        
+        if (Y<100) { //seta pra baixo - Inverte o sentido de rotação
+            Fase_M2_D = 1;
+            wait(tempo);
+            Fase_M2_D = 0;
+            Fase_M2_C = 1;
+            wait(tempo);
+            Fase_M2_C = 0;
+            Fase_M2_B = 1;
+            wait(tempo);
+            Fase_M2_B = 0;
+            Fase_M2_A = 1;
+            wait(tempo);
+            Fase_M2_A = 0;
+            PosicaoUsuario[1] = PosicaoUsuario[1] - 5;
+            
+            }
+        
+        if (Y>900) { //seta pra cima - Sentido normal de rotação
+            Fase_M2_A = 1;
+            wait(tempo);
+            Fase_M2_A = 0;
+            Fase_M2_B = 1;
+            wait(tempo);
+            Fase_M2_B = 0;
+            Fase_M2_C = 1;
+            wait(tempo);
+            Fase_M2_C = 0;
+            Fase_M2_D = 1;
+            wait(tempo);
+            Fase_M2_D = 0;
+            PosicaoUsuario[1] = PosicaoUsuario[1] + 5;
+            
+            }
+        if (Fim == 1) {
+            estado = 0;
+            }
+        }
     }
 
-void recebe_comandos(){ //Recebe os comandos do IHM
-    
+void Botao_Emergencia() {
+    if (debounce.read_ms()>200) {
+        Emergencia = 1;
+        debounce.reset();
+        }
+    }
+
+void movimentacao_inicial_X_esquerda() {
+    Fase_M1_D = 1;
+    wait(tempo);
+    Fase_M1_D = 0;
+    Fase_M1_C = 1;
+    wait(tempo);
+    Fase_M1_C = 0;
+    Fase_M1_B = 1;
+    wait(tempo);
+    Fase_M1_B = 0;
+    Fase_M1_A = 1;
+    wait(tempo);
+    Fase_M1_A = 0;
+    }
+
+void movimentacao_inicial_Y_esquerda() {
+    Fase_M1_D = 1;
+    wait(tempo);
+    Fase_M1_D = 0;
+    Fase_M1_C = 1;
+    wait(tempo);
+    Fase_M1_C = 0;
+    Fase_M1_B = 1;
+    wait(tempo);
+    Fase_M1_B = 0;
+    Fase_M1_A = 1;
+    wait(tempo);
+    Fase_M1_A = 0;
+    }
+
+
+void movimentacao_inicial_Z_esquerda() {
+    Fase_M1_D = 1;
+    wait(tempo);
+    Fase_M1_D = 0;
+    Fase_M1_C = 1;
+    wait(tempo);
+    Fase_M1_C = 0;
+    Fase_M1_B = 1;
+    wait(tempo);
+    Fase_M1_B = 0;
+    Fase_M1_A = 1;
+    wait(tempo);
+    Fase_M1_A = 0;
     }
 
+
+
+void zeramento() {
+    int Posicao_inicial[3];
+    int parada_total_X = 0;
+    int parada_total_Y = 0;
+    int parada_total_Z = 0;
+    int paradaX_esquerda = 0;
+    int paradaY_esquerda = 0;
+    int paradaZ_esquerda = 0;
+    int SensorX_1 = SensorParadaX_1;
+    int SensorY_1 = SensorParadaY_1;
+    int SensorZ_1 = SensorParadaZ_1;
+    
+    while (parada_total_X == 0) { //Eixo X começa a se mover, quando o sensor de parada for ativado ele para e começa a rodar no sentido contrário.
+             
+        if (paradaX_esquerda == 1) {
+            movimentacao_inicial_X_esquerda(); //roda a função de movimentação do eixo X
+            
+            if (SensorX_1 == 1) {
+                Posicao_inicial[0] = 0;
+                paradaX_esquerda = 0;
+                parada_total_X = 1;
+                }
+            }
+        }
+    
+    while (parada_total_Y == 0) { //Eixo X começa a se mover, quando o sensor de parada for ativado ele para e começa a rodar no sentido contrário.
+        
+        if (paradaY_esquerda == 1) {
+            movimentacao_inicial_Y_esquerda(); //roda a função de movimentação do eixo X
+            
+            if (SensorY_1 == 1) {
+                Posicao_inicial[1] = 0;
+                paradaY_esquerda = 0;
+                parada_total_Y = 1;
+                }
+            }
+        }
+                
+    while (parada_total_Z == 0) { //Eixo X começa a se mover, quando o sensor de parada for ativado ele para e começa a rodar no sentido contrário.
+        
+        if (paradaZ_esquerda == 1) {
+            movimentacao_inicial_Z_esquerda(); //roda a função de movimentação do eixo X
+            
+            if (SensorZ_1 == 1) {
+                Posicao_inicial[2] = 0;
+                paradaZ_esquerda = 0;
+                parada_total_Z = 1;
+                }
+            }
+        }     
+    }
+
+void ativa_pipeta() { // Função para fazer a pipeta sugar/soltar o líquido - 1ml
+    Pipeta = 0;
+    wait(0.2);
+    Pipeta = 1;
+    }
+    
 void avaliar_distancia(){ //Função para detectar e movimentar a pipeta até o local da pega
     int x = 0;
     int y = 0;
@@ -205,6 +518,10 @@
             }
         
         if (Posicao_inicial[0] == PosicaoRecipiente[0]) {
+            Fase_M1_A = 0;
+            Fase_M1_B = 0;
+            Fase_M1_C = 0;
+            Fase_M1_D = 0;
             x_2 = 1;
             }
         }
@@ -245,9 +562,13 @@
          }
      
      if (Posicao_inicial[1] == PosicaoRecipiente[1]) {
-         y_2 = 1;
-         }
-      }
+        Fase_M2_A = 0;
+        Fase_M2_B = 0;
+        Fase_M2_C = 0;
+        Fase_M2_D = 0;
+        y_2 = 1;
+        }
+    }
     
     while (z_2 == 0){ // Checa se a pipeta está na posição de solta, se n estiver ela se movimenta até lá
      if (Posicao_inicial[2] < PosicaoRecipiente[2]) {
@@ -285,20 +606,28 @@
          }
     
      if (Posicao_inicial[2] == PosicaoRecipiente[2]) {
-         z_2 = 1;
-         }
+        Fase_M3_A = 0;
+        Fase_M3_B = 0;
+        Fase_M3_C = 0;
+        Fase_M3_D = 0;
+        z_2 = 1;
+        }
      } 
-    
+    ativa_pipeta();
 }
 
 void repetir(){
-    
+    for (i=0 , i<3 , i++) {
+        avaliar_distancia()
+        }
     }
 
 int main() {
+    debounce.start();
+    butao.rise(&Botao_Emergencia);
     zeramento();
     Joystick();
-    
+    repetir();
     }