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@10:c26257e7778a, 2018-08-16 (annotated)
- Committer:
- lcaepusp
- Date:
- Thu Aug 16 13:39:52 2018 +0000
- Revision:
- 10:c26257e7778a
- Parent:
- 9:3e57f111bace
Biblioteca de Prote??o 2018 - Novo sentido de rota??o do bra?o
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 | 10:c26257e7778a | 33 | Ctrl_Motor = min(5*Ctrl_dPhi, 1); |
lcaepusp | 10:c26257e7778a | 34 | Ctrl_Direita = 0; |
lcaepusp | 10:c26257e7778a | 35 | Ctrl_Esquerda = 1; |
lcaepusp | 10:c26257e7778a | 36 | } |
lcaepusp | 10:c26257e7778a | 37 | else if(Ctrl_dPhi<=-0.01){ |
lcaepusp | 10:c26257e7778a | 38 | Ctrl_Motor = min(-5*Ctrl_dPhi, 1); |
ArthurCBdO | 0:f2bc43be0fc7 | 39 | Ctrl_Direita = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 40 | Ctrl_Esquerda = 0; |
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 | |
lcaepusp | 10:c26257e7778a | 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; |
lcaepusp | 10:c26257e7778a | 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(); |
lcaepusp | 10:c26257e7778a | 61 | Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 0.1); |
lcaepusp | 10:c26257e7778a | 62 | Ctrl_Trava.attach(&Ctrl_Travar, Ctrl_Ts/10); |
ArthurCBdO | 0:f2bc43be0fc7 | 63 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 64 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 65 | |
ArthurCBdO | 0:f2bc43be0fc7 | 66 | void Ctrl_BlinkOk(void){ |
ArthurCBdO | 0:f2bc43be0fc7 | 67 | Ctrl_Status = !Ctrl_Status; |
ArthurCBdO | 0:f2bc43be0fc7 | 68 | } |
ArthurCBdO | 0:f2bc43be0fc7 | 69 | |
lcaepusp | 2:01fdfdb7b853 | 70 | void Protecao_Init(QEI* Encoder_Motor, Ticker* Control_Interrupt, double angle){ |
lcaepusp | 2:01fdfdb7b853 | 71 | |
ArthurCBdO | 0:f2bc43be0fc7 | 72 | Ctrl_Encoder_Motor = Encoder_Motor; |
ArthurCBdO | 1:00b8f71cfe7e | 73 | Controler_Interrupt = Control_Interrupt; |
lcaepusp | 2:01fdfdb7b853 | 74 | LimitAngle = angle; |
lcaepusp | 2:01fdfdb7b853 | 75 | |
ArthurCBdO | 0:f2bc43be0fc7 | 76 | Ctrl_Status = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 77 | Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1); |
lcaepusp | 2:01fdfdb7b853 | 78 | |
lcaepusp | 2:01fdfdb7b853 | 79 | if(LimitAngle>0) |
lcaepusp | 2:01fdfdb7b853 | 80 | Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts); |
ArthurCBdO | 0:f2bc43be0fc7 | 81 | |
ArthurCBdO | 0:f2bc43be0fc7 | 82 | Ctrl_Motor = 0.0; |
ArthurCBdO | 0:f2bc43be0fc7 | 83 | Ctrl_Direita = 0; |
ArthurCBdO | 0:f2bc43be0fc7 | 84 | Ctrl_Esquerda = 1; |
ArthurCBdO | 0:f2bc43be0fc7 | 85 | } |