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@5:87f427c20e48, 2017-08-25 (annotated)

Committer:
lcaepusp
Date:
Fri Aug 25 14:22:16 2017 +0000
Revision:
5:87f427c20e48
Parent:
4:093a4686fe06
Child:
7:afc76d3bbaa1
Child:
9:3e57f111bace
Child:
11:7c32b75ecff1
Template base para uso de controles. Est? inclu?do as interrup??es para trava de seguran?a (n?o alterar exceto quando solicitado).

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 1:00b8f71cfe7e 9 Ticker * Controler_Interrupt;
ArthurCBdO 0:f2bc43be0fc7 10
lcaepusp 5:87f427c20e48 11 DigitalOut Ctrl_Direita(PTD6);
lcaepusp 5:87f427c20e48 12 DigitalOut Ctrl_Esquerda(PTC1);
ArthurCBdO 0:f2bc43be0fc7 13 PwmOut Ctrl_Motor(PTD5);
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){
ArthurCBdO 1:00b8f71cfe7e 33 Ctrl_Motor = min(2*Ctrl_dPhi, 0.7);
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){
ArthurCBdO 1:00b8f71cfe7e 38 Ctrl_Motor = min(-2*Ctrl_dPhi, 0.7);
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 }