Biblioteca contendo o algoritmo de segurança para os pendulos de furuta

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers PTC3471.cpp Source File

PTC3471.cpp

00001 #include "PTC3471.h"
00002 #include "mbed.h"
00003 #include "QEI.h"
00004 #include "USBSerial.h"
00005 
00006 DigitalOut Ctrl_Status(LED1);
00007 
00008 QEI * Ctrl_Encoder_Motor;
00009 Ticker * Controler_Interrupt;
00010 
00011 DigitalOut Ctrl_Direita(PTC1);
00012 DigitalOut Ctrl_Esquerda(PTD6);
00013 PwmOut Ctrl_Motor(PTD5);
00014 
00015 Ticker Ctrl_StatusOk;
00016 Ticker Ctrl_Trava;
00017 
00018 double Ctrl_Phi = 0;
00019 double Ctrl_Phi1 = 0;
00020 double Ctrl_dPhi = 0;
00021 
00022 void Ctrl_LerAngulos(void){
00023     Ctrl_Phi1 = Ctrl_Phi;
00024     Ctrl_Phi = pi*Ctrl_Encoder_Motor->getPulses()/600.0;
00025     Ctrl_dPhi = (Ctrl_Phi-Ctrl_Phi1);
00026 }
00027 
00028 void Ctrl_Travar(void){
00029     Ctrl_LerAngulos();
00030     
00031     if(Ctrl_dPhi>=0.01){
00032         Ctrl_Motor = min(2*Ctrl_dPhi, 0.7);
00033         Ctrl_Direita = 1;
00034         Ctrl_Esquerda = 0;
00035     }
00036     else if(Ctrl_dPhi<=-0.01){
00037         Ctrl_Motor = min(-2*Ctrl_dPhi, 0.7);
00038         Ctrl_Direita = 0;
00039         Ctrl_Esquerda = 1;
00040     }
00041     else{
00042         Ctrl_Direita=1;
00043         Ctrl_Esquerda=1;
00044     }
00045     
00046     if(Ctrl_Status==0){
00047         Ctrl_Status = 1;
00048     }
00049     else{
00050         Ctrl_Status = 0;
00051     }
00052 }
00053 
00054 void Ctrl_ChecarTrava(void){
00055     Ctrl_LerAngulos();
00056     if(Ctrl_Phi>pi || Ctrl_Phi<-pi){
00057         Ctrl_StatusOk.detach();
00058         Ctrl_Trava.detach();
00059         Controler_Interrupt->detach();
00060         Ctrl_Trava.attach(&Ctrl_Travar, Ctrl_Ts);
00061     }
00062 }
00063 
00064 void Ctrl_BlinkOk(void){
00065     Ctrl_Status = !Ctrl_Status;
00066 }
00067 
00068 void Protecao_Init(QEI* Encoder_Motor, Ticker* Control_Interrupt){
00069     Ctrl_Encoder_Motor = Encoder_Motor;
00070     Controler_Interrupt = Control_Interrupt;
00071     Ctrl_Status = 1;
00072     Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1);
00073     Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts);
00074     
00075     Ctrl_Motor = 0.0;
00076     Ctrl_Direita = 0;
00077     Ctrl_Esquerda = 1;
00078 }