/* 
 * File:   PID.c
 * Author: Christian
 *
 * Created on 27 de mayo de 2019, 14:43
 */

#include "main.h"


/**************
 *  Sensores 
 **************/
double bias;
double Curr_GYRO;
double ACC_angle;
double accData[3];
const double _180PI = 180.0/PI;


/**************
 * Balance PID 
 **************/
double B_SP;    // (Set Point) Valor en el que se alcanza el equilibrio
double B_PV;    // (Process Variable) Valor actual (de la inclinacion)
double B_Pout_2,
       B_Pout;  // (Process Out) Potencia transmitida a las ruedas (en 1000/1000)

double B_pGain;
double B_iGain;
double B_dGain;

PID_TUNING_OBJ Static;
PID_TUNING_OBJ Recover;
PID_TUNING_OBJ Steering;

double B_err;
double B_errOT; // (Error Over Time) Media del error acumulado 
#define errOTMax 400

double B_lastPV;
double B_dPV;   // Derived error

volatile bool BL_PID_ENABLED;

#define errBlindSpot 0.25
#define staticThreshold 2
#define recoveryThreshold 30
double StaticVel;

int differential_zero_offset = 00;
int differential = 1000; // Control de giros. 1000 == reparto equitativo 50-50. 1500 == 75-25

PID_TUNING_STATE B_TuningState;


/**************
 *  Speed PID 
 **************/
#define SPEEDSUM_N 10
double speedSum;
double speedCollection[SPEEDSUM_N];
double* speedCollHead = &speedCollection[0];
double speedMean(double new_speed){
    speedSum += new_speed; 
    speedCollHead[0] = new_speed;
    if(speedCollHead < &speedCollection[SPEEDSUM_N-1]){
        speedCollHead++;
        speedSum -= *speedCollHead;
    }else{
        speedSum -= speedCollection[0];
        speedCollHead = speedCollection;
    }
    return speedSum/SPEEDSUM_N;
}

double S_SP;  // (Set Point) Velocidad deseada
double S_PV;  // (Process Variable) Velocidad actual
double S_Pout; /* (Process Out) Desfase sobre el B_SP 
             * Se suma un desfase sobre el SetPoint del equilibrio para inclinar
             * el robot en la direccion deseada y alcanzar la velocidad deseada.
             */
#define S_PoutMax 8
double S_PoutCap  = S_PoutMax;
double S_pGain;
double S_iGain;
double S_dGain;

PID_TUNING_OBJ Straight;
PID_TUNING_OBJ Turn;

double S_err;
double S_errOT; // (Error Over Time) Media del error acumulado 
double S_errOT_reachedMax; // (Error Over Time) Media del error acumulado 
double twoCmWithGain;
#define S_errOTMax 5

double S_lastPV;
double S_dPV; // Derived error

volatile bool SP_PID_ENABLED;
volatile bool SPID_pout_enabled;
volatile bool SPID_AW_is_ON = true;
volatile char sampling_wheel=0; /* Rueda de la que medir velocidad
                                 * -  0: media de ambas
                                 * -  1: derecha
                                 * -(-1): izquierda
                                 */

PID_TUNING_STATE S_TuningState;


/**************
 * PID Tuner 
 **************/
bool PID_TUNING_IS_ON;

void Inic_PID(void)
{
    BL_PID_ENABLED = BL_PIDNBL;
    SP_PID_ENABLED = SP_PIDNBL;
    
/* Balance PID */  
    B_SP = -0.65;
//    B_SP = -0.90;//0.0;
    /* Estatico */
    Static.p = 0;//35;
    Static.i = 0;//2.6;//2.1;
    Static.d = 0;//100;//130;
    /* Recuperacion */
    
    Recover.p = 35.2;
    Recover.i = 253;
    Recover.d = 1.7099;
    
    /* Giros */
    Steering.p = 0;
    Steering.i = 0;
    Steering.d = 0;
    
    change_toRecover();
//    change_toSteeri6ng();
    StaticVel = cms_to_raw(1.0);
    
//    set_Differential(-1000);
//    set_sampling_wheel(-1);
    
/* Speed PID */    
    S_SP = 0.0;//cms_to_raw(2);
    S_Pout = 0.0;
    
    
    Straight.p = 0.095;
    Straight.i = 0.24;
    Straight.d = 0.002;
    
    Turn.p = 0.0095;
    Turn.i = 0.024;
    Turn.d = 0.0002;
    
    Turn.p = 0.019;
    Turn.i = 0.048;
    Turn.d = 0.0004;
    
    Turn.p = 0.0475;
    Turn.i = 0.012;
    Turn.d = 0.001;
    
    change_S_toStraight();
//    change_S_toTurn();
    
    SPID_pout_enabled = true;
    
    twoCmWithGain = 2.0*S_iGain;
    
    // Primera lectura del sensor
    B_PV=0.0;
    ACC_readSensor(accData);
    B_PV = asinf(accData[2]/(sqrtf(powf(accData[2],2)+powf(accData[1],2))))*_180PI;
    GYR_angle = B_PV;
}

volatile double GYR_angle;
void Sensor_fusion(void)
{
    /* Operaciones trigonometricas para calcular algulos independientemente 
     * accData[]:
     *  - 0: X
     *  - 1: Z
     *  - 2: Y
     */
    double rZY = sqrtf(powf(accData[2],2)+powf(accData[1],2)); //Modulo del vector combinacion de los ejes Z y X del acelerómetro.
    ACC_angle = asinf(accData[2]/rZY)*_180PI; //Calcular angulo entre ZX y Z = inclinacion (absoluta) del robot.
    GYR_angle = GYR_angle + Curr_GYRO * PRSeconds; //Inclinacion (relativa) del robot, proviniente del giroscopo
    
    /* Filtro Complementario */
    B_PV = a * (B_PV + Curr_GYRO * PRSeconds) + (1-a) * ACC_angle; //Fusion de sensores con filtro complementario 
}
/* Otros Metodos */
    bool get_TiltDirection(void){
        return B_err < 0;
    }
    bool get_BetweenStaticMargin(void){
        return ((B_err)<staticThreshold && B_err>-staticThreshold);
    }
    bool get_BetweenRecoveryMargin(void){
        return ((B_err)<recoveryThreshold && B_err>-recoveryThreshold);
    }
    bool get_BetweenBlindSpot(void){
        return ((B_err)<errBlindSpot && B_err>-errBlindSpot);
    }
    
    
    
volatile int T4_PrevT_B = 0,T4_Time;
volatile int T4_PrevT_S = 0;
double At; // delta de t . Diferencia de tiempo entre una iteración y la anterior
void Balance_PID(void)
{
    /* PV PID */
    B_err = B_PV- (S_Pout+B_SP);
    if(BL_PID_ENABLED ){
        
        T4_Time=_TFCTMR;
        At = ((double)(T4_Time+(_TFCPR-T4_PrevT_B)))*T4_TimeSeconds_Const;
        
        B_errOT += B_iGain*B_err*At;
        B_dPV = (B_PV-B_lastPV)/At;
        
        T4_PrevT_B = T4_Time;
        B_lastPV = B_PV;
        
        /*Anti-Windup*/
        if(B_errOT > 1000){ // 1000 es el 100% del PWM
            B_errOT = 1000;
        }else if(B_errOT < -1000){
            B_errOT = -1000; 
        }
        /*Ecuacion del PID*/
        B_Pout = B_pGain*B_err + B_errOT + B_dGain*B_dPV ;
        
        /*Diferencial y limitador del output*/
        int aux = (int16_t)(((int32_t)B_Pout * (int32_t)(differential+differential_zero_offset))/1000);
        
        if(aux > 1000){ // 1000 es el 100% del PWM
            aux = 1000;
        }else if(aux < -1000){
            aux = -1000;
        }
        
        int auz = B_Pout -(aux-B_Pout);
        
        if(auz > 1000){ // 1000 es el 100% del PWM
            auz = 1000;
        }else if(auz < -1000){
            auz = -1000;
        }
        RUEDAS_setSpeed_right_p1000(-aux);
        RUEDAS_setSpeed_left_p1000(-auz);
    }
}
/* Otros Metodos */
    void toggle_BalancePID(void){
        BL_PID_ENABLED=!BL_PID_ENABLED;
        if(BL_PID_ENABLED){
            B_err    = 0.0;
            B_errOT  = 0.0;
            B_dPV    = 0.0;
            B_Pout   = 0.0;
        }else{
            RUEDAS_setSpeed_right_p1000(0.0);
            RUEDAS_setSpeed_left_p1000(0.0);
        }
    }
    void enable_BalancePID(void){
        BL_PID_ENABLED=1;

        B_err    = 0.0;
        B_errOT  = 0.0;
        B_dPV    = 0.0;
        B_lastPV = 0.0;
        B_Pout   = 0.0;
    }
    void disable_BalancePID(void){
        BL_PID_ENABLED=0;

        RUEDAS_setSpeed_right_p1000(0.0);
        RUEDAS_setSpeed_left_p1000(0.0);
    }
    void change_pGain(double v){
        B_pGain = v;
        io_cf.GainChange = 1;
    }
    void change_iGain(double v){
        B_iGain = v;
        io_cf.GainChange = 1;
    }
    void change_dGain(double v){
        B_dGain = v;
        io_cf.GainChange = 1;
    }
    void change_toStatic(){
        B_pGain = Static.p;
        B_iGain = Static.i;
        B_dGain = Static.d;
        
        B_TuningState = Static_S;
    }
    void change_toRecover(void){
        B_pGain = Recover.p;
        B_iGain = Recover.i;
        B_dGain = Recover.d;
        
        B_TuningState = Recover_S;
    }
    void change_toSteering(void){
        B_pGain = Steering.p;
        B_iGain = Steering.i;
        B_dGain = Steering.d;
        
        B_TuningState = Steering_S;
    }
    void save_Static(void){
        Static.p = B_pGain ;
        Static.i = B_iGain ;
        Static.d = B_dGain ;
    }
    void save_Recover(void){
        Recover.p = B_pGain ;
        Recover.i = B_iGain ;
        Recover.d = B_dGain ;
    }
    void save_Advance(void){
        Steering.p = B_pGain ;
        Steering.i = B_iGain ;
        Steering.d = B_dGain ;
    }
    void set_Differential(int nDifferential){
        if (nDifferential <= 1000 && nDifferential >= -1000){
            differential = nDifferential+1000;
        }
    }

void Speed_PID(double S_pv)
{
    double S_PoutAux;
    S_PV = speedMean(S_pv); // Suavizado de la velocidad (media de 10)
    
    if(SP_PID_ENABLED){
        
        T4_Time=_TFCTMR;
        At = ((double)(T4_Time+(_TFCPR-T4_PrevT_S)))*T4_TimeSeconds_Const;
        
        S_err = S_PV-S_SP;              /*Termino Proportional*/
//        S_errOT += S_iGain * S_err; 
//        S_dPV = S_lastPV - S_PV;    
        S_errOT += S_iGain*S_err*At;    /*Termino   Integral  */
        S_dPV = (S_lastPV - S_PV)/At;   /*Termino  Derivativo */
        
        T4_PrevT_S = T4_Time;
        S_lastPV = S_PV; // Guardar ultimo valor de PV
        
        /* Anti-Windup */
        if(SPID_AW_is_ON){
            if(S_errOT > S_errOTMax){           // Este es todo el codigo anti-windup (si SPID_AW_is_ON es true)
                S_errOT = S_errOTMax;           // El resto del codigo es en realidad una pequeña aplicacion
            }else if(S_errOT < -S_errOTMax){    // para demostrar la recuperación de la posicion desactivando el
                S_errOT = -S_errOTMax;          // codigo anti-windup
            }
        }else{
            if((S_errOT > S_errOTMax)){                                     // Se desactiva el codigo anti-windup y se vigila la distancia
                if(S_errOT > S_errOT_reachedMax){                           // recorrida, para limitar el angulo de inclinacion maximo 
                    S_errOT_reachedMax = S_errOT;                           // mientras se recupera la posicion, para evitar que el robot 
                }else if ( S_errOT < (S_errOT_reachedMax-twoCmWithGain)){   // pierda el control.
                    S_PoutCap = 0.5;
                }
            }else if((S_errOT < -S_errOTMax)){                              // Si el error integral supera el maximo del anti-windup
                if(S_errOT < S_errOT_reachedMax){                           // Se empieza a registrar la distancia maxima recorrida
                    S_errOT_reachedMax = S_errOT;
                }else if ( S_errOT > (S_errOT_reachedMax+twoCmWithGain)){   // Si el error integral se reduce 2cm por debajo del maximo alcanzado
                    S_PoutCap = 0.5;                                        // Se limita el angulo que impone el PID a medio grado
                }
            }else{                                                          // Si se vuelve a entrar en los limites del anti-windup
                S_errOT_reachedMax = 0.0;                                   // Se reinicia la distancia maxima para la siguiete situacion de recuperacion de posicion
                S_PoutCap = S_PoutMax;                                      // Se restaura el angulo maximo al original del programa
            }
        }
        
        S_PoutAux = (S_pGain*S_err + S_errOT - S_dGain*S_dPV );
        
        /* limitador del output */
        if(S_PoutAux > S_PoutCap){ 
            S_PoutAux = S_PoutCap;
        }else if(S_PoutAux < -S_PoutCap){
            S_PoutAux = -S_PoutCap;
        }
        
        if(SPID_pout_enabled){  // Permite inhabilitar el S_PID sin que deje de funcionar
            S_Pout = S_PoutAux;
        }
    }
}
    void set_SpeedGoal(double speedGoal){
        S_SP = speedGoal; // Velocidad deseada
//        enable_SpeedPID();
    }
    void toggle_SpeedPID(void){
        SP_PID_ENABLED=!SP_PID_ENABLED;
        if(SP_PID_ENABLED){
            S_err    = 0.0;
            S_errOT  = 0.0;
            S_dPV    = 0.0;
        }else{
            S_Pout = 0.0;
        }
    }
    void enable_SpeedPID(void){
        SP_PID_ENABLED = true;

        S_err    = 0.0;
        S_errOT  = 0.0;
        S_dPV    = 0.0;
    }
    void disable_SpeedPID(void){
        SP_PID_ENABLED=0;

        S_Pout = 0.0;
    }
    void change_S_toStraight(void){
        S_pGain = Straight.p;
        S_iGain = Straight.i;
        S_dGain = Straight.d;
        
        S_TuningState = SStraight_S;
    }
    void change_S_toTurn(void){
        S_pGain = Turn.p;
        S_iGain = Turn.i;
        S_dGain = Turn.d;
        
        S_TuningState = STurn_S;
    }
    void toggle_SPID_AntiWindup(void){
        SPID_AW_is_ON = !SPID_AW_is_ON;
    }
    void set_sampling_wheel(char sw){
        sampling_wheel = sw;
    }
    

PID_Tuner_states PIDT_StateMachine = Init;
double *tuningGain;
double GainIncrement;
double Osc_freq;  
bool fliped;
bool keepRunningTuner;

//const double *PIDT_RR;
//const double PIDT_RR_Bal_1 = 1.1/2;
//const double PIDT_RR_Spe_1 = 1.0/2;
//const double PIDT_RR_Bal_2 = 2.0/3;
//const double PIDT_RR_Spe_2 = 2.0/3;
//const double PIDT_RR_Bal_3 = 3.0/5;
//const double PIDT_RR_Spe_3 = 3.0/5;

//double OC_P_Thresh;
//const double OC_P_Bal_Thresh = 0;
//const double OC_P_Spe_Thresh = 0;
//double OC_I_Thresh;
//const double OC_I_Bal_Thresh = 0;
//const double OC_I_Spe_Thresh = 0;
//double OC_D_Thresh;
//const double OC_D_Bal_Thresh = 10;
//const double OC_D_Spe_Thresh = 12;

//#define OC_P_tuning    ((peakdif_mean-oscillation_freq)<*OC_P_Thresh || oscillation_freq < 1.0 || fliped)
//#define OC_I_tuning    ((peakdif_mean-oscillation_freq)<*OC_I_Thresh || oscillation_freq < 1.0 || fliped)
//#define OC_D_tuning_2    (oscillation_freq > oscillation_freq/peakdif_mean && oscillation_freq > *OC_D_Thresh)
//#define OC_dampening     (!(oscillation_mean< 16E6/6 &&   peakdif_mean> 2.5) && peakdif_mean < oscillation_freq)
//
#define BPIDT_OSCI_Parameter &B_err
#define SPIDT_OSCI_Parameter &S_err
//void PID_tuner(bool restart)
//{
//    if(restart) PIDT_StateMachine = Init;
//    switch (PIDT_StateMachine){
//        case Init:
//            if(!FUNCTION==BPIDT_OSCI_Parameter){                
//                B_pGain = 20.0;
//                B_iGain = 1.0;
//                B_dGain = 0.0;
//                tuningGain = &B_pGain;
//                GainIncrement = B_pG_Increment;
//                
//                OC_P_Thresh = &OC_P_Bal_Thresh;
//                OC_I_Thresh = &OC_I_Bal_Thresh;
//                OC_D_Thresh = &OC_D_Bal_Thresh;
//                
//                PIDT_RR = &PIDT_RR_Bal_1;
//                
//                enable_RUEDAS();
//                enable_BalancePID();
//                
//                PID_TUNING_IS_ON = true;
//                PIDT_StateMachine = Calibrate_pG;
//                
//                fliped = false;
//                keepRunningTuner = true;
//                WL_registerMileStone(PIDTFreq,PID_TUNER_returnToPIDTuner,1);
//            }else{                
//                S_pGain = 0.050;
//                S_iGain = 0.0;
//                S_dGain = 0.0;
//                tuningGain = &S_pGain;
//                GainIncrement = S_pG_Increment;
//                
//                OC_P_Thresh = &OC_P_Spe_Thresh;
//                OC_I_Thresh = &OC_I_Spe_Thresh;
//                OC_D_Thresh = &OC_D_Spe_Thresh;
//                
//                PIDT_RR = &PIDT_RR_Spe_1;
//                
//                enable_RUEDAS();
//                enable_BalancePID();
//                enable_SpeedPID();
//                
//                PID_TUNING_IS_ON = true;
//                PIDT_StateMachine = Calibrate_pG;
//                
//                fliped = false;
//                keepRunningTuner = true;
//                WL_registerMileStone(PIDTFreq,PID_TUNER_returnToPIDTuner,1);
//            }
//            break;
//        case Calibrate_pG:
//            if(/*1/*/OC_P_tuning/**/){
//                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
//                sprintf(c,"Tune pGain:%0.3f\rf:%2.0f/7 A:%1.0f/7 O:%d",*tuningGain,16E6/oscillation_mean, peakdif_mean, OSCILATING);
//                setLCDC(0x00);LCD_Send_String(c);
//            }else{
//                fliped = true;
//                *tuningGain *= *PIDT_RR;
//                PIDT_StateMachine = Calibrate_iG_0;
//                sprintf(c,"Def pGain:%0.3f    \rWait osc to dampen",*tuningGain);
//                setLCDC(0x00);LCD_Send_String(c);
//                if(!FUNCTION==BPIDT_OSCI_Parameter){
//                    tuningGain = &B_iGain;
//                    GainIncrement = B_iG_Increment;
//                    PIDT_RR = &PIDT_RR_Bal_2;
//                }else{
//                    tuningGain = &S_iGain;
//                    GainIncrement = S_iG_Increment;
//                    PIDT_RR = &PIDT_RR_Spe_2;
//                    SPID_pout_enabled = false;
//                    S_Pout = 0.0;
//                }
//                iGainMean = 0.0;
//                iGainIterations = 0;
//            }
//            break;
//        case Calibrate_iG_0:
//            if(OC_dampening){
//                PIDT_StateMachine = Calibrate_iG_1;
//                fliped = false;
//            }
//            break;
//        case Calibrate_iG_1:
//            PIDT_StateMachine = Calibrate_iG_2;
//            SPID_pout_enabled = true;
//            break;
//        case Calibrate_iG_2:
//            if(/*1/*/OC_I_tuning/**/){
//                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
//                sprintf(c,"Tune iGain:%5.3f\rf:%2.0f/6 A:%1.0f/6.5 O:%d",*tuningGain,16E6/oscillation_mean, peakdif_mean, OSCILATING);
//                setLCDC(0x00);LCD_Send_String(c);
//            }else{
//                fliped = true;
//                PIDT_StateMachine = Calibrate_iG_0;
//                *tuningGain *= *PIDT_RR;
//                iGainMean += *tuningGain;
//                iGainIterations++;
//                if(iGainIterations >= 1){
//                    *tuningGain = iGainMean/iGainIterations;
//                    PIDT_StateMachine = Calibrate_dG_0;
//                    sprintf(c,"final iGain:%5.3f\rWait osc to dampen",*tuningGain);
//                    SPID_pout_enabled = false;
//                    S_Pout = 0.0;
//                    setLCDC(0x00);LCD_Send_String(c);
//                    if(!FUNCTION==BPIDT_OSCI_Parameter){
//                        tuningGain = &B_dGain;
//                        GainIncrement = B_dG_Increment;
//                        PIDT_RR = &PIDT_RR_Bal_3;
//                    }else{
//                        tuningGain = &S_dGain;
//                        GainIncrement = S_dG_Increment;
//                        PIDT_RR = &PIDT_RR_Spe_3;
//                    }
//                }else{
//                    sprintf(c,"part iGain:%4.2f\rWait osc to dampen",*tuningGain);
//                    if(!FUNCTION==BPIDT_OSCI_Parameter){
//                    }else{
//                        SPID_pout_enabled = false;
//                        S_Pout = 0.0;
//                    }
//                }
//            }
//            break;
//        case Calibrate_dG_0:
//            if(OC_dampening){
//                PIDT_StateMachine = Calibrate_dG_1;
//                fliped = false;
//                SPID_pout_enabled = true;
//            }
//            break;
//        case Calibrate_dG_1:
//            if(/*1/*/!OC_D_tuning_2/**/){
//                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
//                sprintf(c,"Tune  dGain:%4.2f\rf:%2.0f/6 A:%1.0f/6.5 O:%d",*tuningGain,16E6/oscillation_mean, peakdif_mean, OSCILATING);
//                setLCDC(0x00);LCD_Send_String(c);
//            }else{
//                fliped = true;
//                PIDT_StateMachine = TuningFinished_0;
//                *tuningGain *= *PIDT_RR;
//                sprintf(c,"final dGain:%4.2f\rf:%2.0f/7 A:%1.0f/7 O:%d",*tuningGain,16E6/oscillation_mean, peakdif_mean, OSCILATING);
//                setLCDC(0x00);LCD_Send_String(c);
//            }
//            break;
//        case TuningFinished_0:
//                keepRunningTuner = false;
//                
//                LCD_ClearScreen();
//                sprintf(c,"Tuning process\r\t\tfinished");
//                setLCDC(0x00);LCD_Send_String(c);
//                
//                PIDT_StateMachine++;
//                
//                WL_registerMileStone(1 s,PID_TUNER_returnToPIDTuner,0);
//            break;
//        case TuningFinished_1:
//            
//                LCD_ClearScreen();
//                sprintf(c,"Final results:");
//                setLCDC(0x00);LCD_Send_String(c);
//                
//                PIDT_StateMachine++;
//                
//                WL_registerMileStone(1 s,PID_TUNER_returnToPIDTuner,0);
//            break;
//        case TuningFinished_2:
//            
//                sprintf(c," pGain:");
//                setLCDC(0x40);LCD_Send_String(c);
//                
//                PIDT_StateMachine++;
//                
//                WL_registerMileStone(1 s,PID_TUNER_returnToPIDTuner,0);
//            break;
//        case TuningFinished_3:
//            
//                tuningGain = !FUNCTION==BPIDT_OSCI_Parameter?&B_pGain:&S_pGain;
//            
//                sprintf(c," pGain: %5.2f",*tuningGain);
//                setLCDC(0x40);LCD_Send_String(c);
//                
//                PIDT_StateMachine++;
//                
//                WL_registerMileStone(2 s,PID_TUNER_returnToPIDTuner,0);
//            break;
//        case TuningFinished_4:
//            
//                sprintf(c," iGain:      ");
//                setLCDC(0x40);LCD_Send_String(c);
//                
//                PIDT_StateMachine++;
//                
//                WL_registerMileStone(1 s,PID_TUNER_returnToPIDTuner,0);
//            break;
//        case TuningFinished_5:
//            
//                tuningGain = !FUNCTION==BPIDT_OSCI_Parameter?&B_iGain:&S_iGain;
//            
//                sprintf(c," iGain: %5.3f",*tuningGain);
//                setLCDC(0x40);LCD_Send_String(c);
//                
//                PIDT_StateMachine++;
//                
//                WL_registerMileStone(2 s,PID_TUNER_returnToPIDTuner,0);
//            break;
//        case TuningFinished_6:
//            
//                sprintf(c," dGain:      ");
//                setLCDC(0x40);LCD_Send_String(c);
//                
//                PIDT_StateMachine++;
//                
//                WL_registerMileStone(1 s,PID_TUNER_returnToPIDTuner,0);
//            break;
//        case TuningFinished_7:
//            
//                tuningGain = !FUNCTION==BPIDT_OSCI_Parameter?&B_dGain:&S_dGain;
//            
//                sprintf(c," dGain: %5.1f",*tuningGain);
//                setLCDC(0x40);LCD_Send_String(c);
//                                
//                WL_registerMileStone(1 s,lightshow,0);
//            break;
//        default:break;
//    }
//}

#define PIDT_RR_B_1  1.1/2;
#define PIDT_RR_B_2  2.0/3;
#define PIDT_RR_B_3  3.0/5;

#define OC_BP_Thresh 0.0
#define OC_BI_Thresh 0.0
#define OC_BD_Thresh 10.0

#define OC_BP_tuning    ((Osc_Peakdif_mean-Osc_freq)<OC_BP_Thresh || Osc_freq < 1.0 || fliped)
#define OC_BI_tuning    ((Osc_Peakdif_mean-Osc_freq)<OC_BI_Thresh || Osc_freq < 1.0 || fliped)
#define OC_BD_tuning_2    (Osc_freq > Osc_freq/Osc_Peakdif_mean && Osc_freq > OC_BD_Thresh)
#define OC_B_dampening     (!(Osc_ATime_mean< 16E6/6 &&   Osc_Peakdif_mean> 2.5) && Osc_Peakdif_mean < Osc_freq)
void PID_B_tuner(bool restart)
{
    if(restart) PIDT_StateMachine = Init;
    switch (PIDT_StateMachine){
        case Init:
            FUNCTION = BPIDT_OSCI_Parameter ;
            PIDT_resetOscilometer();
                    
            B_pGain = 20.0;
            B_iGain = 0.0;
            B_dGain = 0.0;
            tuningGain = &B_pGain;
            GainIncrement = B_pG_Increment;

            enable_RUEDAS();
            enable_BalancePID();

            PID_TUNING_IS_ON = true;
            PIDT_StateMachine = Calibrate_pG;

            fliped = false;
            keepRunningTuner = true;
            WL_registerMileStone(PIDTFreq,PID_B_TUNER_returnToPIDTuner,1);

            break;
        case Calibrate_pG:
            if(/*1/*/OC_BP_tuning/**/){
                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
                sprintf(c,"Tune pGain:%0.3f\rf:%2.0f/7 A:%1.0f/7",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }else{
                fliped = true;
                *tuningGain *= PIDT_RR_B_1;
                PIDT_StateMachine = Calibrate_iG_0;
                sprintf(c,"Def pGain:%0.3f    \rWait osc to dampen",*tuningGain);
                setLCDC(0x00);LCD_Send_String(c);
                
                tuningGain = &B_iGain;
                GainIncrement = B_iG_Increment;
                    
            }
            break;
        case Calibrate_iG_0:
            if(OC_B_dampening){
                PIDT_StateMachine = Calibrate_iG_2;
                fliped = false;
            }
            break;
        case Calibrate_iG_2:
            if(/*1/*/OC_BI_tuning/**/){
                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
                sprintf(c,"Tune iGain:%5.3f\rf:%2.0f/6 A:%1.0f/6.5",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }else{
                fliped = true;
                PIDT_StateMachine = Calibrate_iG_0;
                *tuningGain *= PIDT_RR_B_2;
                
                PIDT_StateMachine = Calibrate_dG_0;
                sprintf(c,"final iGain:%5.3f\rWait osc to dampen",*tuningGain);
                setLCDC(0x00);LCD_Send_String(c);

                tuningGain = &B_dGain;
                GainIncrement = B_dG_Increment;
            }
            break;
        case Calibrate_dG_0:
            if(OC_B_dampening){
                PIDT_StateMachine = Calibrate_dG_1;
                fliped = false;
            }
            break;
        case Calibrate_dG_1:
            if(/*1/*/!OC_BD_tuning_2/**/){
                *tuningGain += GainIncrement; //incrementar ganancia derivativa hasta detectar oscilaciones;
                sprintf(c,"Tune  dGain:%4.2f\rf:%2.0f/6 A:%1.0f/6.5",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }else{
                fliped = true;
                PIDT_StateMachine = TuningFinished_0;
                *tuningGain *= PIDT_RR_B_3;
                sprintf(c,"final dGain:%4.2f\rf:%2.0f/7 A:%1.0f/7",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }
            break;
        case TuningFinished_0:
                keepRunningTuner = false;
                
                LCD_ClearScreen();
                sprintf(c,"Tuning process\r\t\tfinished");
                setLCDC(0x00);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_B_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_1:
            
                LCD_ClearScreen();
                sprintf(c,"Final results:");
                setLCDC(0x00);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_B_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_2:
            
                sprintf(c," pGain:");
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_B_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_3:
            
                tuningGain = &B_pGain;
            
                sprintf(c," pGain: %5.2f",*tuningGain);
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(2 s,PID_B_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_4:
            
                sprintf(c," iGain:      ");
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_B_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_5:
            
                tuningGain = &B_iGain;
            
                sprintf(c," iGain: %5.3f",*tuningGain);
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(2 s,PID_B_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_6:
            
                sprintf(c," dGain:      ");
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_B_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_7:
            
                tuningGain = &B_dGain;
            
                sprintf(c," dGain: %5.1f",*tuningGain);
                setLCDC(0x40);LCD_Send_String(c);
                                
                WL_registerMileStone(1 s,lightshow,0);
            break;
        default:break;
    }
}

#define PIDT_RR_S_1  1.0/2;
#define PIDT_RR_S_2  2.0/3;
#define PIDT_RR_S_3  3.0/5;

#define OC_SP_Thresh 0.0
#define OC_SI_Thresh 0.0
#define OC_SD_Thresh 12.0

#define OC_SP_tuning    ((Osc_Peakdif_mean-Osc_freq)<OC_SP_Thresh || Osc_freq < 1.0 || fliped)
#define OC_SI_tuning    ((Osc_Peakdif_mean-Osc_freq)<OC_SI_Thresh || Osc_freq < 1.0 || fliped)
#define OC_SD_tuning_2    (Osc_freq > Osc_freq/Osc_Peakdif_mean && Osc_freq > OC_SD_Thresh)
#define OC_S_dampening     (!(Osc_ATime_mean< 16E6/6 &&   Osc_Peakdif_mean> 2.5) && Osc_Peakdif_mean < Osc_freq)
void PID_S_tuner(bool restart)
{
    if(restart) PIDT_StateMachine = Init;
    switch (PIDT_StateMachine){
        case Init:        
            FUNCTION = SPIDT_OSCI_Parameter;
            PIDT_resetOscilometer();
            
            S_pGain = 0.050;
            S_iGain = 0.0;
            S_dGain = 0.0;
            tuningGain = &S_pGain;
            GainIncrement = S_pG_Increment;

            enable_RUEDAS();
            enable_BalancePID();
            enable_SpeedPID();

            PID_TUNING_IS_ON = true;
            PIDT_StateMachine = Calibrate_pG;

            fliped = false;
            keepRunningTuner = true;
            SPID_pout_enabled = true;
                
            WL_registerMileStone(PIDTFreq,PID_S_TUNER_returnToPIDTuner,1);
            break;
        case Calibrate_pG:
            if(/*1/*/OC_SP_tuning/**/){
                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
                sprintf(c,"Tune pGain:%0.3f\rf:%2.0f/7 A:%1.0f/7",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }else{
                fliped = true;
                *tuningGain *= PIDT_RR_S_1;
                PIDT_StateMachine = Calibrate_iG_0;
                sprintf(c,"Def pGain:%0.3f    \rWait osc to dampen",*tuningGain);
                setLCDC(0x00);LCD_Send_String(c);

                tuningGain = &S_iGain;
                GainIncrement = S_iG_Increment;
                SPID_pout_enabled = false;
                S_Pout = 0.0;
            }
            break;
        case Calibrate_iG_0:
            if(OC_S_dampening){
                PIDT_StateMachine = Calibrate_iG_1;
                fliped = false;
            }
            break;
        case Calibrate_iG_1:
            PIDT_StateMachine = Calibrate_iG_2;
            SPID_pout_enabled = true;
            break;
        case Calibrate_iG_2:
            if(/*1/*/OC_SI_tuning/**/){
                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
                sprintf(c,"Tune iGain:%5.3f\rf:%2.0f/6 A:%1.0f/6.5",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }else{
                fliped = true;
                PIDT_StateMachine = Calibrate_iG_0;
                *tuningGain *= PIDT_RR_S_2;

                PIDT_StateMachine = Calibrate_dG_0;
                sprintf(c,"final iGain:%5.3f\rWait osc to dampen",*tuningGain);
                SPID_pout_enabled = false;
                S_Pout = 0.0;
                setLCDC(0x00);LCD_Send_String(c);

                tuningGain = &S_dGain;
                GainIncrement = S_dG_Increment;
            }
            break;
        case Calibrate_dG_0:
            if(OC_S_dampening){
                PIDT_StateMachine = Calibrate_dG_1;
                fliped = false;
                SPID_pout_enabled = true;
            }
            break;
        case Calibrate_dG_1:
            if(/*1/*/!OC_SD_tuning_2/**/){
                *tuningGain += GainIncrement; //incrementar ganancia proporcional hasta detectar oscilaciones;
                sprintf(c,"Tune  dGain:%4.2f\rf:%2.0f/6 A:%1.0f/6.5",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }else{
                fliped = true;
                PIDT_StateMachine = TuningFinished_0;
                *tuningGain *= PIDT_RR_S_3;
                sprintf(c,"final dGain:%4.2f\rf:%2.0f/7 A:%1.0f/7",*tuningGain,16E6/Osc_ATime_mean, Osc_Peakdif_mean);
                setLCDC(0x00);LCD_Send_String(c);
            }
            break;
        case TuningFinished_0:
                keepRunningTuner = false;
                
                LCD_ClearScreen();
                sprintf(c,"Tuning process\r\t\tfinished");
                setLCDC(0x00);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_S_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_1:
            
                LCD_ClearScreen();
                sprintf(c,"Final results:");
                setLCDC(0x00);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_S_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_2:
            
                sprintf(c," pGain:");
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_S_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_3:
            
                tuningGain = &S_pGain;
            
                sprintf(c," pGain: %5.2f",*tuningGain);
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(2 s,PID_S_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_4:
            
                sprintf(c," iGain:      ");
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_S_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_5:
            
                tuningGain = &S_iGain;
            
                sprintf(c," iGain: %5.3f",*tuningGain);
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(2 s,PID_S_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_6:
            
                sprintf(c," dGain:      ");
                setLCDC(0x40);LCD_Send_String(c);
                
                PIDT_StateMachine++;
                
                WL_registerMileStone(1 s,PID_S_TUNER_returnToPIDTuner,0);
            break;
        case TuningFinished_7:
            
                tuningGain = &S_dGain;
            
                sprintf(c," dGain: %5.1f",*tuningGain);
                setLCDC(0x40);LCD_Send_String(c);
                                
                WL_registerMileStone(1 s,lightshow,0);
            break;
        default:break;
    }
}

    double *FUNCTION;
    char Osc_State = 0;
    char initial_Samples = 0;
    bool direction;
    PID_TUNER_OBJ max,min;
    
    #define extWinSize 10                                   // Ventana de picos capturados
    PID_TUNER_OBJ extremeWindow[extWinSize];               // Cada objeto almacena tiempo, 
    PID_TUNER_OBJ *extremeWindowHead = &extremeWindow[0];  // amplitud y diferencia con el anterior.
                                                            // Se almacenan 10 muestras para realizar
                                                            // la media de cada valor
    double Osc_PrevPreak;       // Amplitud del pico anterior, para calcular diferencia
    uint32_t Osc_PreviousTime;  // Timepo anterior para calcular delta Tiempo
    uint32_t Osc_CurrentTime;   // Tiempo actual para calcular delta Tiempo
    uint32_t Osc_ATime;         // Delta de tiempo.
    double Osc_ATime_sum;       
    double Osc_ATime_mean;
    double Osc_freq;
    double Osc_Peak_sum;
    double Osc_Peak_mean;
    double Osc_Peakdif_sum;
    double Osc_Peakdif_mean;
//    bool oscillating_condition(PID_Tuner_states select){
//        switch(select){
//            case Calibrate_pG:
//                return OC_P_tuning;
//            case Calibrate_iG_2:
//                return OC_I_tuning;
//            case Calibrate_dG_1:
//                return OC_D_tuning_2;
//            case Calibrate_iG_0:
//            case Calibrate_dG_0:
//                return OC_dampening;
//            default:
//                return false;
//        }
//    }
    double PIDT_Oscilometer2(void){
        
        /* Obtener Tiempo actual */
        Osc_CurrentTime = _Pwm_POSTMR;
        
        
        /* Obentener Maximos y minimos */
        switch (Osc_State){
            case 0:
                if(*FUNCTION != 0){
                    direction = *FUNCTION>0;
                    if(direction){
                        max.peak = *FUNCTION;
                        max.ATime = Osc_CurrentTime;
                    }else{
                        min.peak = *FUNCTION;
                        min.ATime = Osc_CurrentTime;
                    }
                    Osc_State++;
                }
                return 0.0;
            case 1:
                if(direction){
                    if(*FUNCTION>=max.peak){
                        max.peak = *FUNCTION;
                        max.ATime = Osc_CurrentTime;
                    }else{
                        /* guardar el tiempo del primer pico como primer previo */
                        Osc_PrevPreak = max.peak;
                        Osc_PreviousTime = max.ATime;
                        
                        min.peak = *FUNCTION;
                        direction = false;
                        Osc_State++;
                    }
                }else{
                    if(*FUNCTION<=min.peak){
                        min.peak = *FUNCTION;
                        min.ATime = Osc_CurrentTime;
                    }else{
                        /* guardar el tiempo del primer pico como primer previo */
                        Osc_PrevPreak = min.peak;
                        Osc_PreviousTime = min.ATime;
                        
                        max.peak = *FUNCTION;
                        direction = true;
                        Osc_State++;
                    }
                }
                return 0.0;
            case 2:
                Nop();
                if(direction){
                    if(*FUNCTION>=max.peak){
                        max.peak = *FUNCTION;
                        max.ATime = Osc_CurrentTime;
                        return 0.0;
                    }else{
                        /* Calcular diferencia de tiempo */
                        Osc_ATime = ((max.ATime<Osc_PreviousTime)?max.ATime+(UINT32_MAX-Osc_PreviousTime):max.ATime-Osc_PreviousTime);
                        Osc_PreviousTime = max.ATime;
                        /* Almacenar informacion */
                        extremeWindowHead[0].peak = max.peak;
                        extremeWindowHead[0].peakdif = max.peak-Osc_PrevPreak;
                        Osc_PrevPreak = max.peak;
                        extremeWindowHead[0].ATime = Osc_ATime;
                        /* Calcular medias */
                        Osc_Peak_sum += max.peak;
                        Osc_Peakdif_sum += extremeWindowHead[0].peakdif;
                        Osc_ATime_sum += Osc_ATime;
                        
                        Osc_Peak_mean = Osc_Peak_sum/initial_Samples;
                        Osc_Peakdif_mean = Osc_Peakdif_sum/initial_Samples;
                        Osc_ATime_mean = Osc_ATime_sum/initial_Samples;
                        initial_Samples++;
                        
                        /* Activar flag de estado OSCILANDO */
//                        OSCILATING = 0;//*/OSCILLATING_CONDITION;
                        
                        extremeWindowHead++;
                        min.peak = *FUNCTION;
                        direction = false;
                    }
                }else{
                    if(*FUNCTION<=min.peak){
                        min.peak = *FUNCTION;
                        min.ATime = Osc_CurrentTime;
                        return 0.0;
                    }else{
                        /* Calcular diferencia de tiempo */
                        Osc_ATime = ((min.ATime<Osc_PreviousTime)?min.ATime+(UINT32_MAX-Osc_PreviousTime):min.ATime-Osc_PreviousTime);
                        Osc_PreviousTime = min.ATime;
                        /* Almacenar informacion */
                        extremeWindowHead[0].peak = min.peak;
                        extremeWindowHead[0].peakdif = Osc_PrevPreak-min.peak;
                        Osc_PrevPreak = min.peak;
                        extremeWindowHead[0].ATime = Osc_ATime;
                        /* Calcular medias */
                        Osc_Peak_sum += min.peak;
                        Osc_Peakdif_sum += extremeWindowHead[0].peakdif;
                        Osc_ATime_sum += Osc_ATime;
                        
                        Osc_Peak_mean = Osc_Peak_sum/initial_Samples;
                        Osc_Peakdif_mean = Osc_Peakdif_sum/initial_Samples;
                        Osc_ATime_mean = Osc_ATime_sum/initial_Samples;
                        initial_Samples++;
                        
                        /* Activar flag de estado OSCILANDO */
//                        OSCILATING = 0;//*/OSCILLATING_CONDITION;
                        
                        extremeWindowHead++;
                        max.peak = *FUNCTION;
                        direction = true;
                    }
                }
                if (initial_Samples>=11){ 
                    Osc_State++;
                    extremeWindowHead = extremeWindow;
                }
                break;
            default:
                Nop();
                if(direction){
                    if(*FUNCTION>=max.peak){
                        max.peak = *FUNCTION;
                        max.ATime = Osc_CurrentTime;
                    }else{
                        /* Se elimina del sumatorio el elemento de la posicion actual en la 
                         * lista antes de actualizarla y añadir el ultimo pico al sumatorio */
                        Osc_ATime_sum -= extremeWindowHead->ATime; 
                        Osc_Peakdif_sum -= extremeWindowHead->peakdif;
                        Osc_Peak_sum -= extremeWindowHead->peak; 
                        
                        /* Calcular diferencia de tiempo */
                        Osc_ATime = ((max.ATime<Osc_PreviousTime)?max.ATime+(UINT32_MAX-Osc_PreviousTime):max.ATime-Osc_PreviousTime);
                        Osc_PreviousTime = max.ATime;
                        /* Almacenar nueva informacion */
                        extremeWindowHead[0].peak = max.peak;
                        extremeWindowHead[0].peakdif = max.peak-Osc_PrevPreak; // diferencia entre picos
                        Osc_PrevPreak = max.peak;
                        extremeWindowHead[0].ATime = Osc_ATime;
                        /* Calcular medias */
                        Osc_Peak_sum += max.peak;
                        Osc_Peakdif_sum += extremeWindowHead[0].peakdif;
                        Osc_ATime_sum += Osc_ATime;
                        
                        Osc_Peak_mean = Osc_Peak_sum/extWinSize;
                        Osc_Peakdif_mean = Osc_Peakdif_sum/extWinSize;
                        Osc_ATime_mean = Osc_ATime_sum/extWinSize;
                        /* Activar flag de estado OSCILANDO */
//                        OSCILATING = oscillating_condition(PIDT_StateMachine);
                        
                        extremeWindowHead++;
                        if(extremeWindowHead >= &extremeWindow[extWinSize]){extremeWindowHead = extremeWindow;}
                        min.peak = *FUNCTION;
                        direction = false;
                    }
                }else{
                    if(*FUNCTION<=min.peak){
                        min.peak = *FUNCTION;
                        min.ATime = Osc_CurrentTime;
                    }else{
                        /* Se elimina del sumatorio el elemento de la posicion actual en la 
                         * lista antes de actualizarla y añadir el ultimo pico al sumatorio */
                        Osc_ATime_sum -= extremeWindowHead->ATime; 
                        Osc_Peakdif_sum -= extremeWindowHead->peakdif;
                        Osc_Peak_sum -= extremeWindowHead->peak;
                        
                        /* Calcular diferencia de tiempo */
                        Osc_ATime = ((min.ATime<Osc_PreviousTime)?min.ATime+(UINT32_MAX-Osc_PreviousTime):min.ATime-Osc_PreviousTime);
                        Osc_PreviousTime = min.ATime;
                        /* Almacenar nueva informacion */
                        extremeWindowHead[0].peak = min.peak;
                        extremeWindowHead[0].peakdif = Osc_PrevPreak-min.peak; // diferencia entre picos
                        Osc_PrevPreak = min.peak;
                        extremeWindowHead[0].ATime = Osc_ATime;
                        /* Calcular medias */
                        Osc_Peak_sum += min.peak;
                        Osc_Peakdif_sum += extremeWindowHead[0].peakdif;
                        Osc_ATime_sum += Osc_ATime;
                        
                        Osc_Peak_mean = Osc_Peak_sum/extWinSize;
                        Osc_Peakdif_mean = Osc_Peakdif_sum/extWinSize;
                        Osc_ATime_mean = Osc_ATime_sum/extWinSize;
                        /* Activar flag de estado OSCILANDO */
//                        OSCILATING = oscillating_condition(PIDT_StateMachine);
                        
                        extremeWindowHead++;
                        if(extremeWindowHead >= &extremeWindow[extWinSize]){extremeWindowHead = extremeWindow;}
                        max.peak = *FUNCTION;
                        direction = true;
                    }
                }
                break;
        }        
        return (Osc_freq = 16E6/Osc_ATime_mean);
    }
    void PIDT_resetOscilometer(){
        
        Osc_State = 0;
        initial_Samples = 1;
        int i;
        for (i=0;i<extWinSize;i++){
            extremeWindow[i].peak = 0.0;
            extremeWindow[i].peakdif = 0.0;
            extremeWindow[i].ATime = 0;
        }
        extremeWindowHead = &extremeWindow[0];
        
        Osc_ATime_mean  = 0.0;
        Osc_ATime_sum   = 0.0;
        Osc_Peak_sum    = 0.0;
        Osc_Peak_mean   = 0.0;
        Osc_Peakdif_sum = 0.0;
        Osc_Peakdif_mean= 0.0;
    }