/***********************************************************************
*  FILE        : motor_controller.c
***********************************************************************/
#include "config_parameter.h"
#include "r_smc_entry.h"
#include "motor_controller.h"
#include "driver.h"
#include "position_sensor.h"
#include <mathf.h>
#include <stdio.h>

/* 変数定義 */
float Edc = 0.0f;        /* 直流電圧検出値[V] */
float Edc_fil = 0.0f;    /* 直流電圧検出フィルタ値[V] */
float Edc_fil_gain = (T_PWM / EDC_FIL_TIME);    /* 直流電圧フィルタ演算係数 */
float Volume = 0.0f;        /* ボリューム信号[V] */
float Volume_fil = 0.0f;    /* ボリューム信号検出フィルタ値[V] */
float Volume_fil_gain = (T_PWM / VOLUME_FIL_TIME);      /* ボリューム信号フィルタ演算係数 */
float Volume2RpmGain = (TARGET_MAX_RPM - TARGET_MIN_RPM)/(AD_MAX_VOL - MOTOR_START_VOL);    /* ボリューム電圧―速度の傾き */

float TargetSpeed_rpm = 0.0f;       /* 速度目標値[rpm] */
float ReferenceSpeed_rpm = 0.0f;    /* 速度指令値[rpm] */
float ReferenceSpeed_hz = 0.0f;     /* 速度指令値[Hz] */
float Omega1Star = 0.0f;             /* 速度指令値[rad/s] */
float Omega1 = 0.0f;                 /* 速度検出値[rad/s] */

iuvw_struct Iuvw = {0.0f};   /* 相電流検出値 */
float Ia = 0.0f;  /*α軸電流*/
float Ib = 0.0f;  /*β軸電流*/
float Id = 0.0f;  /*d軸電流*/
float Iq = 0.0f;  /*q軸電流*/
float IdFil = 0.0f;  /*d軸電流フィルタ値 */
float IqFil = 0.0f;  /*q軸電流フィルタ値 */
float Idq_Fil_Gain = (T_PWM / IDQ_FIL_TIME);
float IdStar = ID_STAR;     /* d軸電流指令値 */
float IqStar = 0.0f;        /* q軸電流指令値 */

float VdStar = 0.0f;  /* d軸電圧指令値 */
float VqStar = 0.0f;  /* q軸電圧指令値 */
float Va = 0.0f;  /*α軸電圧*/
float Vb = 0.0f;  /*β軸電圧*/
float Vu = 0.0f;   /* u相電圧(HIP変調前) */
float Vv = 0.0f;   /* v相電圧(HIP変調前) */
float Vw = 0.0f;   /* w相電圧(HIP変調前) */
vuvw_struct VuvwStar = {0.0f};       /* 相電圧指令値(HIP変調済み) */

float ThetaD = 0.0f;    /* 回転子位相：θd[rad] */
float ThetaV = 0.0f;    /* 電圧位相：θv[rad] */

float IqStar_ASR = 0.0f;    /* ASR出力(q軸電流指令値) */
float AsrIsum = 0.0f;       /* ASR積分項 */
//float Kp_Asr = ASR_TORQUE_2_CURRENT * MOTOR_J / POLEPAIR * ASR_OMEGA_B;	/* 比例ゲイン(トルク変換式含む) */
//float Ki_Asr = ASR_TORQUE_2_CURRENT * MOTOR_J / POLEPAIR * (ASR_OMEGA_B * ASR_OMEGA_B) / (ASR_N * ASR_N) * T_PWM;	/* 積分ゲイン(トルク変換式含む) */
float Kp_Asr = MOTOR_J / POLEPAIR * ASR_OMEGA_B;	/* 比例ゲイン(トルク変換式含む) */
float Ki_Asr = MOTOR_J / POLEPAIR * (ASR_OMEGA_B * ASR_OMEGA_B) / (ASR_N * ASR_N) * T_PWM;	/* 積分ゲイン(トルク変換式含む) */
float DeltaVdStar = 0.0f;   /* d軸電圧(ACR出力) */
float DeltaVqStar = 0.0f;   /* q軸電圧(ACR出力) */
float VdStarDash = 0.0f;    /* d軸電圧(非干渉制御出力) */
float VqStarDash = 0.0f;    /* q軸電圧(非干渉制御出力) */
float AcrIdsum = 0.0f;
float AcrIqsum = 0.0f;
float Kp_Acr = MOTOR_LS * ACR_OMEGA_B;
float Ki_Acr = MOTOR_R * ACR_OMEGA_B * T_PWM;

int MainStatus;  /* メインステータス[0:STOP, 1:RUN, 2:ERR] */

/* 関数プロトタイプ宣言 */
static uint16_t check_edc_voltage_err(void);
static uint16_t check_sw_over_current_err(void);
static void state_machine(void);

/* 直流電圧異常チェック処理 */
static uint16_t check_edc_voltage_err(void)
{
    if((Edc_fil >= EDC_ERR_VOL_HI)
    || (Edc_fil <= EDC_ERR_VOL_LO))
    {
        return CHECK_NG;    /* 正常範囲外：check結果NGを返却 */
    }
    else
    {
        return CHECK_OK;    /* 正常範囲内：check結果OKを返却 */
    }
}

/* ソフトウェア検出過電流チェック処理 */
static uint16_t check_sw_over_current_err(void)
{
    if((Iuvw.Iu >= SW_OVER_CURRENT)
    || (Iuvw.Iv >= SW_OVER_CURRENT)
    || (Iuvw.Iw >= SW_OVER_CURRENT))
    {
        return CHECK_NG;    /* SW過電流発生：check結果NGを返却 */
    }
    else
    {
        return CHECK_OK;    /* SW過電流発生していない：check結果OKを返却 */
    }
}

/* モータ起動前に実施する処理 */
static void motor_pre_start_task(void)
{
    IdFil = 0.0f;
    IqFil = 0.0f;
    AsrIsum = 0.0f;
    AcrIdsum = 0.0f;
    AcrIqsum = 0.0f;
}

/* 状態遷移処理 */
static void state_machine(void)
{
    int sts_work;

    sts_work = MainStatus;  /* 現在の状態を保管 */

    /* 状態遷移判定 */
    switch (MainStatus)
    {
    case STOP_STS:   /* 現在、STOP状態 */
        /* 状態遷移判定 */
        if((check_edc_voltage_err() == CHECK_NG)
        || (check_sw_over_current_err() == CHECK_NG)
        || (check_hw_over_current_err() == CHECK_NG))
        {
            sts_work = ERR_STS; /* ERR状態へ */
        }
        else if((OffsetEndFlg == 1)                 /* オフセット完了フラグON */
             && (Volume_fil >= MOTOR_START_VOL))    /* ボリューム電圧≧起動電圧 */
        {
            sts_work = RUN_STS; /* RUN状態へ */
            motor_pre_start_task(); /* モータ起動前処理 */
            start_pwm_output();     /* PWM信号出力開始 */
        }
        else{/* 現在の状態を保持 */}
        break;

    case RUN_STS:   /* 現在、RUN状態 */
        /* 状態遷移判定 */
        if((check_edc_voltage_err() == CHECK_NG)
        || (check_hw_over_current_err() == CHECK_NG))
        // || (check_sw_over_current_err() == CHECK_NG))
        {
            stop_pwm_output();  /* PWM信号出力停止 */
            sts_work = ERR_STS; /* ERR状態へ遷移 */
        }
        else if(Volume_fil <= MOTOR_STOP_VOL)   /* ボリューム電圧≦停止電圧 */
        {
            stop_pwm_output();  /* PWM信号出力停止 */
            sts_work = STOP_STS; /* STOP状態へ遷移 */
        }
        else{/* 現在の状態を保持 */}
        break;

    case ERR_STS:   /* 現在、ERR状態 */
        /* 状態遷移判定 */
        if((check_edc_voltage_err() == CHECK_OK)
        && (check_sw_over_current_err() == CHECK_OK)
        && (check_hw_over_current_err() == CHECK_OK)
        && (Volume_fil <= MOTOR_STOP_VOL))
        {
            sts_work = STOP_STS; /* STOP状態へ遷移 */
        }
        else{/* 現在の状態を保持 */}
        break;
    
    default:
        break;
    }

    MainStatus = sts_work;  /* メインステータス更新 */
}

/* 速度指令値[rad/s]設定処理 */
static void set_omega_star(void)
{
    /* ボリューム信号[V]から目標速度[rpm]を求める */
    if(MainStatus == RUN_STS)
    {
        TargetSpeed_rpm = Volume2RpmGain * (Volume_fil - MOTOR_START_VOL) + TARGET_MIN_RPM;   /* 機械角：[rpm] */
        if(TargetSpeed_rpm >= TARGET_MAX_RPM){TargetSpeed_rpm = TARGET_MAX_RPM;}    /* 上限リミット */
        if(TargetSpeed_rpm <= TARGET_MIN_RPM){TargetSpeed_rpm = TARGET_MIN_RPM;}    /* 下限リミット */
    }
    else
    {
        TargetSpeed_rpm = 0.0f;
    }

    /* 目標速度まで加減速 */
    if(TargetSpeed_rpm > ReferenceSpeed_rpm)
    {
        ReferenceSpeed_rpm += (TARGET_ACC_RPM * T_PWM);    /* 加速 */
        if(ReferenceSpeed_rpm >= TargetSpeed_rpm){ReferenceSpeed_rpm = TargetSpeed_rpm;}    /* リミット */
    }
    else if(TargetSpeed_rpm < ReferenceSpeed_rpm)
    {
        ReferenceSpeed_rpm -= (TARGET_ACC_RPM * T_PWM);    /* 減速 */
        if(ReferenceSpeed_rpm <= TargetSpeed_rpm){ReferenceSpeed_rpm = TargetSpeed_rpm;}    /* リミット */
    }else{/* 空 */}

    /* 制御で使うω*[rad/s]に変換 */
    ReferenceSpeed_hz = ReferenceSpeed_rpm * POLEPAIR / 60.0f;  /* 電気角周波数：[Hz] */
    Omega1Star = ReferenceSpeed_hz * RAD_2PI;      /* 電気角速度：[rad/s] */
}

/* 電流uvw→dq変換 */
static void change_uvw_to_dq_current(void)
{
    float work_Sin;
    float work_Cos;
    /* UVW -> αβ【相対変換】 */
    Ia = 0.66667f * (Iuvw.Iu - 0.5 * (Iuvw.Iv + Iuvw.Iw));    /* Iα=(2/3)*{Iu-(Iv+Iw)/2} */
    Ib = 0.57735f * (Iuvw.Iv - Iuvw.Iw);                      /* Iβ=(1/sqrt(3))*(Iv-Iw) */
    /* αβ -> dq */
    work_Sin = sinf(ThetaD);
    work_Cos = cosf(ThetaD);
    Id = Ia * work_Cos + Ib * work_Sin;
    Iq = -Ia * work_Sin + Ib * work_Cos;
    IdFil += (Id - IdFil) * Idq_Fil_Gain;
    IqFil += (Iq - IqFil) * Idq_Fil_Gain;
}

/* 電圧dq→uvw変換 */
static void change_dq_to_uvw_voltage(void)
{
    float work_Sin;
    float work_Cos;
    float Vmax;  /* 最大電圧 */
    float Vmin;  /* 最小電圧 */
    float Vzero;    /* (最大ー最小電圧)/2 */

    /* 電圧位相更新 */
    ThetaV = ThetaD + 1.5f * T_PWM_HALF * Omega1; /* θv[rad]=θd[rad]+1.5Ts[s]*ω[rad/s]・・・現在の位相θdから1.5周期先の位相が電圧出力時の位相 */

    /* dq -> αβ */
    work_Sin = sinf(ThetaV);
    work_Cos = cosf(ThetaV);
    Va = VdStar * work_Cos - VqStar * work_Sin;
    Vb = VdStar * work_Sin + VqStar * work_Cos;
    /* αβ -> Vuvw【相対変換】 */
    Vu = Va;                                /* Vu=Vα */
    Vv = 0.5f * (-Va + 1.73205f * Vb);    /* Vv=(1/2)*(-Vα+sqrt(3)*Vβ) */
    Vw = 0.5f * (-Va - 1.73205f * Vb);    /* Vw=(1/2)*(-Vα-sqrt(3)*Vβ) */

    /* HIP変調 */
    if(Vu > Vv)
    {
        if(Vu > Vw)  /* Max=Vu */
        {
            Vmax = Vu;
            if(Vv > Vw){Vmin = Vw;}  /* Min=Vw */
            else{Vmin = Vv;}                   /* Min=Vv */
        }
        else
        {
            Vmax = Vw;  /* Max=Vw */
            Vmin = Vv;  /* Min=Vv */
        }
    }
    else
    {
        if(Vv > Vw)  /* Max=Vv */
        {
            Vmax = Vv;
            if(Vu > Vw){Vmin = Vw;}  /* Min=Vw */
            else{Vmin = Vu;}                   /* Min=Vu */
        }
        else
        {
            Vmax = Vw;  /* Max=Vw */
            Vmin = Vu;  /* Min=Vu */
        }
    }
    Vzero = (Vmax + Vmin) * 0.5; /* MaxとMinの平均値 */
    VuvwStar.Vu = Vu - Vzero;
    VuvwStar.Vv = Vv - Vzero;
    VuvwStar.Vw = Vw - Vzero;
}

/* 速度制御 */
static void speed_control(void)
{
    float work_OmegaErr;
    work_OmegaErr = Omega1Star - Omega1;  /* 誤差 */
    /* 積分項演算 */
    AsrIsum += work_OmegaErr * Ki_Asr;
    if(AsrIsum > ISTAR_LIM){AsrIsum = ISTAR_LIM;}
    if(AsrIsum < -ISTAR_LIM){AsrIsum = -ISTAR_LIM;}
    /* PI制御 */
    IqStar_ASR = work_OmegaErr * Kp_Asr + AsrIsum;
    if(IqStar_ASR > ISTAR_LIM){IqStar_ASR = ISTAR_LIM;}
    if(IqStar_ASR < -ISTAR_LIM){IqStar_ASR = -ISTAR_LIM;}
}

/* 電流制御 */
static void current_control(void)
{
    float work_Ierr;
    /* d軸側 */
    work_Ierr = IdStar - IdFil;  /* 誤差 */
    /* 積分項演算 */
    AcrIdsum += work_Ierr * Ki_Acr;
    if(AcrIdsum > VSTAR_LIM){AcrIdsum = VSTAR_LIM;}
    if(AcrIdsum < -VSTAR_LIM){AcrIdsum = -VSTAR_LIM;}
    /* PI制御 */
    DeltaVdStar = work_Ierr * Kp_Acr + AcrIdsum;

    /* q軸側 */
    IqStar = IqStar_ASR;    /* 指令値設定 */
    work_Ierr = IqStar - IqFil;  /* 誤差 */
    /* 積分項演算 */
    AcrIqsum += work_Ierr * Ki_Acr;
    if(AcrIqsum > VSTAR_LIM){AcrIqsum = VSTAR_LIM;}
    if(AcrIqsum < -VSTAR_LIM){AcrIqsum = -VSTAR_LIM;}
    /* PI制御 */
    DeltaVqStar = work_Ierr * Kp_Acr + AcrIqsum;
}

/* 非干渉制御 */
static void decoupling_control(void)
{
    /* d軸側 */
//    VdStarDash = -Omega1 * MOTOR_LS * IqStar;
    VdStarDash = MOTOR_R * IdStar - Omega1 * MOTOR_LS * IqStar;
    VdStar = VdStarDash + DeltaVdStar;
    if(VdStar > VSTAR_LIM){VdStar = VSTAR_LIM;}
    if(VdStar < -VSTAR_LIM){VdStar = -VSTAR_LIM;}

    /* q軸側 */
//    VqStarDash = Omega1 * MOTOR_LS * IdStar + MOTOR_KE * Omega1;
    VqStarDash = MOTOR_R * IqStar + Omega1 * MOTOR_LS * IdStar + MOTOR_KE * Omega1;
    VqStar = VqStarDash + DeltaVqStar;
    if(VqStar > VSTAR_LIM){VqStar = VSTAR_LIM;}
    if(VqStar < -VSTAR_LIM){VqStar = -VSTAR_LIM;}
}

/* PWM周期処理 */
void pwm_task(uint16_t YamaTani)
{
    position_sensor(&ThetaD, &Omega1);  /* 位置センサ処理 */

    if(YamaTani == INT_TANI)
    {
        /* PWM谷割込みの時だけ実施 */
        Iuvw = get_phase_current();   /* 相電流取得 */

        Edc = get_edc_voltage();  /* 直流電圧取得 */
        Edc_fil += (Edc - Edc_fil) * Edc_fil_gain;      /* 一次遅れフィルタ */

        Volume = get_volume_voltage();    /* ボリューム信号電圧取得 */
        Volume_fil += (Volume - Volume_fil) * Volume_fil_gain;      /* 一次遅れフィルタ */

        /* ------------------------------ */
        /* メインステータス[MainStatus]更新 */
        /* ------------------------------ */
        state_machine();

        /* --------- */
        /* モータ制御 */
        /* --------- */
        /* (1)速度指令設定 */
        set_omega_star();
        
        /* (2)電流uvw→dq変換 */
        change_uvw_to_dq_current();

        /* (3)ベクトル制御 */
        /* (3-1)速度制御 */
        speed_control();

        /* (3-2)電流制御 */
//        current_control();
        IqStar = IqStar_ASR;    /* 指令値設定 */
 
        /* (3-3)非干渉制御 */
        decoupling_control();
    }

    /* (4)電圧dq→uvw変換(HIP変調込み) */
    change_dq_to_uvw_voltage();

    /* (5)PWMタイマ設定 */
    set_pwm_counter(&VuvwStar, Edc_fil/2.0f);
}


int counter = 0;

/* メイン関数 */
void main(void)
{
    /* PWM出力停止 */
    stop_pwm_output();

	/* Timer and Interrupt Start */
	R_Config_MTU3_MTU4_Start();
	R_Config_CMT0_Start();
    R_Config_S12AD0_Start();
    R_Config_S12AD2_Start();
    position_sensor_init();

    /* loop */
    while (1)
    {
    	/* CPUボードのLEDを1secごとに点滅 */
        if(counter < F_PWM)
        {
            PORT2.PODR.BIT.B0 = 1;
        }
        else
        {
        	PORT2.PODR.BIT.B0 = 0;
            if(counter >= (F_PWM * 2))
            {
                counter = 0;
            }
        }
    }
}


//for debug
void debug_ram(void)
{
    setpsw_i(); /* 割込み許可 */

    if(MainStatus == RUN_STS)
    {
        printf("%.2f ",IqStar);
        printf("%.2f ",IdFil);
        printf("%.2f ",IqFil);
        printf("%.2f ",VdStar);
        printf("%.2f ",VqStar);
        printf("%.2f ",Omega1Star);
        printf("%.2f\n",Omega1);
    }
}