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@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?

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"
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 }