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:
- 11:7c32b75ecff1
- Parent:
- 5:87f427c20e48
- Child:
- 12:321ea73f98db
--- a/PTC3471.cpp Fri Aug 25 14:22:16 2017 +0000 +++ b/PTC3471.cpp Thu Sep 15 18:22:58 2022 +0000 @@ -1,16 +1,16 @@ #include "PTC3471.h" #include "mbed.h" #include "QEI.h" -#include "USBSerial.h" +/////////////////////#include "USBSerial.h" DigitalOut Ctrl_Status(LED1); QEI * Ctrl_Encoder_Motor; Ticker * Controler_Interrupt; -DigitalOut Ctrl_Direita(PTD6); +DigitalOut Ctrl_Direita(PTD5);// DigitalOut Ctrl_Esquerda(PTC1); -PwmOut Ctrl_Motor(PTD5); +PwmOut Ctrl_Motor(PTD6);// Ticker Ctrl_StatusOk; Ticker Ctrl_Trava; @@ -29,13 +29,13 @@ void Ctrl_Travar(void){ Ctrl_LerAngulos(); - if(Ctrl_dPhi>=0.01){ - Ctrl_Motor = min(2*Ctrl_dPhi, 0.7); + if(Ctrl_dPhi>=0.005){ + Ctrl_Motor = min(1.5*Ctrl_dPhi, 0.15); Ctrl_Direita = 1; Ctrl_Esquerda = 0; } - else if(Ctrl_dPhi<=-0.01){ - Ctrl_Motor = min(-2*Ctrl_dPhi, 0.7); + else if(Ctrl_dPhi<=-0.005){ + Ctrl_Motor = min(-1.5*Ctrl_dPhi, 0.15); Ctrl_Direita = 0; Ctrl_Esquerda = 1; } @@ -54,7 +54,7 @@ void Ctrl_ChecarTrava(void){ Ctrl_LerAngulos(); - if(Ctrl_Phi>LimitAngle || Ctrl_Phi<-LimitAngle){ + if(Ctrl_Phi>LimitAngle || Ctrl_Phi<-LimitAngle || Ctrl_dPhi > 1.05 || Ctrl_dPhi < -1.05){ Ctrl_StatusOk.detach(); Ctrl_Trava.detach(); Controler_Interrupt->detach();