Diff: PTC3471.cpp

Revision:

1:00b8f71cfe7e

Parent:

0:f2bc43be0fc7

Child:

2:01fdfdb7b853

--- a/PTC3471.cpp	Mon Aug 07 20:32:20 2017 +0000
+++ b/PTC3471.cpp	Mon Aug 07 22:32:23 2017 +0000
@@ -6,18 +6,18 @@
 DigitalOut Ctrl_Status(LED1);
 
 QEI * Ctrl_Encoder_Motor;
+Ticker * Controler_Interrupt;
 
 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;
+double Ctrl_Phi = 0;
+double Ctrl_Phi1 = 0;
+double Ctrl_dPhi = 0;
 
 void Ctrl_LerAngulos(void){
     Ctrl_Phi1 = Ctrl_Phi;
@@ -29,15 +29,19 @@
     Ctrl_LerAngulos();
     
     if(Ctrl_dPhi>=0.01){
-        Ctrl_Motor = min(3*Ctrl_dPhi, 0.7);
+        Ctrl_Motor = min(2*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_Motor = min(-2*Ctrl_dPhi, 0.7);
         Ctrl_Direita = 0;
         Ctrl_Esquerda = 1;
     }
+    else{
+        Ctrl_Direita=1;
+        Ctrl_Esquerda=1;
+    }
     
     if(Ctrl_Status==0){
         Ctrl_Status = 1;
@@ -48,11 +52,12 @@
 }
 
 void Ctrl_ChecarTrava(void){
+    Ctrl_LerAngulos();
     if(Ctrl_Phi>pi || Ctrl_Phi<-pi){
         Ctrl_StatusOk.detach();
-        Ctrl_Leitura.detach();
         Ctrl_Trava.detach();
-        Ctrl_Trava.attach(&Ctrl_Travar, 2*Ctrl_Ts);
+        Controler_Interrupt->detach();
+        Ctrl_Trava.attach(&Ctrl_Travar, Ctrl_Ts);
     }
 }
 
@@ -60,11 +65,11 @@
     Ctrl_Status = !Ctrl_Status;
 }
 
-void Protecao_Init(QEI* Encoder_Motor){
+void Protecao_Init(QEI* Encoder_Motor, Ticker* Control_Interrupt){
     Ctrl_Encoder_Motor = Encoder_Motor;
+    Controler_Interrupt = Control_Interrupt;
     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;