#define FREQ_PWM        10000.0f
#define FREQ_CONTROLE   1000.0f

#define PER_PWM         (1.0f/FREQ_PWM)
#define PER_CONTROLE    (1.0f/FREQ_CONTROLE)

#include "mbed.h"

DigitalOut operacao(LED_RED);
Serial     Console(USBTX, USBRX);
PwmOut     dt_cycle(PTD4);
AnalogIn   EntradaAnalog(PTB0);

Ticker Controlador;
Ticker Monitor;

float new_dt_cycle;

void MonitorOperacao(void) //cria a rampa de subida e descida no PWM
{
    operacao = !operacao;
}

float ReferenciaVS;
float FeedbackVS;
float LeituraVS;
float ErroControlador;
float Integral;
float DecisaoComando;
float GanhoP;
float GanhoI;
float K;
int LoopClosed;

void FuncaoControle(void) //calcula o controle a atribui a nova decisao de PWM 
                          //para a saida PTD4
{
    LeituraVS = K * EntradaAnalog; //converte a leitura do AD em PTB0 (entre 0.0 e 1.0) no 
                                   //valor de engenharia da tensao externaS
    
    //FeedbackVS = LeituraVS; //sem filtragem
    //FeedbackVS = (FeedbackVS + LeituraVS)/2.0f; //a leitura eh repleta de ruidos, vamos filtrar
    FeedbackVS = (4.0f*FeedbackVS + LeituraVS)/5.0f; //a leitura eh repleta de ruidos, vamos filtrar
    //Pode ser filtrado mais?
        
    //calcula o erro do controlador em [V]
    ErroControlador = ReferenciaVS - FeedbackVS;
    
    //calcula o termo da integral do erro caso a malha esteja fechada
    if(LoopClosed)
    {
        Integral = Integral + ErroControlador*PER_CONTROLE;
        if(Integral<-100.0f) Integral = -100.0f;
        if(Integral>100.0f)  Integral = 100.0f;
    }
    
    //calcula o termo proporcional e o integral que fazem a decisao de comando
    DecisaoComando = GanhoP*ErroControlador + GanhoI*Integral;
  
    if(LoopClosed) //se a malha esta fechada, a DecisaoComando deve ser usada na saida
        new_dt_cycle = DecisaoComando;
    
    //para nao pedir valores de PWM impossiveis
    if(new_dt_cycle<0.0f) new_dt_cycle=0.00f;
    if(new_dt_cycle>1.0f) new_dt_cycle=1.00f;
        
    dt_cycle = 1.0f - new_dt_cycle; //invertido pois o transistor e active LOW
}

int main() 
{
    float *var;
    
    dt_cycle.period(PER_PWM);
    dt_cycle = 1.0f;
    new_dt_cycle = 0.0f;

    ReferenciaVS = 3.0f;
    GanhoP = 0.30f;
    GanhoI = 20.0f;
    K = 14.33f;
    LoopClosed = 0;
    var = &ReferenciaVS;

    Console.printf("\r\n\r\nAVR - Automatic Voltage Regulator\r\n");
    Console.printf("Programa com controlador PI\r\n");
    Console.printf("Pressione ENTER para ajuda com os comandos\r\n");

    Monitor.attach(MonitorOperacao, 1.0f);
    Controlador.attach(FuncaoControle, PER_CONTROLE);
    
    while(1) 
    {
        if(Console.readable())
        {
            switch(Console.getc())
            {
                case '1':
                    var = &ReferenciaVS;
                    Console.printf("Alterando Referencia VS [V]\r\n");
                    break;
                case '2':
                    var = &K;
                    Console.printf("Alterando K - fator de calibracao da medida externa\r\n");
                    break;
                case '3':
                    var = &new_dt_cycle;
                    Console.printf("Alterando a saida PWM [%] - so funciona em malha aberta\r\n");
                    break;
                case '4':
                    var = &GanhoP;
                    Console.printf("Alterando GanhoP\r\n");
                    break;
                case '5':
                    var = &GanhoI;
                    Console.printf("Alterando GanhoI\r\n");
                    break;
                    
                case ' ': //pressionaram ESPACO
                    LoopClosed=!LoopClosed;
                    if(LoopClosed)
                        Console.printf("Malha fechada\r\n");
                    else
                        Console.printf("Malha aberta\r\n");
                    break;    
                
                case 27: //pressionaram ESC
                    LoopClosed=0;
                    Console.printf("EMERGENCIA DETECTADA - Malha aberta e reajuste a valores default\r\n");
                    ReferenciaVS = 3.0f;
                    GanhoP = 0.35f;
                    GanhoI = 20.0f;
                    K = 14.33f;
                    new_dt_cycle = 0.0f;
                    Integral = 0.0f;
                    break;    

                case 'a':
                    if(*var<=100.0) *var+=0.01;
                    break;
                case 'A':
                    if(*var<=100.0) *var+=0.1;
                    break;
                case 'z':
                    if(*var>=0) *var-=0.01;
                    break;
                case 'Z':
                    if(*var>=0) *var-=0.1;
                    break;
                    
                case 13:
                    Console.printf("\r\n\r\nPressione as seguintes teclas para ajustar o AVR\r\n");
                    Console.printf("ESPACO    - para abrir ou fechar a malha de controle\r\n");
                    Console.printf("1,2,3,4,5 - para escolher as variaveis de ajuste\r\n");
                    Console.printf("            1 - RefVs    2 - Calibracao K    3 - saida PWM\r\n");
                    Console.printf("            4 - GanhoP   5 - Ganho I\r\n");
                    Console.printf("a ou A    - para aumentar em 0.01 ou 0.1 a variavel\r\n");
                    Console.printf("z ou Z    - para diminuir em 0.01 ou 0.1 a variavel\r\n");                    
                    Console.printf("ESC       - para emergencia - abre a malha e retorna aos parametros default\r\n");
                    Console.printf("\r\n\r\n");
                    break;
            }
        }

        Console.printf("PWM=%03.1f[%%]  VS=%3.2f[V]  VRef=%3.2f[V]   K=%3.2f  GanhoP=%2.2f  GanhoI=%2.2f\r\n", 
                        new_dt_cycle * 100.0f,
                        FeedbackVS,
                        ReferenciaVS,
                        K, 
                        GanhoP, 
                        GanhoI);
        wait(0.05f);
    }
}