#include "ACMSim.h"
#if MACHINE_TYPE == 1 || MACHINE_TYPE == 11
#define IM ACM // 权宜之计

REAL sat_kappa(REAL x){
    if(x>marino.kappa){
        return marino.kappa;
    }else if(x<-marino.kappa){
        return -marino.kappa;
    }else{
        return x;
    }
}
REAL deriv_sat_kappa(REAL x){
    if(x>marino.kappa){
        return 0;
    }else if(x<-marino.kappa){
        return -0;
    }else{
        return 1;
    }
}
void controller_marino2005(){

    // Cascaded from other system
    /* VM based Flux Est. */
    // 如果观测到的磁链作帕克变换的时候不是用的marino.xRho的话，那么交轴磁链永远为零，转速观测校正项和xTL自适应律增益都将无效。
    // Marino05需要的磁链观测是需要变换到xRho所定义的同步系下才行的！
    // Marino05需要的磁链观测是需要变换到xRho所定义的同步系下才行的！
    // Marino05需要的磁链观测是需要变换到xRho所定义的同步系下才行的！
    // Akatsu_RrId();
    simulation_only_flux_estimator();

    VM_Saturated_ExactOffsetCompensation_WithAdaptiveLimit();

    VM_Ohtani1990();
    VM_HoltzQuan2002();
    VM_LascuAndreescus2006();
    VM_HoltzQuan2003();
    VM_HuWu1998();
    VM_Stojic2015();
    VM_Harnefors2003_SCVM();
    stableFME();


    // flux feedback
    // simulation only flux
        marino.psi_Dmu = holtz.psi_D2_ode1;
        marino.psi_Qmu = holtz.psi_Q2_ode1;
        // marino.psi_Dmu = holtz.psi_D2_ode1_v2;
        // marino.psi_Qmu = holtz.psi_Q2_ode1_v2;
    // ohtani
        // marino.psi_Dmu = ohtani.psi_DQ2[0];
        // marino.psi_Qmu = ohtani.psi_DQ2[1];
        // marino.psi_Dmu = AB2M(ohtani.psi_2[0], ohtani.psi_2[1], CTRL.S->cosT, CTRL.S->sinT);
        // marino.psi_Qmu = AB2T(ohtani.psi_2[0], ohtani.psi_2[1], CTRL.S->cosT, CTRL.S->sinT);
        if(CTRL.timebase>0){
            // #define COMPENSATION 0
            // marino.psi_Dmu = AB2M(ohtani.psi_2[0]-COMPENSATION*ohtani.filtered_compensation[0], ohtani.psi_2[1]-COMPENSATION*ohtani.filtered_compensation[1], CTRL.S->cosT, CTRL.S->sinT);
            // marino.psi_Qmu = AB2T(ohtani.psi_2[0]-COMPENSATION*ohtani.filtered_compensation[0], ohtani.psi_2[1]-COMPENSATION*ohtani.filtered_compensation[1], CTRL.S->cosT, CTRL.S->sinT);

            marino.psi_Dmu = AB2M(ohtani.psi_2_real_output[0], ohtani.psi_2_real_output[1], CTRL.S->cosT, CTRL.S->sinT);
            marino.psi_Qmu = AB2T(ohtani.psi_2_real_output[0], ohtani.psi_2_real_output[1], CTRL.S->cosT, CTRL.S->sinT);
        }
    // harnefors
        // marino.psi_Dmu = AB2M(harnefors.psi_2[0], harnefors.psi_2[1], CTRL.S->cosT, CTRL.S->sinT);
        // marino.psi_Qmu = AB2T(harnefors.psi_2[0], harnefors.psi_2[1], CTRL.S->cosT, CTRL.S->sinT);







    // flux error quantities should be updated when feedback is updated | verified: this has nothing to do with the biased xTL at high speeds
    marino.e_psi_Dmu = marino.psi_Dmu - CTRL.I->cmd_psi;
    marino.e_psi_Qmu = marino.psi_Qmu - 0.0;

    // API to the fourth-order system of observer and identifiers
    observer_marino2005();
    CTRL.S->theta_D_elec = marino.xRho;
    CTRL.I->omg_elec = marino.xOmg;
    CTRL.motor->alpha   = marino.xAlpha;
    CTRL.I->TLoad   = marino.xTL;

    // 磁场可测 debug
    // marino.psi_Dmu = holtz.psi_D2;
    // marino.psi_Qmu = holtz.psi_Q2;
    // CTRL.S->theta_D_elec = ACM.theta_M;
    // CTRL.I->omg_elec = CTRL.I->omg_elec;
    // CTRL.motor->alpha   = ACM.alpha;
    // CTRL.I->TLoad   = ACM.TLoad;

    // flux error quantities (moved up)
    // marino.e_psi_Dmu = marino.psi_Dmu - CTRL.I->cmd_psi;
    // marino.e_psi_Qmu = marino.psi_Qmu - 0.0;

    CTRL.motor->alpha_inv = 1.0/CTRL.motor->alpha;

    // αβ to DQ
    CTRL.S->cosT = cos(CTRL.S->theta_D_elec);
    CTRL.S->sinT = sin(CTRL.S->theta_D_elec);
    CTRL.I->iDQ[0] = AB2M(IS_C(0), IS_C(1), CTRL.S->cosT, CTRL.S->sinT);
    CTRL.I->iDQ[1] = AB2T(IS_C(0), IS_C(1), CTRL.S->cosT, CTRL.S->sinT);

    // TODO
    // 当磁链幅值给定平稳时，这项就是零。
    marino.deriv_iD_cmd = 1.0*CTRL.motor->Lmu_inv*(  CTRL.I->cmd_deriv_psi \
                                            + CTRL.I->cmd_dderiv_psi*CTRL.motor->alpha_inv \
                                            - CTRL.I->cmd_deriv_psi*CTRL.motor->alpha_inv*CTRL.motor->alpha_inv*marino.deriv_xAlpha);
    // TODO 重新写！
    // REAL mu_temp     = CTRL.motor->npp_inv*CTRL.motor->Js * CLARKE_TRANS_TORQUE_GAIN_INVERSE*CTRL.motor->npp_inv;
    // REAL mu_temp_inv = CTRL.motor->npp*CTRL.motor->Js_inv * CLARKE_TRANS_TORQUE_GAIN*CTRL.motor->npp;
    // 第一项很有用，第二项无用。
    marino.deriv_iQ_cmd =   CTRL.motor->npp_inv*CTRL.motor->Js * CLARKE_TRANS_TORQUE_GAIN_INVERSE*CTRL.motor->npp_inv * (\
        1.0*(-marino.k_omega*deriv_sat_kappa(CTRL.I->omg_elec-CTRL.I->cmd_omg_elec) * (marino.deriv_xOmg - CTRL.I->cmd_deriv_omg_elec) + CTRL.motor->Js_inv*CTRL.motor->npp*marino.deriv_xTL + CTRL.I->cmd_dderiv_omg_elec ) * CTRL.I->cmd_psi_inv\
      - 1.0*(-marino.k_omega*      sat_kappa(CTRL.I->omg_elec-CTRL.I->cmd_omg_elec) + CTRL.motor->Js_inv*CTRL.motor->npp*CTRL.I->TLoad + CTRL.I->cmd_deriv_omg_elec) * (CTRL.I->cmd_deriv_psi * CTRL.I->cmd_psi_inv*CTRL.I->cmd_psi_inv)
        );

    // current error quantities
    CTRL.I->iDQ_cmd[0] = ( CTRL.I->cmd_psi + CTRL.I->cmd_deriv_psi*CTRL.motor->alpha_inv ) * CTRL.motor->Lmu_inv;
    CTRL.I->iDQ_cmd[1] = ( CTRL.motor->npp_inv*CTRL.motor->Js *( 1*CTRL.I->cmd_deriv_omg_elec - marino.k_omega*sat_kappa(CTRL.I->omg_elec-CTRL.I->cmd_omg_elec) ) + CTRL.I->TLoad ) * (CLARKE_TRANS_TORQUE_GAIN_INVERSE*CTRL.motor->npp_inv*CTRL.I->cmd_psi_inv);
    marino.e_iDs = CTRL.I->iDQ[0] - CTRL.I->iDQ_cmd[0];
    marino.e_iQs = CTRL.I->iDQ[1] - CTRL.I->iDQ_cmd[1];

    marino.torque_cmd = CLARKE_TRANS_TORQUE_GAIN * CTRL.motor->npp * (CTRL.I->iDQ_cmd[1] * CTRL.I->cmd_psi   - CTRL.I->iDQ_cmd[0]*(0));
    marino.torque__fb = CLARKE_TRANS_TORQUE_GAIN * CTRL.motor->npp * (CTRL.I->iDQ[1]     * marino.psi_Dmu - CTRL.I->iDQ[0] * marino.psi_Qmu);
    // marino.torque__fb = CLARKE_TRANS_TORQUE_GAIN * CTRL.motor->npp * (CTRL.I->iDQ[1]     * marino.psi_Dmu);


    // linear combination of error
    marino.zD = marino.e_iDs + CTRL.motor->Lsigma_inv*marino.e_psi_Dmu;
    marino.zQ = marino.e_iQs + CTRL.motor->Lsigma_inv*marino.e_psi_Qmu;

    // known signals to feedforward (to cancel)
    marino.Gamma_D = CTRL.motor->Lsigma_inv * (-CTRL.motor->rs*CTRL.I->iDQ[0] -CTRL.motor->alpha*CTRL.motor->Lmu*CTRL.I->iDQ_cmd[0] +CTRL.motor->alpha  *CTRL.I->cmd_psi +CTRL.S->omega_syn*marino.e_psi_Qmu) +CTRL.S->omega_syn*CTRL.I->iDQ[1] - marino.deriv_iD_cmd;
    marino.Gamma_Q = CTRL.motor->Lsigma_inv * (-CTRL.motor->rs*CTRL.I->iDQ[1] -CTRL.motor->alpha*CTRL.motor->Lmu*CTRL.I->iDQ_cmd[1] -CTRL.I->omg_elec*CTRL.I->cmd_psi -CTRL.S->omega_syn*marino.e_psi_Dmu) -CTRL.S->omega_syn*CTRL.I->iDQ[0] - marino.deriv_iQ_cmd;

    // voltage commands
    CTRL.O->uDQ_cmd[0] = CTRL.motor->Lsigma * (-(marino.kz+0.25*CTRL.motor->Lsigma*CTRL.motor->Lmu*marino.xAlpha_Max)*marino.zD - marino.Gamma_D);
    CTRL.O->uDQ_cmd[1] = CTRL.motor->Lsigma * (-(marino.kz+0.25*CTRL.motor->Lsigma*CTRL.motor->Lmu*marino.xAlpha_Max)*marino.zQ - marino.Gamma_Q);
    CTRL.O->uab_cmd[0] = MT2A(CTRL.O->uDQ_cmd[0], CTRL.O->uDQ_cmd[1], CTRL.S->cosT, CTRL.S->sinT);
    CTRL.O->uab_cmd[1] = MT2B(CTRL.O->uDQ_cmd[0], CTRL.O->uDQ_cmd[1], CTRL.S->cosT, CTRL.S->sinT);
}


void controller_IFOC();
void controller(){

    // 0. 参数时变
    // if (fabs(CTRL.timebase-1.25)<CL_TS){
    //     ACM.alpha = 1.5*IM_ROTOR_RESISTANCE / IM_MAGNETIZING_INDUCTANCE;
    //     ACM.rreq = ACM.alpha*ACM.Lmu;
    //     ACM.rr   = ACM.alpha*(ACM.Lm+ACM.Llr);
    // }


    // 1. 生成转速指令
    static REAL rpm_speed_command=0.0, amp_current_command=0.0;
    {
        // commands(&rpm_speed_command, &amp_current_command);
        static REAL local_dc_rpm_cmd = 0.0; 
        static REAL local_dc_rpm_cmd_deriv = 0.0; 

        static REAL last_omg_cmd=0.0;
        REAL OMG1;
        #define DELAY 100
        #define BIAS 100
        if(CTRL.timebase>8){
            if      (CTRL.timebase>3.875){
                OMG1 = (2*M_PI*32);
            }else if(CTRL.timebase>3.75){
                OMG1 = (2*M_PI*16);
            }else if(CTRL.timebase>3.5){
                OMG1 = (2*M_PI*8);
            }else{
                OMG1 = (2*M_PI*4);
            }
            OMG1 = (2*M_PI*4);
            // OMG1 = 0;

            // 反转
            if(CTRL.timebase>5 && CTRL.timebase<40){
                // OMG1 = 0;
                // TODO: test positive and negative high speed
                local_dc_rpm_cmd_deriv = -200;
                local_dc_rpm_cmd      += CL_TS * local_dc_rpm_cmd_deriv;
            }else{
                local_dc_rpm_cmd_deriv = 0.0;
            }

            rpm_speed_command           = (100          * sin(OMG1*CTRL.timebase) + local_dc_rpm_cmd       );
            CTRL.I->cmd_omg_elec        = (100          * sin(OMG1*CTRL.timebase) + local_dc_rpm_cmd       )*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = (100*OMG1     * cos(OMG1*CTRL.timebase) + local_dc_rpm_cmd_deriv )*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_dderiv_omg_elec = (100*OMG1*OMG1*-sin(OMG1*CTRL.timebase) + 0                      )*RPM_2_ELEC_RAD_PER_SEC;
        // }else if(CTRL.timebase>2*DELAY){
        }else if(CTRL.timebase>5+BIAS){
            rpm_speed_command           = local_dc_rpm_cmd;
            CTRL.I->cmd_omg_elec        = rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = 0;
            CTRL.I->cmd_dderiv_omg_elec = 0;
        }else if(CTRL.timebase>3+BIAS){
            rpm_speed_command += CL_TS*150;
            local_dc_rpm_cmd            = rpm_speed_command;
            CTRL.I->cmd_omg_elec        = rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = (CTRL.I->cmd_omg_elec - last_omg_cmd)*CL_TS_INVERSE;
            CTRL.I->cmd_dderiv_omg_elec = 0;

            last_omg_cmd = CTRL.I->cmd_omg_elec;
        }else if(CTRL.timebase>2+BIAS){
            rpm_speed_command -= CL_TS*150;
            local_dc_rpm_cmd            = rpm_speed_command;
            CTRL.I->cmd_omg_elec        = rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = (CTRL.I->cmd_omg_elec - last_omg_cmd)*CL_TS_INVERSE;
            CTRL.I->cmd_dderiv_omg_elec = 0;

            last_omg_cmd = CTRL.I->cmd_omg_elec;
        }else if(CTRL.timebase>1+BIAS){
            rpm_speed_command += CL_TS*150;
            local_dc_rpm_cmd            = rpm_speed_command;
            CTRL.I->cmd_omg_elec        = rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = (CTRL.I->cmd_omg_elec - last_omg_cmd)*CL_TS_INVERSE;
            CTRL.I->cmd_dderiv_omg_elec = 0;

            last_omg_cmd = CTRL.I->cmd_omg_elec;
        }else if(CTRL.timebase>1+0.0){
            rpm_speed_command           = local_dc_rpm_cmd;
            CTRL.I->cmd_omg_elec        = rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = 0;
            CTRL.I->cmd_dderiv_omg_elec = 0;
        }else if(CTRL.timebase>0.5+0.0){
            rpm_speed_command           += CL_TS*500;
            local_dc_rpm_cmd            = rpm_speed_command;
            CTRL.I->cmd_omg_elec        = rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = (CTRL.I->cmd_omg_elec - last_omg_cmd)*CL_TS_INVERSE;
            CTRL.I->cmd_dderiv_omg_elec = 0;
            last_omg_cmd = CTRL.I->cmd_omg_elec;
        }else{
            rpm_speed_command           = 0; // 0*20                   * sin(2*M_PI*CTRL.timebase);
            CTRL.I->cmd_omg_elec        = 0; // 0*20                   * sin(2*M_PI*CTRL.timebase)*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_deriv_omg_elec  = 0; // 0*20*(2*M_PI)          * cos(2*M_PI*CTRL.timebase)*RPM_2_ELEC_RAD_PER_SEC;
            CTRL.I->cmd_dderiv_omg_elec = 0; // 0*20*(2*M_PI)*(2*M_PI) *-sin(2*M_PI*CTRL.timebase)*RPM_2_ELEC_RAD_PER_SEC;
        }
    }


    // 2. 生成磁链指令
    #define TIME_COST 0.1
    if(CTRL.timebase<TIME_COST){
        CTRL.I->cmd_psi_raw   += CL_TS*CTRL.I->m0/TIME_COST;
        CTRL.I->cmd_psi        = CTRL.I->cmd_psi_raw;
        CTRL.I->cmd_deriv_psi  = CTRL.I->m0/TIME_COST;
        CTRL.I->cmd_dderiv_psi = 0.0;
    }else{
        // CTRL.I->m1 = 0.0;
        CTRL.I->cmd_psi_raw    = CTRL.I->m0 + CTRL.I->m1 * sin(CTRL.I->omega1*CTRL.timebase);
        CTRL.I->cmd_psi        = CTRL.I->cmd_psi_raw; // _lpf(CTRL.I->cmd_psi_raw, CTRL.I->cmd_psi, 5);
        CTRL.I->cmd_deriv_psi  = CTRL.I->m1 * CTRL.I->omega1 * cos(CTRL.I->omega1*CTRL.timebase);
        CTRL.I->cmd_dderiv_psi = CTRL.I->m1 * CTRL.I->omega1 * CTRL.I->omega1 * -sin(CTRL.I->omega1*CTRL.timebase);
    }
    CTRL.I->cmd_psi_inv = 1.0 / CTRL.I->cmd_psi;
    CTRL.I->cmd_psi_ABmu[0] = MT2A(CTRL.I->cmd_psi, 0.0, CTRL.S->cosT, CTRL.S->sinT); // TODO 这里的cosT和sinT还没更新到当前步，有没有关系？
    CTRL.I->cmd_psi_ABmu[1] = MT2B(CTRL.I->cmd_psi, 0.0, CTRL.S->cosT, CTRL.S->sinT); // TODO 这里的cosT和sinT还没更新到当前步，有没有关系？


    #if CONTROL_STRATEGY == INDIRECT_FOC
        controller_IFOC();
    #elif CONTROL_STRATEGY == MARINO_2005_ADAPTIVE_SENSORLESS_CONTROL
        controller_marino2005();
    #endif

    // debug marino05 observer---not work as expected. For Marino05 you cannot tune observer and controller separately. They are just coupled together.
    // controller_IFOC();

    #if PC_SIMULATION == TRUE
    // for plot
    ACM.rpm_cmd = rpm_speed_command;
    // CTRL.speed_ctrl_err = (CTRL.I->omg_elec - CTRL.I->cmd_omg_elec)*ELEC_RAD_PER_SEC_2_RPM;
    #endif
}



void controller_IFOC(){

    // 3. 电气转子位置和电气转子转速反馈
        //（编码器反馈）
        // CTRL.I->omg_elec     = qep.omg_elec;
        // CTRL.S->theta_D_elec__fb = qep.theta_d;

    //（实际反馈，实验中不可能）
    // CTRL.I->omg_elec     = im.omg_elec;

        //（无感）
        // harnefors_scvm();
        // CTRL.I->omg_elec     = omg_harnefors;
        // CTRL.S->theta_D_elec__fb = theta_d_harnefors;

    // 4. 帕克变换
    #define THE_FIELD_IS_KNOWN FALSE
    #if THE_FIELD_IS_KNOWN
        CTRL.S->theta_D_elec = atan2(IM.x[3], IM.x[2]); 
        CTRL.S->cosT = cos(CTRL.S->theta_D_elec); 
        CTRL.S->sinT = sin(CTRL.S->theta_D_elec);
    #else
        // 间接磁场定向第一部分
        CTRL.S->theta_D_elec += CL_TS * CTRL.S->omega_syn;
        CTRL.S->cosT = cos(CTRL.S->theta_D_elec); 
        CTRL.S->sinT = sin(CTRL.S->theta_D_elec);
        if(CTRL.S->theta_D_elec > M_PI){
            CTRL.S->theta_D_elec -= 2*M_PI;        
        }else if(CTRL.S->theta_D_elec < -M_PI){
            CTRL.S->theta_D_elec += 2*M_PI; // 反转！
        }
    #endif
    CTRL.I->iDQ[0] = AB2M(IS_C(0), IS_C(1), CTRL.S->cosT, CTRL.S->sinT);
    CTRL.I->iDQ[1] = AB2T(IS_C(0), IS_C(1), CTRL.S->cosT, CTRL.S->sinT);
    pid1_iM.Fbk = CTRL.I->iDQ[0];
    pid1_iT.Fbk = CTRL.I->iDQ[1];


    // 5. 转速环
    static int vc_count = 0;
    if(vc_count++ == SPEED_LOOP_CEILING){
        vc_count = 0;

        pid1_spd.Ref = CTRL.I->cmd_omg_elec; //rpm_speed_command*RPM_2_ELEC_RAD_PER_SEC;
        pid1_spd.Fbk = CTRL.I->omg_elec;
        pid1_spd.calc(&pid1_spd);
        pid1_iT.Ref = pid1_spd.Out;
        CTRL.I->iDQ_cmd[1] = pid1_iT.Ref;
    }
    // 磁链环
    // if(ob.taao_flux_cmd_on){
    //     CTRL.I->iDQ_cmd[0] = IM_FLUX_COMMAND_DC_PART * CTRL.motor->Lmu_inv   \
    //                    + (   M1*OMG1*cos(OMG1*ob.timebase)  )/ CTRL.motor->rreq; ///////////////////////////////// 
    // }else{
        // CTRL.I->iDQ_cmd[0] =  * CTRL.motor->Lmu_inv + (deriv_fluxModCmd)/ CTRL.motor->rreq; 
        CTRL.I->cmd_psi = IM_FLUX_COMMAND_DC_PART;
        CTRL.I->iDQ_cmd[0] = CTRL.I->cmd_psi * CTRL.motor->Lmu_inv;
    // }
    pid1_iM.Ref = CTRL.I->iDQ_cmd[0];
    // 计算转矩
    CTRL.I->Tem_cmd = CLARKE_TRANS_TORQUE_GAIN * CTRL.motor->npp * CTRL.I->iDQ_cmd[1] * (CTRL.I->cmd_psi);
    // 间接磁场定向第二部分
    CTRL.S->omega_sl  = CTRL.motor->rreq*CTRL.I->iDQ_cmd[1]/(CTRL.I->cmd_psi);
    CTRL.S->omega_syn = CTRL.I->omg_elec + CTRL.S->omega_sl;

    // 扫频将覆盖上面产生的励磁、转矩电流指令
    #if EXCITATION_TYPE == EXCITATION_SWEEP_FREQUENCY
        #if SWEEP_FREQ_C2V == TRUE
            pid1_iT.Ref = amp_current_command; 
        #endif
        #if SWEEP_FREQ_C2C == TRUE
            pid1_iT.Ref = 0.0;
            pid1_iM.Ref = amp_current_command;
        #else
            pid1_iM.Ref = 0.0;
        #endif
    #endif



    // 6. 电流环
    REAL decoupled_M_axis_voltage=0.0, decoupled_T_axis_voltage=0.0;
    pid1_iM.calc(&pid1_iM);
    pid1_iT.calc(&pid1_iT);
    {   // Steady state dynamics based decoupling circuits for current regulation
        #if VOLTAGE_CURRENT_DECOUPLING_CIRCUIT == TRUE
            // decoupled_M_axis_voltage = vM + (CTRL.motor->rs+CTRL.motor->rreq)*CTRL.iMs + CTRL.motor->Lsigma*(-CTRL.S->omega_syn*CTRL.iTs) - CTRL.motor->alpha*CTRL.psimod_fb; // Jadot09
            // decoupled_T_axis_voltage = vT + (CTRL.motor->rs+CTRL.motor->rreq)*CTRL.iTs + CTRL.motor->Lsigma*( CTRL.S->omega_syn*CTRL.iMs) + CTRL.omg_fb*CTRL.psimod_fb;
            // decoupled_T_axis_voltage = vT + CTRL.S->omega_syn*(CTRL.motor->Lsigma+CTRL.motor->Lmu)*CTRL.iMs; // 这个就不行，说明：CTRL.motor->Lmu*iMs != ob.taao_flux_cmd，而是会因iMs的波动在T轴控制上引入波动和不稳定

            decoupled_M_axis_voltage = pid1_iM.Out + (CTRL.motor->Lsigma) * (-CTRL.S->omega_syn*CTRL.I->iDQ[1]); // Telford03/04
            decoupled_T_axis_voltage = pid1_iT.Out + CTRL.S->omega_syn*(CTRL.I->cmd_psi + CTRL.motor->Lsigma*CTRL.I->iDQ[0]); // 这个行，但是无速度运行时，会导致M轴电流在转速暂态高频震荡。
            // decoupled_T_axis_voltage = pid1_iT.Out; // 无感用这个
        #else
            decoupled_M_axis_voltage = pid1_iM.Out;
            decoupled_T_axis_voltage = pid1_iT.Out;
        #endif
    }

    // 7. 反帕克变换
    CTRL.O->uab_cmd[0] = MT2A(decoupled_M_axis_voltage, decoupled_T_axis_voltage, CTRL.S->cosT, CTRL.S->sinT);
    CTRL.O->uab_cmd[1] = MT2B(decoupled_M_axis_voltage, decoupled_T_axis_voltage, CTRL.S->cosT, CTRL.S->sinT);
}



// 初始化函数
void experiment_init(){
    CTRL_init();    // 控制器结构体初始化
    rk4_init();     // 龙格库塔法结构体初始化
    observer_init();
}
void CTRL_init(){

    allocate_CTRL(&CTRL);

    #define LOCAL_SCALE 1.0
    marino.kz         = LOCAL_SCALE * 2*700.0; // zd, zq
    marino.k_omega    = LOCAL_SCALE * 0.5*88*60.0; // 6000  // e_omega // 增大这个可以消除稳态转速波形中的正弦扰动（源自q轴电流给定波形中的正弦扰动，注意实际的q轴电流里面是没有正弦扰动的）


    marino.kappa      = 1e4*24; // \in [0.1, 1e4*24] no difference // e_omega // 增大这个意义不大，转速控制误差基本上已经是零了，所以kappa取0.05和24没有啥区别。

    // lammbda_inv和gamma_inv是竞争的关系
    // marino.lambda_inv = 5* 0.1 * 1.5 * 6000.0;          // omega 磁链反馈为实际值时，这两个增益取再大都没有意义。
    // marino.gamma_inv  = 10 * 3e0 * 180/MOTOR_SHAFT_INERTIA; // TL    磁链反馈为实际值时，这两个增益取再大都没有意义。
    // marino.delta_inv  = 0*75.0; // alpha 要求磁链幅值时变

    marino.lambda_inv = LAMBDA_INV_xOmg;
    marino.gamma_inv  = GAMMA_INV_xTL;
    marino.delta_inv  = DELTA_INV_alpha;

    marino.xTL_Max = 8.0;
    marino.xAlpha_Max = 8.0;
    marino.xAlpha_min = 1.0;

    marino.xRho = 0.0;
    marino.xTL = 0.0;
    marino.xAlpha = IM_ROTOR_RESISTANCE/IM_MAGNETIZING_INDUCTANCE;
    marino.xOmg = 0.0;

    printf("alpha: %g in [%g, %g]?\n", marino.xAlpha, marino.xAlpha_min, marino.xAlpha_Max);

    marino.deriv_xTL = 0.0;
    marino.deriv_xAlpha = 0.0;
    marino.deriv_xOmg = 0.0;

    marino.psi_Dmu = 0.0;
    marino.psi_Qmu = 0.0;

    marino.zD = 0.0;
    marino.zQ = 0.0;
    marino.e_iDs = 0.0;
    marino.e_iQs = 0.0;
    marino.e_psi_Dmu = 0.0;
    marino.e_psi_Qmu = 0.0;

    marino.deriv_iD_cmd = 0.0;
    marino.deriv_iQ_cmd = 0.0;

    marino.Gamma_D = 0.0;
    marino.Gamma_Q = 0.0;

    marino.torque_cmd = 0.0;
    marino.torque__fb = 0.0;

    // struct Holtz2003
    holtz.psi_D2 = 0.0;
    holtz.psi_Q2 = 0.0;
    holtz.psi_D1_ode1 = 0.0;
    holtz.psi_Q1_ode1 = 0.0;
    holtz.psi_D2_ode1 = 0.0;
    holtz.psi_Q2_ode1 = 0.0;
    holtz.psi_D1_ode4 = 0.0;
    holtz.psi_Q1_ode4 = 0.0;
    holtz.psi_D2_ode4 = 0.0;
    holtz.psi_Q2_ode4 = 0.0;





    int i=0,j=0;

    CTRL.timebase = 0.0;

    /* Parameter (including speed) Adaptation */ 
        CTRL.motor->rs     = IM_STAOTR_RESISTANCE;
        CTRL.motor->rreq   = IM_ROTOR_RESISTANCE;

        CTRL.motor->npp    = MOTOR_NUMBER_OF_POLE_PAIRS;
        CTRL.motor->Lsigma = IM_TOTAL_LEAKAGE_INDUCTANCE;
        CTRL.motor->Lmu    = IM_MAGNETIZING_INDUCTANCE;
        CTRL.motor->Js     = MOTOR_SHAFT_INERTIA * (1.0+LOAD_INERTIA);

        CTRL.motor->alpha  = CTRL.motor->rreq/CTRL.motor->Lmu;
        CTRL.motor->alpha_inv = 1.0/CTRL.motor->alpha;

        CTRL.motor->npp_inv     = 1.0/CTRL.motor->npp;
        CTRL.motor->Lsigma_inv  = 1.0/CTRL.motor->Lsigma;
        CTRL.motor->Lmu_inv     = 1.0/CTRL.motor->Lmu;
        CTRL.motor->Js_inv      = 1.0/CTRL.motor->Js;

        CTRL.I->TLoad  = 0.0;

    CTRL.S->cosT = 1.0;
    CTRL.S->sinT = 0.0;

    CTRL.I->m0 = IM_FLUX_COMMAND_DC_PART;
    CTRL.I->m1 = IM_FLUX_COMMAND_SINE_PART;
    CTRL.I->omega1 = 2*M_PI*IM_FLUX_COMMAND_SINE_HERZ;

    CTRL.I->sensorless    = SENSORLESS_CONTROL;
    CTRL.I->ctrl_strategy = CONTROL_STRATEGY;

    #define AKATSU00 FALSE
    #if AKATSU00 == TRUE
    int ind;
    for(ind=0;ind<2;++ind){
    // for(i=0;i<2;++i){
        hav.emf_stator[ind] = 0;

        hav.psi_1[ind] = 0;
        hav.psi_2[ind] = 0;
        hav.psi_2_prev[ind] = 0;

        hav.psi_1_nonSat[ind] = 0;
        hav.psi_2_nonSat[ind] = 0;

        hav.psi_1_min[ind] = 0;
        hav.psi_1_max[ind] = 0;
        hav.psi_2_min[ind] = 0;
        hav.psi_2_max[ind] = 0;

        hav.rs_est = 3.04;
        hav.rreq_est = 1.6;

        hav.Delta_t = 1;
        hav.u_off[ind] = 0;
        hav.u_off_integral_input[ind] = 0;
        hav.gain_off = 0.025;

        hav.flag_pos2negLevelA[ind] = 0;
        hav.flag_pos2negLevelB[ind] = 0;
        hav.time_pos2neg[ind] = 0;
        hav.time_pos2neg_prev[ind] = 0;

        hav.flag_neg2posLevelA[ind] = 0;
        hav.flag_neg2posLevelB[ind] = 0;
        hav.time_neg2pos[ind] = 0;
        hav.time_neg2pos_prev[ind] = 0;    

        hav.sat_min_time[ind] = 0.0;
        hav.sat_max_time[ind] = 0.0;
    }

    a92v.awaya_lambda = 31.4*1;
    a92v.q0 = 0.0;
    a92v.q1_dot = 0.0;
    a92v.q1 = 0.0;
    a92v.tau_est = 0.0;
    a92v.sum_A = 0.0;
    a92v.sum_B = 0.0;
    a92v.est_Js_variation = 0.0;
    a92v.est_Js = 0.0;
    #endif



    // /*Jadot2009*/
    // CTRL.is_ref[0] = 0.0;
    // CTRL.is_ref[1] = 0.0;
    // CTRL.psi_ref[0] = 0.0;
    // CTRL.psi_ref[1] = 0.0;

    // CTRL.pi_vsJadot_0.Kp = 7; 
    // CTRL.pi_vsJadot_0.Ti = 1.0/790.0; 
    // CTRL.pi_vsJadot_0.Kp = 15; 
    // CTRL.pi_vsJadot_0.Ti = 0.075; 
    // CTRL.pi_vsJadot_0.Ki = CTRL.pi_vsJadot_0.Kp / CTRL.pi_vsJadot_0.Ti * TS;
    // CTRL.pi_vsJadot_0.i_state = 0.0;
    // CTRL.pi_vsJadot_0.i_limit = 300.0; // unit: Volt

    // CTRL.pi_vsJadot_1.Kp = 7; 
    // CTRL.pi_vsJadot_1.Ti = 1.0/790.0; 
    // CTRL.pi_vsJadot_1.Kp = 15; 
    // CTRL.pi_vsJadot_1.Ti = 0.075; 
    // CTRL.pi_vsJadot_1.Ki = CTRL.pi_vsJadot_1.Kp / CTRL.pi_vsJadot_1.Ti * TS;
    // CTRL.pi_vsJadot_1.i_state = 0.0;
    // CTRL.pi_vsJadot_1.i_limit = 300.0; // unit: Volt

    // PID调谐
    ACMSIMC_PIDTuner();
}

#endif
