#include "foc.h"
//https://dengfoc.com
/*
1. 3個電流矢量Ia,Ib,Ic(相位差120度的sin波形)，轉換為2個電流矢量I_alpha,I_beta(相差90度)
2. 公式:
    I_alpha = Ia- Ib*sin30°- Ic*sin30°即  I_alpha = Ia- Ib/2- Ic/2
    I_beta = Ib*cos30°- Ic*cos30°即  I_beta = Ib*sqrt(3)/2- Ic*sqrt(3)/2
3. 等幅值變換:電流需要乘2/3，推導見網頁
   I_alpha = (Ia- Ib/2- Ic/2) *2/3
   I_beta = (Ib*sqrt(3)/2- Ic*sqrt(3)/2)  *2/3
   又有基爾霍夫電流定律: Ia+Ib+Ic = 0,消去Ic，得公式如下:
   I_alpha = Ia
   I_beta = (Ib*2+Ia)/sqrt(3)
   實際等幅值變換需額外修正功率系數，不用于實際foc控制
4. 等功率變換:電流需要乘sqrt(2/3),理論依據是瞬間功率相等，Ia*Ua + Ib*Ub + Ic*Uc = I_alpha*U_alpha + I_beta*U_beta,最后推導出系數為sqrt(2/3)，
   又有基爾霍夫電流定律: Ia+Ib+Ic = 0,消去Ic，得公式如下:
    I_alpha = Ia*sqrt(2/3)
    I_beta = (Ib*2+Ia)/sqrt(2)

*/
void clark(double *Ia, double *Ib, double *I_alpha, double *I_beta)
{
    *I_alpha = (*Ia) * sqrt(2.0 / 3.0);
    *I_beta = (*Ib * 2 + *Ia) / sqrt(2.0);
}
void park(double *I_alpha, double *I_beta, double *Id, double *Iq, double theta)
{
    *Id = (*I_alpha) * cos(theta) + (*I_beta) * sin(theta);
    *Iq = -(*I_alpha) * sin(theta) + (*I_beta) * cos(theta);
}

void ipark(double *I_alpha, double *I_beta, double *Id, double *Iq, double theta)
{
*I_alpha = (*Id) * sin(theta) + (*Iq) * cos(theta);
*I_beta = (*Id) * cos(theta) - (*Iq) * sin(theta);
}

#ifndef _FOC_H_
#define _FOC_H_
#include "main.h"

void clark(double *Ia, double *Ib, double *I_alpha, double *I_beta);
void park(double *I_alpha, double *I_beta, double *Id, double *Iq, double theta);

　void ipark(double *I_alpha, double *I_beta, double *Id, double *Iq, double theta);

#endif