Diff: PTC3471.cpp

Revision:

0:f2bc43be0fc7

Child:

1:00b8f71cfe7e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PTC3471.cpp	Mon Aug 07 20:32:20 2017 +0000
@@ -0,0 +1,73 @@
+#include "PTC3471.h"
+#include "mbed.h"
+#include "QEI.h"
+#include "USBSerial.h"
+
+DigitalOut Ctrl_Status(LED1);
+
+QEI * Ctrl_Encoder_Motor;
+
+DigitalOut Ctrl_Direita(PTC1);
+DigitalOut Ctrl_Esquerda(PTD6);
+PwmOut Ctrl_Motor(PTD5);
+
+Ticker Ctrl_StatusOk;
+Ticker Ctrl_Leitura;
+Ticker Ctrl_Trava;
+
+double Ctrl_Phi = 0, Ctrl_Theta = 0;
+double Ctrl_Phi1 = 0, Ctrl_Theta1 = 0;
+double Ctrl_dPhi = 0, Ctrl_dTheta = 0;
+
+void Ctrl_LerAngulos(void){
+    Ctrl_Phi1 = Ctrl_Phi;
+    Ctrl_Phi = pi*Ctrl_Encoder_Motor->getPulses()/600.0;
+    Ctrl_dPhi = (Ctrl_Phi-Ctrl_Phi1);
+}
+
+void Ctrl_Travar(void){
+    Ctrl_LerAngulos();
+    
+    if(Ctrl_dPhi>=0.01){
+        Ctrl_Motor = min(3*Ctrl_dPhi, 0.7);
+        Ctrl_Direita = 1;
+        Ctrl_Esquerda = 0;
+    }
+    else if(Ctrl_dPhi<=-0.01){
+        Ctrl_Motor = min(-3*Ctrl_dPhi, 0.7);
+        Ctrl_Direita = 0;
+        Ctrl_Esquerda = 1;
+    }
+    
+    if(Ctrl_Status==0){
+        Ctrl_Status = 1;
+    }
+    else{
+        Ctrl_Status = 0;
+    }
+}
+
+void Ctrl_ChecarTrava(void){
+    if(Ctrl_Phi>pi || Ctrl_Phi<-pi){
+        Ctrl_StatusOk.detach();
+        Ctrl_Leitura.detach();
+        Ctrl_Trava.detach();
+        Ctrl_Trava.attach(&Ctrl_Travar, 2*Ctrl_Ts);
+    }
+}
+
+void Ctrl_BlinkOk(void){
+    Ctrl_Status = !Ctrl_Status;
+}
+
+void Protecao_Init(QEI* Encoder_Motor){
+    Ctrl_Encoder_Motor = Encoder_Motor;
+    Ctrl_Status = 1;
+    Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1);
+    Ctrl_Leitura.attach(&Ctrl_LerAngulos, Ctrl_Ts);
+    Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts);
+    
+    Ctrl_Motor = 0.0;
+    Ctrl_Direita = 0;
+    Ctrl_Esquerda = 1;
+}
\ No newline at end of file