Biblioteca contendo o algoritmo de segurança para os pendulos de furuta
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Aug 25 2022 00:22:25 by
1.7.2