File content as of revision 1:00b8f71cfe7e:

#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(PTC1);
DigitalOut Ctrl_Esquerda(PTD6);
PwmOut Ctrl_Motor(PTD5);

Ticker Ctrl_StatusOk;
Ticker Ctrl_Trava;

double Ctrl_Phi = 0;
double Ctrl_Phi1 = 0;
double Ctrl_dPhi = 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>pi || Ctrl_Phi<-pi){
        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){
    Ctrl_Encoder_Motor = Encoder_Motor;
    Controler_Interrupt = Control_Interrupt;
    Ctrl_Status = 1;
    Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1);
    Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts);
    
    Ctrl_Motor = 0.0;
    Ctrl_Direita = 0;
    Ctrl_Esquerda = 1;
}