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@12:321ea73f98db, 19 months ago (annotated)

Committer:
lcaepusp
Date:
Tue Sep 27 18:07:40 2022 +0000
Revision:
12:321ea73f98db
Parent:
11:7c32b75ecff1 
Rev sentido rotacao

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 11:7c32b75ecff1 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 12:321ea73f98db 11 DigitalOut Ctrl_Direita(PTC1);//
lcaepusp 12:321ea73f98db 12 DigitalOut Ctrl_Esquerda(PTD5);
lcaepusp 11:7c32b75ecff1 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
lcaepusp 11:7c32b75ecff1 32 if(Ctrl_dPhi>=0.005){
lcaepusp 11:7c32b75ecff1 33 Ctrl_Motor = min(1.5*Ctrl_dPhi, 0.15);
ArthurCBdO 0:f2bc43be0fc7 34 Ctrl_Direita = 1;
ArthurCBdO 0:f2bc43be0fc7 35 Ctrl_Esquerda = 0;
ArthurCBdO 0:f2bc43be0fc7 36 }
lcaepusp 11:7c32b75ecff1 37 else if(Ctrl_dPhi<=-0.005){
lcaepusp 11:7c32b75ecff1 38 Ctrl_Motor = min(-1.5*Ctrl_dPhi, 0.15);
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 11:7c32b75ecff1 57 if(Ctrl_Phi>LimitAngle || Ctrl_Phi<-LimitAngle || Ctrl_dPhi > 1.05 || Ctrl_dPhi < -1.05){
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 }