/*
 * ramp_zadanie_tools.c
 *
 *  Created on: 13 нояб. 2024 г.
 *      Author: Evgeniy_Sokolov
 */



#include <edrk_main.h>

#include <params.h>
#include <params_alg.h>
#include <params_norma.h>
#include <params_pwm24.h>
#include <params_temper_p.h>
#include <project.h>
#include "IQmathLib.h"
#include "mathlib.h"
#include "ramp_zadanie_tools.h"
#include "v_rotor.h"



void change_ramp_zadanie(void)
{
    _iq rampafloat, rampafloat_plus, rampafloat_minus;

    if (edrk.cmd_very_slow_start)
    {
        rampafloat_plus  = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS));
        rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS));
        edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus1 = rampafloat_plus;
        edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus1 = -rampafloat_minus;

        edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus1 = rampafloat_minus;
        edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus1 = -rampafloat_plus;

//        rampafloat_plus  = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS));
//        rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_SLOW_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS));
        edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus2 = rampafloat_plus;
        edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus2 = -rampafloat_minus;

        edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus2 = rampafloat_minus;
        edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus2 = -rampafloat_plus;
    }
    else
    {
        rampafloat_plus  = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS));
        rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS));
        edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus1 = rampafloat_plus;
        edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus1 = -rampafloat_minus;

        edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus1 = rampafloat_minus;
        edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus1 = -rampafloat_plus;

        rampafloat_plus  = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_OBOROTS_ROTOR_PLUS));
        rampafloat_minus = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_OBOROTS_ROTOR_MINUS));
        edrk.zadanie.rmp_oborots_zad_hz.PosRampPlus2 = rampafloat_plus;
        edrk.zadanie.rmp_oborots_zad_hz.PosRampMinus2 = -rampafloat_minus;

        edrk.zadanie.rmp_oborots_zad_hz.NegRampPlus2 = rampafloat_minus;
        edrk.zadanie.rmp_oborots_zad_hz.NegRampMinus2 = -rampafloat_plus;

    }

}

void init_ramp_all_zadanie(void)
{
    _iq rampafloat, rampafloat_plus, rampafloat_minus;

//rmp_oborots_imitation
    edrk.zadanie.rmp_oborots_imitation.RampLowLimit = _IQ(MIN_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR);
    edrk.zadanie.rmp_oborots_imitation.RampHighLimit = _IQ(MAX_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR);
    rampafloat = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_IMITATION_OBOROTS_ROTOR));
    edrk.zadanie.rmp_oborots_imitation.RampPlus = rampafloat;
    edrk.zadanie.rmp_oborots_imitation.RampMinus = -rampafloat;

    edrk.zadanie.rmp_oborots_imitation.Out = 0;
    edrk.zadanie.rmp_oborots_imitation.DesiredInput = 0;

// rmp_fzad
    edrk.zadanie.rmp_fzad.RampLowLimit = _IQ(-MAX_ZADANIE_F/NORMA_FROTOR); //0
    edrk.zadanie.rmp_fzad.RampHighLimit = _IQ(MAX_ZADANIE_F/NORMA_FROTOR);

//    rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_F));
    rampafloat = _IQ((MAX_ZADANIE_F/NORMA_FROTOR)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_F));
    edrk.zadanie.rmp_fzad.RampPlus = rampafloat;
    edrk.zadanie.rmp_fzad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_fzad.DesiredInput = 0;
    edrk.zadanie.rmp_fzad.Out = 0;

// rmp_oborots_hz
    edrk.zadanie.rmp_oborots_zad_hz.RampLowLimit = _IQ(MIN_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); //0
    edrk.zadanie.rmp_oborots_zad_hz.RampHighLimit = _IQ(MAX_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR);

    edrk.zadanie.rmp_oborots_zad_hz.RampLowLimit1 = _IQ(MIN_1_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR); //0
    edrk.zadanie.rmp_oborots_zad_hz.RampHighLimit1 = _IQ(MAX_1_ZADANIE_OBOROTS_ROTOR/60.0/NORMA_FROTOR);

//    rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_OBOROTS_ROTOR));
//    rampafloat = _IQ((MAX_ZADANIE_OBOROTS_ROTOR/NORMA_FROTOR/60.0)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_OBOROTS_ROTOR));
//    edrk.zadanie.rmp_oborots_zad_hz.RampPlus = rampafloat;
//    edrk.zadanie.rmp_oborots_zad_hz.RampMinus = -rampafloat;

    change_ramp_zadanie();

    edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0;
    edrk.zadanie.rmp_oborots_zad_hz.Out = 0;


//
    edrk.zadanie.rmp_Izad.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_Izad.RampHighLimit = _IQ(MAX_ZADANIE_I_M/NORMA_ACP);

//    rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_I_M));
    rampafloat = _IQ((MAX_ZADANIE_I_M/NORMA_ACP)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_I_M));
    edrk.zadanie.rmp_Izad.RampPlus = rampafloat;
    edrk.zadanie.rmp_Izad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_Izad.DesiredInput = 0;
    edrk.zadanie.rmp_Izad.Out = 0;

//
    edrk.zadanie.rmp_ZadanieU_Charge.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_ZadanieU_Charge.RampHighLimit = _IQ(MAX_ZADANIE_U_CHARGE/NORMA_ACP);

//    rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_U_CHARGE));
    rampafloat = _IQ((MAX_ZADANIE_U_CHARGE/NORMA_ACP)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_U_CHARGE));
    edrk.zadanie.rmp_ZadanieU_Charge.RampPlus = rampafloat;
    edrk.zadanie.rmp_ZadanieU_Charge.RampMinus = -rampafloat;

    edrk.zadanie.rmp_ZadanieU_Charge.DesiredInput = _IQ(NOMINAL_U_ZARYAD/NORMA_ACP);
    edrk.zadanie.rmp_ZadanieU_Charge.Out = _IQ(NOMINAL_U_ZARYAD/NORMA_ACP);



//
    edrk.zadanie.rmp_k_u_disbalance.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_k_u_disbalance.RampHighLimit = _IQ(MAX_ZADANIE_K_U_DISBALANCE);

//    rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_K_U_DISBALANCE));
    rampafloat = _IQ((MAX_ZADANIE_K_U_DISBALANCE)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_K_U_DISBALANCE));
    edrk.zadanie.rmp_k_u_disbalance.RampPlus = rampafloat;
    edrk.zadanie.rmp_k_u_disbalance.RampMinus = -rampafloat;

    edrk.zadanie.rmp_k_u_disbalance.DesiredInput = 0;
    edrk.zadanie.rmp_k_u_disbalance.Out = 0;


//
    edrk.zadanie.rmp_kplus_u_disbalance.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_kplus_u_disbalance.RampHighLimit = _IQ(MAX_ZADANIE_KPLUS_U_DISBALANCE);

//    rampafloat = _IQ(1.0/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_KPLUS_U_DISBALANCE));
    rampafloat = _IQ((MAX_ZADANIE_KPLUS_U_DISBALANCE)/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_KPLUS_U_DISBALANCE));
    edrk.zadanie.rmp_kplus_u_disbalance.RampPlus = rampafloat;
    edrk.zadanie.rmp_kplus_u_disbalance.RampMinus = -rampafloat;

    edrk.zadanie.rmp_kplus_u_disbalance.DesiredInput = 0;
    edrk.zadanie.rmp_kplus_u_disbalance.Out = 0;



//
    edrk.zadanie.rmp_kzad.RampLowLimit = _IQ(0); //0
    edrk.zadanie.rmp_kzad.RampHighLimit = _IQ(MAX_ZADANIE_K_M);

//    rampafloat = _IQ(1.0/(2.0*FREQ_PWM*T_NARAST_ZADANIE_K_M));
    rampafloat = _IQ((MAX_ZADANIE_K_M)/(2.0*FREQ_PWM*T_NARAST_ZADANIE_K_M));
    edrk.zadanie.rmp_kzad.RampPlus = rampafloat;
    edrk.zadanie.rmp_kzad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_kzad.DesiredInput = 0;
    edrk.zadanie.rmp_kzad.Out = 0;


//
    edrk.zadanie.rmp_powers_zad.RampLowLimit = _IQ(MIN_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ)); //0
    edrk.zadanie.rmp_powers_zad.RampHighLimit = _IQ(MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ));

    edrk.zadanie.rmp_powers_zad.RampLowLimit1 = _IQ(MIN_1_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ));
    edrk.zadanie.rmp_powers_zad.RampHighLimit1 = _IQ(MAX_1_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ));

//    rampafloat = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_POWER));
//    edrk.zadanie.rmp_powers_zad.RampPlus = rampafloat;
//    edrk.zadanie.rmp_powers_zad.RampMinus = -rampafloat;


////
    rampafloat_plus  = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_POWER_PLUS));
    rampafloat_minus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_POWER_MINUS));

    edrk.zadanie.rmp_powers_zad.PosRampPlus1 = rampafloat_plus;
    edrk.zadanie.rmp_powers_zad.PosRampMinus1 = -rampafloat_minus;

    edrk.zadanie.rmp_powers_zad.NegRampPlus1 = rampafloat_minus;
    edrk.zadanie.rmp_powers_zad.NegRampMinus1 = -rampafloat_plus;

    rampafloat_plus  = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_PLUS));
    rampafloat_minus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_MINUS));

    edrk.zadanie.rmp_powers_zad.PosRampPlus2 = rampafloat_plus;
    edrk.zadanie.rmp_powers_zad.PosRampMinus2 = -rampafloat_minus;

    edrk.zadanie.rmp_powers_zad.NegRampPlus2 = rampafloat_minus;
    edrk.zadanie.rmp_powers_zad.NegRampMinus2 = -rampafloat_plus;

////

    edrk.zadanie.rmp_powers_zad.DesiredInput = 0;
    edrk.zadanie.rmp_powers_zad.Out = 0;

//

    edrk.zadanie.rmp_limit_powers_zad.RampLowLimit = 0;//_IQ(0); //0
    edrk.zadanie.rmp_limit_powers_zad.RampHighLimit = _IQ(SUPER_MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ));

    edrk.zadanie.rmp_limit_powers_zad.RampLowLimit1 = 0;
    edrk.zadanie.rmp_limit_powers_zad.RampHighLimit1 = _IQ(MAX_1_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ));

    rampafloat_plus  = _IQ((SUPER_MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_LIMIT_POWER_PLUS));
    rampafloat_minus = _IQ((SUPER_MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T1_NARAST_ZADANIE_LIMIT_POWER_MINUS));

    edrk.zadanie.rmp_limit_powers_zad.PosRampPlus1 = rampafloat_plus;
    edrk.zadanie.rmp_limit_powers_zad.PosRampMinus1 = -rampafloat_minus;

    edrk.zadanie.rmp_limit_powers_zad.NegRampPlus1 = rampafloat_minus;
    edrk.zadanie.rmp_limit_powers_zad.NegRampMinus1 = -rampafloat_plus;

    rampafloat_plus  = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_PLUS));
    rampafloat_minus = _IQ((MAX_ZADANIE_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T2_NARAST_ZADANIE_POWER_MINUS));

    edrk.zadanie.rmp_limit_powers_zad.PosRampPlus2 = rampafloat_plus;
    edrk.zadanie.rmp_limit_powers_zad.PosRampMinus2 = -rampafloat_minus;

    edrk.zadanie.rmp_limit_powers_zad.NegRampPlus2 = rampafloat_minus;
    edrk.zadanie.rmp_limit_powers_zad.NegRampMinus2 = -rampafloat_plus;

//    rampafloat = _IQ((MAX_ZADANIE_LIMIT_POWER*1000.0/(NORMA_MZZ*NORMA_MZZ))/(FREQ_RUN_RAMP*T_NARAST_ZADANIE_LIMIT_POWER));

//    edrk.zadanie.rmp_limit_powers_zad.RampPlus = rampafloat;
//    edrk.zadanie.rmp_limit_powers_zad.RampMinus = -rampafloat;

    edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0;
    edrk.zadanie.rmp_limit_powers_zad.Out = 0;

//


}

//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
#pragma CODE_SECTION(ramp_all_zadanie,".fast_run");
void load_current_ramp_oborots_power(void)
{
    int mode=0;
    static int prev_mode = 0;

    if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_OBOROTS ||
            edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_OBOROTS)
        mode = 1;

    if (edrk.Mode_ScalarVectorUFConst == ALG_MODE_FOC_POWER ||
            edrk.Mode_ScalarVectorUFConst == ALG_MODE_SCALAR_POWER)
        mode = 2;

    if (mode==1 && prev_mode==2)
    {
        // была мощность, а стали обороты
        edrk.zadanie.rmp_oborots_zad_hz.Out = WRotor.iqWRotorSumFilter3;
        //edrk.zadanie.iq_oborots_zad_hz = WRotor.iqWRotorSumFilter3;
    }


    if (mode==2 && prev_mode==1)
    {
        // были обороты, а стала мощность
        edrk.zadanie.rmp_powers_zad.Out = edrk.iq_power_kw_one_filter_znak;//filter.PowerScalarFilter2;
    }

    prev_mode = mode;

}
//////////////////////////////////////////////////////////
#pragma CODE_SECTION(ramp_all_zadanie,".fast_run");
void ramp_all_zadanie(int flag_set_zero)
{

//    if (edrk.Status_Ready.bits.ImitationReady2)
    {
        edrk.zadanie.rmp_oborots_imitation.DesiredInput = edrk.zadanie.iq_oborots_zad_hz;
        edrk.zadanie.rmp_oborots_imitation.calc(&edrk.zadanie.rmp_oborots_imitation);
        edrk.zadanie.rmp_oborots_imitation_rmp = edrk.zadanie.rmp_oborots_imitation.Out;
    }
    //////////////////////////////////////////////
    if (flag_set_zero==0)
      edrk.zadanie.rmp_Izad.DesiredInput = edrk.zadanie.iq_Izad;
    else
    if (flag_set_zero==2)
    {
        edrk.zadanie.rmp_Izad.DesiredInput = 0;
        edrk.zadanie.rmp_Izad.Out = 0;
    }
    else
      edrk.zadanie.rmp_Izad.DesiredInput = 0;

    edrk.zadanie.rmp_Izad.calc(&edrk.zadanie.rmp_Izad);
    edrk.zadanie.iq_Izad_rmp = edrk.zadanie.rmp_Izad.Out;
    //////////////////////////////////////////////
    edrk.zadanie.rmp_ZadanieU_Charge.DesiredInput = edrk.zadanie.iq_ZadanieU_Charge;
    edrk.zadanie.rmp_ZadanieU_Charge.calc(&edrk.zadanie.rmp_ZadanieU_Charge);
    edrk.zadanie.iq_ZadanieU_Charge_rmp = edrk.zadanie.rmp_ZadanieU_Charge.Out;
    //////////////////////////////////////////////
    if (flag_set_zero==0)
        edrk.zadanie.rmp_fzad.DesiredInput = edrk.zadanie.iq_fzad;
    else
    if (flag_set_zero==2)
    {
        edrk.zadanie.rmp_fzad.DesiredInput = 0;
        edrk.zadanie.rmp_fzad.Out = 0;
    }
    else
        edrk.zadanie.rmp_fzad.DesiredInput = 0;

    edrk.zadanie.rmp_fzad.calc(&edrk.zadanie.rmp_fzad);
    edrk.zadanie.iq_fzad_rmp = edrk.zadanie.rmp_fzad.Out;
    //////////////////////////////////////////////
    edrk.zadanie.rmp_k_u_disbalance.DesiredInput = edrk.zadanie.iq_k_u_disbalance;
    edrk.zadanie.rmp_k_u_disbalance.calc(&edrk.zadanie.rmp_k_u_disbalance);
    edrk.zadanie.iq_k_u_disbalance_rmp = edrk.zadanie.rmp_k_u_disbalance.Out;
    //////////////////////////////////////////////
    edrk.zadanie.rmp_kplus_u_disbalance.DesiredInput = edrk.zadanie.iq_kplus_u_disbalance;
    edrk.zadanie.rmp_kplus_u_disbalance.calc(&edrk.zadanie.rmp_kplus_u_disbalance);
    edrk.zadanie.iq_kplus_u_disbalance_rmp = edrk.zadanie.rmp_kplus_u_disbalance.Out;
    //////////////////////////////////////////////
    if (flag_set_zero==0)
        edrk.zadanie.rmp_kzad.DesiredInput = edrk.zadanie.iq_kzad;
    else
    if (flag_set_zero==2)
    {
        edrk.zadanie.rmp_kzad.DesiredInput = 0;
        edrk.zadanie.rmp_kzad.Out = 0;
    }
    else
        edrk.zadanie.rmp_kzad.DesiredInput = 0;
    edrk.zadanie.rmp_kzad.calc(&edrk.zadanie.rmp_kzad);
    edrk.zadanie.iq_kzad_rmp = edrk.zadanie.rmp_kzad.Out;
    //////////////////////////////////////////////
    if (flag_set_zero==0)
        edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = edrk.zadanie.iq_oborots_zad_hz;
    else
    if (flag_set_zero==2)
    {
        edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0;
        edrk.zadanie.rmp_oborots_zad_hz.Out = 0;
    }
    else
        edrk.zadanie.rmp_oborots_zad_hz.DesiredInput = 0;

    edrk.zadanie.rmp_oborots_zad_hz.calc(&edrk.zadanie.rmp_oborots_zad_hz);
    edrk.zadanie.iq_oborots_zad_hz_rmp = edrk.zadanie.rmp_oborots_zad_hz.Out;

    //////////////////////////////////////////////
//    if (flag_set_zero==0)
//        edrk.zadanie.rmp_limit_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad;
//    else
//    if (flag_set_zero==2)
//    {
//        edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0;
//        edrk.zadanie.rmp_limit_powers_zad.Out = 0;
//    }
//    else
//        edrk.zadanie.rmp_limit_powers_zad.DesiredInput = 0;

    edrk.zadanie.rmp_limit_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad;
    edrk.zadanie.rmp_limit_powers_zad.calc(&edrk.zadanie.rmp_limit_powers_zad);
    edrk.zadanie.iq_limit_power_zad_rmp = edrk.zadanie.rmp_limit_powers_zad.Out;



    //////////////////////////////////////////////
    if (flag_set_zero==0)
    {
        if (edrk.zadanie.iq_power_zad>=0)
        {
        if (edrk.zadanie.iq_power_zad>edrk.zadanie.iq_limit_power_zad_rmp)
            edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_limit_power_zad_rmp;
        else
            edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_power_zad;
    }
    else
        {
            if (edrk.zadanie.iq_power_zad<-edrk.zadanie.iq_limit_power_zad_rmp)
                edrk.zadanie.rmp_powers_zad.DesiredInput = -edrk.zadanie.iq_limit_power_zad_rmp;
            else
                edrk.zadanie.rmp_powers_zad.DesiredInput = edrk.zadanie.iq_power_zad;
        }

    }
    else
    if (flag_set_zero==2)
    {
        edrk.zadanie.rmp_powers_zad.DesiredInput = 0;
        edrk.zadanie.rmp_powers_zad.Out = 0;
    }
    else
        edrk.zadanie.rmp_powers_zad.DesiredInput = 0;

    edrk.zadanie.rmp_powers_zad.calc(&edrk.zadanie.rmp_powers_zad);
    edrk.zadanie.iq_power_zad_rmp = edrk.zadanie.rmp_powers_zad.Out;

}

//////////////////////////////////////////////////////////