Laboratório Controle Aplicado / PTC3471

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 Arthur Oliveira

PTC3471.cpp@0:f2bc43be0fc7, 2017-08-07 (annotated)

Committer:
ArthurCBdO
Date:
Mon Aug 07 20:32:20 2017 +0000
Revision:
0:f2bc43be0fc7
Child:
1:00b8f71cfe7e
Template zerado comentado

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ArthurCBdO 0:f2bc43be0fc7 1 #include "PTC3471.h"
ArthurCBdO 0:f2bc43be0fc7 2 #include "mbed.h"
ArthurCBdO 0:f2bc43be0fc7 3 #include "QEI.h"
ArthurCBdO 0:f2bc43be0fc7 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 0:f2bc43be0fc7 9
ArthurCBdO 0:f2bc43be0fc7 10 DigitalOut Ctrl_Direita(PTC1);
ArthurCBdO 0:f2bc43be0fc7 11 DigitalOut Ctrl_Esquerda(PTD6);
ArthurCBdO 0:f2bc43be0fc7 12 PwmOut Ctrl_Motor(PTD5);
ArthurCBdO 0:f2bc43be0fc7 13
ArthurCBdO 0:f2bc43be0fc7 14 Ticker Ctrl_StatusOk;
ArthurCBdO 0:f2bc43be0fc7 15 Ticker Ctrl_Leitura;
ArthurCBdO 0:f2bc43be0fc7 16 Ticker Ctrl_Trava;
ArthurCBdO 0:f2bc43be0fc7 17
ArthurCBdO 0:f2bc43be0fc7 18 double Ctrl_Phi = 0, Ctrl_Theta = 0;
ArthurCBdO 0:f2bc43be0fc7 19 double Ctrl_Phi1 = 0, Ctrl_Theta1 = 0;
ArthurCBdO 0:f2bc43be0fc7 20 double Ctrl_dPhi = 0, Ctrl_dTheta = 0;
ArthurCBdO 0:f2bc43be0fc7 21
ArthurCBdO 0:f2bc43be0fc7 22 void Ctrl_LerAngulos(void){
ArthurCBdO 0:f2bc43be0fc7 23 Ctrl_Phi1 = Ctrl_Phi;
ArthurCBdO 0:f2bc43be0fc7 24 Ctrl_Phi = pi*Ctrl_Encoder_Motor->getPulses()/600.0;
ArthurCBdO 0:f2bc43be0fc7 25 Ctrl_dPhi = (Ctrl_Phi-Ctrl_Phi1);
ArthurCBdO 0:f2bc43be0fc7 26 }
ArthurCBdO 0:f2bc43be0fc7 27
ArthurCBdO 0:f2bc43be0fc7 28 void Ctrl_Travar(void){
ArthurCBdO 0:f2bc43be0fc7 29 Ctrl_LerAngulos();
ArthurCBdO 0:f2bc43be0fc7 30
ArthurCBdO 0:f2bc43be0fc7 31 if(Ctrl_dPhi>=0.01){
ArthurCBdO 0:f2bc43be0fc7 32 Ctrl_Motor = min(3*Ctrl_dPhi, 0.7);
ArthurCBdO 0:f2bc43be0fc7 33 Ctrl_Direita = 1;
ArthurCBdO 0:f2bc43be0fc7 34 Ctrl_Esquerda = 0;
ArthurCBdO 0:f2bc43be0fc7 35 }
ArthurCBdO 0:f2bc43be0fc7 36 else if(Ctrl_dPhi<=-0.01){
ArthurCBdO 0:f2bc43be0fc7 37 Ctrl_Motor = min(-3*Ctrl_dPhi, 0.7);
ArthurCBdO 0:f2bc43be0fc7 38 Ctrl_Direita = 0;
ArthurCBdO 0:f2bc43be0fc7 39 Ctrl_Esquerda = 1;
ArthurCBdO 0:f2bc43be0fc7 40 }
ArthurCBdO 0:f2bc43be0fc7 41
ArthurCBdO 0:f2bc43be0fc7 42 if(Ctrl_Status==0){
ArthurCBdO 0:f2bc43be0fc7 43 Ctrl_Status = 1;
ArthurCBdO 0:f2bc43be0fc7 44 }
ArthurCBdO 0:f2bc43be0fc7 45 else{
ArthurCBdO 0:f2bc43be0fc7 46 Ctrl_Status = 0;
ArthurCBdO 0:f2bc43be0fc7 47 }
ArthurCBdO 0:f2bc43be0fc7 48 }
ArthurCBdO 0:f2bc43be0fc7 49
ArthurCBdO 0:f2bc43be0fc7 50 void Ctrl_ChecarTrava(void){
ArthurCBdO 0:f2bc43be0fc7 51 if(Ctrl_Phi>pi || Ctrl_Phi<-pi){
ArthurCBdO 0:f2bc43be0fc7 52 Ctrl_StatusOk.detach();
ArthurCBdO 0:f2bc43be0fc7 53 Ctrl_Leitura.detach();
ArthurCBdO 0:f2bc43be0fc7 54 Ctrl_Trava.detach();
ArthurCBdO 0:f2bc43be0fc7 55 Ctrl_Trava.attach(&Ctrl_Travar, 2*Ctrl_Ts);
ArthurCBdO 0:f2bc43be0fc7 56 }
ArthurCBdO 0:f2bc43be0fc7 57 }
ArthurCBdO 0:f2bc43be0fc7 58
ArthurCBdO 0:f2bc43be0fc7 59 void Ctrl_BlinkOk(void){
ArthurCBdO 0:f2bc43be0fc7 60 Ctrl_Status = !Ctrl_Status;
ArthurCBdO 0:f2bc43be0fc7 61 }
ArthurCBdO 0:f2bc43be0fc7 62
ArthurCBdO 0:f2bc43be0fc7 63 void Protecao_Init(QEI* Encoder_Motor){
ArthurCBdO 0:f2bc43be0fc7 64 Ctrl_Encoder_Motor = Encoder_Motor;
ArthurCBdO 0:f2bc43be0fc7 65 Ctrl_Status = 1;
ArthurCBdO 0:f2bc43be0fc7 66 Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1);
ArthurCBdO 0:f2bc43be0fc7 67 Ctrl_Leitura.attach(&Ctrl_LerAngulos, Ctrl_Ts);
ArthurCBdO 0:f2bc43be0fc7 68 Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts);
ArthurCBdO 0:f2bc43be0fc7 69
ArthurCBdO 0:f2bc43be0fc7 70 Ctrl_Motor = 0.0;
ArthurCBdO 0:f2bc43be0fc7 71 Ctrl_Direita = 0;
ArthurCBdO 0:f2bc43be0fc7 72 Ctrl_Esquerda = 1;
ArthurCBdO 0:f2bc43be0fc7 73 }