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
PTC3471.cpp@9:3e57f111bace, 2018-08-09 (annotated)
- Committer:
- lcaepusp
- Date:
- Thu Aug 09 19:45:20 2018 +0000
- Revision:
- 9:3e57f111bace
- Parent:
- 5:87f427c20e48
- Child:
- 10:c26257e7778a
Template PTC3471 2018
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ArthurCBdO | 0:f2bc43be0fc7 | 1 | #include "PTC3471.h" |
ArthurCBdO | 0:f2bc43be0fc7 | 2 | #include "mbed.h" |
ArthurCBdO | 0:f2bc43be0fc7 | 3 | #include "QEI.h" |
lcaepusp | 9:3e57f111bace | 4 | /////////////////////#include "USBSerial.h" |
ArthurCBdO | 0:f2bc43be0fc7 | 5 | |
ArthurCBdO | 0:f2bc43be0fc7 | 6 | DigitalOut Ctrl_Status(LED1); |
ArthurCBdO | 0:f2bc43be0fc7 | 7 | |
ArthurCBdO | 0:f2bc43be0fc7 | 8 | QEI * Ctrl_Encoder_Motor; |
ArthurCBdO | 1:00b8f71cfe7e | 9 | Ticker * Controler_Interrupt; |
ArthurCBdO | 0:f2bc43be0fc7 | 10 | |
lcaepusp | 9:3e57f111bace | 11 | DigitalOut Ctrl_Direita(PTD5);// |
lcaepusp | 5:87f427c20e48 | 12 | DigitalOut Ctrl_Esquerda(PTC1); |
lcaepusp | 9:3e57f111bace | 13 | PwmOut Ctrl_Motor(PTD6);// |
ArthurCBdO | 0:f2bc43be0fc7 | 14 | |
ArthurCBdO | 0:f2bc43be0fc7 | 15 | Ticker Ctrl_StatusOk; |
ArthurCBdO | 0:f2bc43be0fc7 | 16 | Ticker Ctrl_Trava; |
ArthurCBdO | 0:f2bc43be0fc7 | 17 | |
ArthurCBdO | 1:00b8f71cfe7e | 18 | double Ctrl_Phi = 0; |
ArthurCBdO | 1:00b8f71cfe7e | 19 | double Ctrl_Phi1 = 0; |
ArthurCBdO | 1:00b8f71cfe7e | 20 | double Ctrl_dPhi = 0; |
lcaepusp | 2:01fdfdb7b853 | 21 | double LimitAngle = 0; |
ArthurCBdO | 0:f2bc43be0fc7 | 22 | |
ArthurCBdO | 0:f2bc43be0fc7 | 23 | void Ctrl_LerAngulos(void){ |
ArthurCBdO | 0:f2bc43be0fc7 | 24 | Ctrl_Phi1 = Ctrl_Phi; |
ArthurCBdO | 0:f2bc43be0fc7 | 25 | Ctrl_Phi = pi*Ctrl_Encoder_Motor->getPulses()/600.0; |
ArthurCBdO | 0:f2bc43be0fc7 | 26 | Ctrl_dPhi = (Ctrl_Phi-Ctrl_Phi1); |
ArthurCBdO | 0:f2bc43be0fc7 | 27 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 28 | |
ArthurCBdO | 0:f2bc43be0fc7 | 29 | void Ctrl_Travar(void){ |
ArthurCBdO | 0:f2bc43be0fc7 | 30 | Ctrl_LerAngulos(); |
ArthurCBdO | 0:f2bc43be0fc7 | 31 | |
ArthurCBdO | 0:f2bc43be0fc7 | 32 | if(Ctrl_dPhi>=0.01){ |
lcaepusp | 9:3e57f111bace | 33 | Ctrl_Motor = min(2*Ctrl_dPhi, 0.3); |
ArthurCBdO | 0:f2bc43be0fc7 | 34 | Ctrl_Direita = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 35 | Ctrl_Esquerda = 0; |
ArthurCBdO | 0:f2bc43be0fc7 | 36 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 37 | else if(Ctrl_dPhi<=-0.01){ |
lcaepusp | 9:3e57f111bace | 38 | Ctrl_Motor = min(-2*Ctrl_dPhi, 0.3); |
ArthurCBdO | 0:f2bc43be0fc7 | 39 | Ctrl_Direita = 0; |
ArthurCBdO | 0:f2bc43be0fc7 | 40 | Ctrl_Esquerda = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 41 | } |
ArthurCBdO | 1:00b8f71cfe7e | 42 | else{ |
ArthurCBdO | 1:00b8f71cfe7e | 43 | Ctrl_Direita=1; |
ArthurCBdO | 1:00b8f71cfe7e | 44 | Ctrl_Esquerda=1; |
ArthurCBdO | 1:00b8f71cfe7e | 45 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 46 | |
ArthurCBdO | 0:f2bc43be0fc7 | 47 | if(Ctrl_Status==0){ |
ArthurCBdO | 0:f2bc43be0fc7 | 48 | Ctrl_Status = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 49 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 50 | else{ |
ArthurCBdO | 0:f2bc43be0fc7 | 51 | Ctrl_Status = 0; |
ArthurCBdO | 0:f2bc43be0fc7 | 52 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 53 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 54 | |
ArthurCBdO | 0:f2bc43be0fc7 | 55 | void Ctrl_ChecarTrava(void){ |
ArthurCBdO | 1:00b8f71cfe7e | 56 | Ctrl_LerAngulos(); |
lcaepusp | 2:01fdfdb7b853 | 57 | if(Ctrl_Phi>LimitAngle || Ctrl_Phi<-LimitAngle){ |
ArthurCBdO | 0:f2bc43be0fc7 | 58 | Ctrl_StatusOk.detach(); |
ArthurCBdO | 0:f2bc43be0fc7 | 59 | Ctrl_Trava.detach(); |
ArthurCBdO | 1:00b8f71cfe7e | 60 | Controler_Interrupt->detach(); |
ArthurCBdO | 1:00b8f71cfe7e | 61 | Ctrl_Trava.attach(&Ctrl_Travar, Ctrl_Ts); |
ArthurCBdO | 0:f2bc43be0fc7 | 62 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 63 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 64 | |
ArthurCBdO | 0:f2bc43be0fc7 | 65 | void Ctrl_BlinkOk(void){ |
ArthurCBdO | 0:f2bc43be0fc7 | 66 | Ctrl_Status = !Ctrl_Status; |
ArthurCBdO | 0:f2bc43be0fc7 | 67 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 68 | |
lcaepusp | 2:01fdfdb7b853 | 69 | void Protecao_Init(QEI* Encoder_Motor, Ticker* Control_Interrupt, double angle){ |
lcaepusp | 2:01fdfdb7b853 | 70 | |
ArthurCBdO | 0:f2bc43be0fc7 | 71 | Ctrl_Encoder_Motor = Encoder_Motor; |
ArthurCBdO | 1:00b8f71cfe7e | 72 | Controler_Interrupt = Control_Interrupt; |
lcaepusp | 2:01fdfdb7b853 | 73 | LimitAngle = angle; |
lcaepusp | 2:01fdfdb7b853 | 74 | |
ArthurCBdO | 0:f2bc43be0fc7 | 75 | Ctrl_Status = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 76 | Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1); |
lcaepusp | 2:01fdfdb7b853 | 77 | |
lcaepusp | 2:01fdfdb7b853 | 78 | if(LimitAngle>0) |
lcaepusp | 2:01fdfdb7b853 | 79 | Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts); |
ArthurCBdO | 0:f2bc43be0fc7 | 80 | |
ArthurCBdO | 0:f2bc43be0fc7 | 81 | Ctrl_Motor = 0.0; |
ArthurCBdO | 0:f2bc43be0fc7 | 82 | Ctrl_Direita = 0; |
ArthurCBdO | 0:f2bc43be0fc7 | 83 | Ctrl_Esquerda = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 84 | } |