Biblioteca PTC3471 - Algoritmo de Trava - V1.0
Dependents: Template_PTC3471_Geral LQR_With_Integrator_Implicito LQR_No_Integrator_Implicito Template_PTC3471_Geral_2020_V2 ... more
Fork of PTC3471 by
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;