Diff: main.cpp

Revision:

2:2547de0cde50

Parent:

1:4ba95a6ae582

--- a/main.cpp	Tue May 18 17:59:46 2021 +0000
+++ b/main.cpp	Tue May 18 18:54:20 2021 +0000
@@ -10,17 +10,23 @@
 
 BusOut motor_z(PB_1, PB_15, PB_14, PB_13);
 
-double tempo; //Segundos
+float tempo_horario, tempo_anti_horario; //Segundos
+int pulsos_horario, pulsos_anti_horario;
 int i;
-int Z;
+int J_Z;
+float deslocamento_horario_Z, deslocamento_anti_horario_Z, deslocamento_total_Z;
+float velocidade_Z;
+float passo_motor = 18;//5.625/32;
+int passo_fuso = 5;
 int pegaZ;
 int posicao_salva_estado, botao_emergencia_estado;
 
 void rotina_posicao_salva(void);
 void rotina_emergencia(void);
 void rotida_velocidade_eixo_Z(void);
+void rotida_deslocamento_eixo_Z (void);
 
-float pos_Z;
+
 
 int main()
 {
@@ -32,28 +38,45 @@
 
     while(1) {
 
-        Z = eixo_Z.read_u16();
+        J_Z = eixo_Z.read_u16();
         posicao_salva_estado = posicao_salva.read();
         botao_emergencia_estado = botao_emergencia.read();
-        rotida_velocidade_eixo_Z();
-        pc.printf("Z=%4d, PegaZ=%4d, Posicao_salva_estado=%d, Botao_emergencia_estado=%d \r\n", Z, pegaZ, posicao_salva_estado, botao_emergencia_estado);
+
+        pc.printf("Z=%4d, PegaZ=%4d, Posicao_salva_estado=%d, Botao_emergencia_estado=%d, Tempo1=%4f, Tempo2=%4f, Pulsos=%d, Deslocamento_Z=%f, Velocidade_Z=%f\r\n", J_Z, pegaZ, posicao_salva_estado, botao_emergencia_estado, tempo_horario, tempo_anti_horario, pulsos_anti_horario, deslocamento_total_Z, velocidade_Z);
 
-        if(31000 <= Z &  Z <= 35000) {
+        lcd.locate(0,0);
+        lcd.printf("Posicao Z:");
+        lcd.locate(0,1);
+        lcd.printf("%fmm",deslocamento_total_Z);
+        wait(0.5);
+        lcd.cls();
+
+        if(31000 <= J_Z &  J_Z <= 35000) {
 
             motor_z = 0x00;
 
         } else {
-            if (Z > 31000) {
+            if (J_Z > 31000) {
                 for(i = 0; i < 4; i++) {
                     motor_z = 1 << i;
-                    wait(tempo);
+                    rotida_velocidade_eixo_Z();
+                    rotida_deslocamento_eixo_Z();
+                    wait(tempo_horario);
+                    pulsos_horario+=1;
+                    velocidade_Z = deslocamento_horario_Z/tempo_horario;
+
                 }
             }
 
-            if(Z < 35000) {
+            if(J_Z < 35000) {
                 for(i = 3; i > -1; i--) {
                     motor_z = 1 << i;
-                    wait(tempo);
+                    rotida_velocidade_eixo_Z();
+                    rotida_deslocamento_eixo_Z();
+                    wait(tempo_anti_horario);
+                    pulsos_anti_horario+=1;
+                    velocidade_Z = deslocamento_horario_Z/tempo_horario;
+
                 }
             }
         }
@@ -64,7 +87,7 @@
 
 void rotina_posicao_salva()
 {
-    if(posicao_salva_estado == 0) pegaZ = Z;
+    if(posicao_salva_estado == 0) pegaZ = J_Z;
     else pegaZ = 0;
 }
 
@@ -75,18 +98,13 @@
 
 void rotida_velocidade_eixo_Z()
 {
-    tempo = (Z/65535)*(1/512);
-    
-while(1) {
+    tempo_horario = -1*J_Z*0.0000028 +0.1875;
+    tempo_anti_horario = J_Z*0.000002785 +0.0025;
+}
 
-        pos_Z = eixo_Z;
-        lcd.locate(0,0);
-        lcd.printf("Posicao Z:");
-        lcd.locate(0,1);
-        lcd.printf("%fmm",pos_Z);
-        wait(0.5);
-        lcd.cls();
-
-    }
-
+void rotida_deslocamento_eixo_Z()
+{
+    deslocamento_horario_Z = (passo_fuso*pulsos_horario*passo_motor)/360;
+    deslocamento_anti_horario_Z = (passo_fuso*pulsos_anti_horario*passo_motor)/360;
+    deslocamento_total_Z = deslocamento_anti_horario_Z - deslocamento_horario_Z;
 }
\ No newline at end of file