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
- Committer:
- lcaepusp
- Date:
- 2018-08-09
- Revision:
- 6:505be4b40f6a
- Parent:
- 4:093a4686fe06
- Child:
- 7:afc76d3bbaa1
File content as of revision 6:505be4b40f6a:
#include "PTC3471.h" #include "mbed.h" #include "QEI.h" /////////////////////#include "USBSerial.h" DigitalOut Ctrl_Status(LED1); QEI * Ctrl_Encoder_Motor; Ticker * Controler_Interrupt; DigitalOut Ctrl_Direita(PTD5);// DigitalOut Ctrl_Esquerda(PTC1); PwmOut Ctrl_Motor(PTD6);// Ticker Ctrl_StatusOk; Ticker Ctrl_Trava; double Ctrl_Phi = 0; double Ctrl_Phi1 = 0; double Ctrl_dPhi = 0; double LimitAngle = 0; void Ctrl_LerAngulos(void){ Ctrl_Phi1 = Ctrl_Phi; Ctrl_Phi = pi*Ctrl_Encoder_Motor->getPulses()/600.0; Ctrl_dPhi = (Ctrl_Phi-Ctrl_Phi1); } void Ctrl_Travar(void){ Ctrl_LerAngulos(); if(Ctrl_dPhi>=0.01){ Ctrl_Motor = min(2*Ctrl_dPhi, 0.7); Ctrl_Direita = 1; Ctrl_Esquerda = 0; } else if(Ctrl_dPhi<=-0.01){ Ctrl_Motor = min(-2*Ctrl_dPhi, 0.7); Ctrl_Direita = 0; Ctrl_Esquerda = 1; } else{ Ctrl_Direita=1; Ctrl_Esquerda=1; } if(Ctrl_Status==0){ Ctrl_Status = 1; } else{ Ctrl_Status = 0; } } void Ctrl_ChecarTrava(void){ Ctrl_LerAngulos(); if(Ctrl_Phi>LimitAngle || Ctrl_Phi<-LimitAngle){ Ctrl_StatusOk.detach(); Ctrl_Trava.detach(); Controler_Interrupt->detach(); Ctrl_Trava.attach(&Ctrl_Travar, Ctrl_Ts); } } void Ctrl_BlinkOk(void){ Ctrl_Status = !Ctrl_Status; } void Protecao_Init(QEI* Encoder_Motor, Ticker* Control_Interrupt, double angle){ Ctrl_Encoder_Motor = Encoder_Motor; Controler_Interrupt = Control_Interrupt; LimitAngle = angle; Ctrl_Status = 1; Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1); if(LimitAngle>0) Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts); Ctrl_Motor = 0.0; Ctrl_Direita = 0; Ctrl_Esquerda = 1; }