文章目錄
- 1、ST龍伯格算法分析(定點數)
- 1.1 符號說明
- 1.2 最大感應電動勢計算
- 1.3 系數計算
- 1.4 龍伯格觀測器計算
- 1.5 鎖相環計算
- 1.6 觀測器增益計算
- 1.7 鎖相環PI計算(ST)
- 1.8 平均速度的用意
- 2、啟動策略
- 2.1 V/F壓頻比控制
- 2.2 I/F壓頻比控制
- 3、算法開發
- 3.1 Luenberger核心算法模塊
- 3.1.1 Luenberger.h
- 3.1.2 Luenberger.c
- 3.2 三段式啟動狀態機模塊
- 3.2.1 mc_statemachine.h
- 3.2.2 mc_statemachine.c
- 3.3 初始化及函數調用
- 3.3.1 初始化
- 3.3.2 反饋速度處理
- 3.3.3 FOC模塊處理
龍伯格觀測+PLL理論篇: https://blog.csdn.net/qq_28149763/article/details/136346434
說明:關鍵代碼已在本文給出(源碼不開源,抱歉)
1、ST龍伯格算法分析(定點數)
1.1 符號說明
1.2 最大感應電動勢計算
1.3 系數計算
1.4 龍伯格觀測器計算
1.5 鎖相環計算
1.6 觀測器增益計算
1.7 鎖相環PI計算(ST)
1.8 平均速度的用意
2、啟動策略
2.1 V/F壓頻比控制
2.2 I/F壓頻比控制
3、算法開發
3.1 Luenberger核心算法模塊
3.1.1 Luenberger.h
/********************************************************************************* @file Luenberger.h* @author hlping* @version V1.0.0* @date 2023-12-28* @brief ******************************************************************************* @attention********************************************************************************/#ifndef __LUENBERGER_H
#define __LUENBERGER_H/* *INDENT-OFF* */
#ifdef __cplusplus
extern "C"
{#endif/* Includes -----------------------------------------------------------------*/#include <types.h>#include "mc_math.h"#include "foc.h"/* Macros -------------------------------------------------------------------*/#define BUFFER_SIZE (u16)64#define BUF_POW2 LOG2(BUFFER_SIZE)#define F1 (s16)(16384)#define F2 (s16)(8192)#define F1LOG LOG2(F1)#define F2LOG LOG2(F2)#define PLL_KP_F (s16)(16384)#define PLL_KI_F (u16)(65535)#define KPLOG LOG2(PLL_KP_F)#define KILOG LOG2(PLL_KI_F)#define PI 3.14159265358979#define VARIANCE_THRESHOLD 0.0625 typedef struct{s32 K1;s32 K2;s16 hC2;s16 hC4;s16 hF1;s16 hF2;s16 hF3;s16 hC1;s16 hC3;s16 hC5;s16 hC6;s32 wIalfa_est;s32 wIbeta_est;s32 wBemf_alfa_est;s32 wBemf_beta_est;s32 hBemf_alfa_est;s32 hBemf_beta_est;}STO_Observer_t;typedef struct{s16 Rs;u16 Rs_factor;s16 Ls;u32 Ls_factor;u16 Pole;u16 pwm_frequency;u32 max_speed_rpm;s16 max_voltage;s16 max_current;s16 motor_voltage_constant;s16 motor_voltage_constant_factor;float motor_voltage_constant_f;u16 max_bemf_voltage;}STO_Parameter_t;typedef struct{bool_t Max_Speed_Out;bool_t Min_Speed_Out;bool_t Is_Speed_Reliable;s16 hSpeed_Buffer[BUFFER_SIZE];u16 bSpeed_Buffer_Index;s32 wMotorMaxSpeed_dpp;u16 hPercentageFactor;s16 hRotor_Speed_dpp;s32 wSpeed_PI_integral_sum;s16 hSpeed_P_Gain;s16 hSpeed_I_Gain;s32 speed_sum;}STO_Speed_t;typedef struct{STO_Observer_t STO_Observer;STO_Speed_t STO_Speed;STO_Parameter_t STO_Parameter;s16 hRotor_El_Angle;s16 hRotor_Speed;s16 hLast_Rotor_Speed;s16 hRotor_avSpeed;}STO_luenberger;/* Typedefs -----------------------------------------------------------------*//* Function declarations ----------------------------------------------------*/void STO_InitSpeedBuffer(STO_luenberger *pHandle);void STO_Init(STO_luenberger *pHandle);void STO_Update_Constant(STO_luenberger *pHandle);void STO_Set_k1k2(STO_luenberger *pHandle,s32 pk1,s32 pk2);void STO_PLL_Set_Gains(STO_luenberger *pHandle,s16 pkp,s16 pki);void STO_Gains_Init(STO_luenberger *pHandle);s16 Speed_PI(STO_luenberger *pHandle,s16 hAlfa_Sin, s16 hBeta_Cos);s16 Calc_Rotor_Speed(STO_luenberger *pHandle,s16 hBemf_alfa, s16 hBemf_beta);void Store_Rotor_Speed(STO_luenberger *pHandle,s16 hRotor_Speed);s16 STO_Get_Speed(STO_luenberger *pHandle);s16 STO_Get_Electrical_Angle(STO_luenberger *pHandle);void STO_Set_Electrical_Angle(STO_luenberger *pHandle,s16 eiAngle);void STO_Calc_Speed(STO_luenberger *pHandle);void STO_CalcElAngle(STO_luenberger *pHandle,FOCVars_t *pfoc, s16 hBusVoltage);/* *INDENT-OFF* */#ifdef __cplusplus
}
#endif
/* *INDENT-ON* */#endif /* __LUENBERGER_H *//**** END OF FILE ****/
3.1.2 Luenberger.c
/********************************************************************************* @file Luenberger.c* @author hlping* @version V1.0.0* @date 2023-12-28* @brief ******************************************************************************* @attention********************************************************************************//* Includes ------------------------------------------------------------------*/
#include "Luenberger.h"/* Private define ------------------------------------------------------------*//**
* @brief 初始化觀測器速度緩沖區
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @return 無返回值
*/
void STO_InitSpeedBuffer(STO_luenberger *pHandle)
{u8 i;/*init speed buffer*/for (i=0;i<BUFFER_SIZE;i++){pHandle->STO_Speed.hSpeed_Buffer[i] = 0x00;}pHandle->STO_Speed.bSpeed_Buffer_Index = 0;
}/**
* @brief 初始化觀測器
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @return 無返回值
*/
void STO_Init(STO_luenberger *pHandle)
{pHandle->STO_Observer.wIalfa_est = 0;pHandle->STO_Observer.wIbeta_est = 0;pHandle->STO_Observer.wBemf_alfa_est = 0;pHandle->STO_Observer.wBemf_beta_est = 0;pHandle->STO_Speed.Is_Speed_Reliable = FALSE;pHandle->STO_Speed.wSpeed_PI_integral_sum = 0;pHandle->STO_Speed.Max_Speed_Out = FALSE;pHandle->STO_Speed.Min_Speed_Out = FALSE;pHandle->STO_Speed.hRotor_Speed_dpp = 0;pHandle->STO_Speed.speed_sum = 0;pHandle->hRotor_avSpeed=0;pHandle->hRotor_El_Angle = 0; //could be used for start-up procedurepHandle->hRotor_avSpeed = 0;STO_InitSpeedBuffer(pHandle);// hSpeed_P_Gain = 1638; //0.1*16384
// hSpeed_I_Gain = 0;
}/**
* @brief 設置觀測器增益參數
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @param pk1 增益參數1
* @param pk2 增益參數2
* @return 無返回值
*/
void STO_Set_k1k2(STO_luenberger *pHandle,s32 pk1,s32 pk2)
{pHandle->STO_Observer.K1 = pk1;pHandle->STO_Observer.K2 = pk2;
}/**
* @brief 設置觀測器PLL增益參數
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @param pkp PLL比例增益參數
* @param pki PLL積分增益參數
* @return 無返回值
*/
void STO_PLL_Set_Gains(STO_luenberger *pHandle,s16 pkp,s16 pki)
{pHandle->STO_Speed.hSpeed_P_Gain = pkp;pHandle->STO_Speed.hSpeed_I_Gain = pki;
}/**
* @brief 更新觀測器常數參數
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @return 無返回值
*/
void STO_Update_Constant(STO_luenberger *pHandle)
{float temp_rs;float temp_ls;temp_rs=pHandle->STO_Parameter.Rs/(float)pHandle->STO_Parameter.Rs_factor;temp_ls=pHandle->STO_Parameter.Ls/(float)pHandle->STO_Parameter.Ls_factor;pHandle->STO_Observer.hC1 = (s32)(pHandle->STO_Observer.hF1 * temp_rs/(temp_ls*pHandle->STO_Parameter.pwm_frequency));//pHandle->STO_Observer.hC2 = (s32)(hF1 * k1/(float)(pHandle->STO_Parameter.pwm_frequency));pHandle->STO_Observer.hC2 = (s32)(pHandle->STO_Observer.K1);//�����?������?�����?�?���?��pHandle->STO_Observer.hC3 = (s32)(pHandle->STO_Observer.hF1 * pHandle->STO_Parameter.max_bemf_voltage/(temp_ls*pHandle->STO_Parameter.max_current*pHandle->STO_Parameter.pwm_frequency));//pHandle->STO_Observer.hC4 = (s32)(((k2 * max_current/(max_bemf_voltage))*hF2)/(float)pHandle->STO_Parameter.pwm_frequency);//pHandle->STO_Observer.hC4 = (s32)(hF1 * k2/(float)(pHandle->STO_Parameter.pwm_frequency));pHandle->STO_Observer.hC4 = (s32)(pHandle->STO_Observer.K2);//�����?������?�����?�?���?��pHandle->STO_Observer.hC5 = (s32)(pHandle->STO_Observer.hF1 * pHandle->STO_Parameter.max_voltage/(temp_ls*pHandle->STO_Parameter.max_current*pHandle->STO_Parameter.pwm_frequency));// hC1 = pHandle->STO_Observer.hC1;
// hC2 = pHandle->STO_Observer.hC2;
// hC3 = pHandle->STO_Observer.hC3;
// hC4 = pHandle->STO_Observer.hC4;
// hC5 = pHandle->STO_Observer.hC5;
}/**
* @brief 初始化觀測器增益參數
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @return 無返回值
*/
void STO_Gains_Init(STO_luenberger *pHandle)
{s16 htempk;pHandle->STO_Observer.hF3 = 1;htempk = (s16)((100*65536)/(F2*2*PI)); //100 rad/swhile (htempk != 0){htempk /= 2;pHandle->STO_Observer.hF3 *= 2;}pHandle->STO_Observer.hC6 = (s16)((F2 * pHandle->STO_Observer.hF3 * 2 * PI)/65536);//10000pHandle->STO_Observer.hF1 = F1;pHandle->STO_Observer.hF2 = F2;pHandle->STO_Parameter.motor_voltage_constant_f = (float)(pHandle->STO_Parameter.motor_voltage_constant/(float)pHandle->STO_Parameter.motor_voltage_constant_factor);pHandle->STO_Parameter.max_bemf_voltage = (u16)((1.2 * pHandle->STO_Parameter.max_speed_rpm*pHandle->STO_Parameter.motor_voltage_constant_f*SQRT_2)/(1000*SQRT_3));
// pHandle->STO_Parameter.max_current = (u16)(pHandle->STO_Parameter.max_current);
// pHandle->STO_Parameter.max_voltage = (s16)(pHandle->STO_Parameter.max_voltage);STO_Update_Constant(pHandle);// hSpeed_P_Gain = PLL_KP_GAIN;
// hSpeed_I_Gain = PLL_KI_GAIN;pHandle->STO_Speed.wMotorMaxSpeed_dpp = (s32)((1.2 * pHandle->STO_Parameter.max_speed_rpm*65536*pHandle->STO_Parameter.Pole)/(float)(pHandle->STO_Parameter.pwm_frequency*60));pHandle->STO_Speed.hPercentageFactor = (u16)(VARIANCE_THRESHOLD*128);
}/**
* @brief 計算電機旋轉速度的PID控制器
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @param hAlfa_Sin e_alpha*sin
* @param hBeta_Cos e_beta*cos
* @return 返回計算得到的電機旋轉速度
*/
s16 Speed_PI(STO_luenberger *pHandle,s16 hAlfa_Sin, s16 hBeta_Cos)
{s32 wSpeed_PI_error, wOutput;s32 wSpeed_PI_proportional_term, wSpeed_PI_integral_term;wSpeed_PI_error = hBeta_Cos - hAlfa_Sin;
#if 0 //????if(wSpeed_PI_error > 50)wSpeed_PI_error = 50;else if(wSpeed_PI_error < -50)wSpeed_PI_error = -50;
#endifwSpeed_PI_proportional_term = pHandle->STO_Speed.hSpeed_P_Gain * wSpeed_PI_error; // !!!pwSpeed_PI_integral_term = pHandle->STO_Speed.hSpeed_I_Gain * wSpeed_PI_error; // !!!iif ( (pHandle->STO_Speed.wSpeed_PI_integral_sum >= 0) && (wSpeed_PI_integral_term >= 0) && (pHandle->STO_Speed.Max_Speed_Out == FALSE) ){if ((s32)(pHandle->STO_Speed.wSpeed_PI_integral_sum + wSpeed_PI_integral_term) < 0){pHandle->STO_Speed.wSpeed_PI_integral_sum = S32_MAX;}else{pHandle->STO_Speed.wSpeed_PI_integral_sum += wSpeed_PI_integral_term; //integral}}else if ( (pHandle->STO_Speed.wSpeed_PI_integral_sum <= 0) && (wSpeed_PI_integral_term <= 0) && (pHandle->STO_Speed.Min_Speed_Out == FALSE) ){if((s32)(pHandle->STO_Speed.wSpeed_PI_integral_sum + wSpeed_PI_integral_term) > 0){pHandle->STO_Speed.wSpeed_PI_integral_sum = -S32_MAX;}else{pHandle->STO_Speed.wSpeed_PI_integral_sum += wSpeed_PI_integral_term; //integral}}else{pHandle->STO_Speed.wSpeed_PI_integral_sum += wSpeed_PI_integral_term; //integral}wOutput = (wSpeed_PI_proportional_term >> KPLOG) + (pHandle->STO_Speed.wSpeed_PI_integral_sum >> KILOG);if (wOutput > pHandle->STO_Speed.wMotorMaxSpeed_dpp){pHandle->STO_Speed.Max_Speed_Out = TRUE;wOutput = pHandle->STO_Speed.wMotorMaxSpeed_dpp;}else if (wOutput < (-pHandle->STO_Speed.wMotorMaxSpeed_dpp)){pHandle->STO_Speed.Min_Speed_Out = TRUE;wOutput = -pHandle->STO_Speed.wMotorMaxSpeed_dpp;}else{pHandle->STO_Speed.Max_Speed_Out = FALSE;pHandle->STO_Speed.Min_Speed_Out = FALSE;}return ((s16)wOutput);
}/**
* @brief 鎖相環計算電機控制器旋轉速度
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @param hBemf_alfa BEMF alpha軸反電動勢觀測值
* @param hBemf_beta BEMF beta軸反電動勢觀測值
* @return 返回計算得到的電機旋轉速度
*/
s16 Calc_Rotor_Speed(STO_luenberger *pHandle,s16 hBemf_alfa, s16 hBemf_beta)
{s32 wAlfa_Sin_tmp, wBeta_Cos_tmp;s16 hOutput;Trig_Components Local_Components;Local_Components = Trig_Functions(pHandle->hRotor_El_Angle);/* Alfa & Beta BEMF multiplied by hRotor_El_Angle Cos & Sin*/wAlfa_Sin_tmp = (s32)(hBemf_alfa * Local_Components.hSin);wBeta_Cos_tmp = (s32)(hBemf_beta * Local_Components.hCos);//alfa_sin_test = wAlfa_Sin_tmp >> 15;//beta_cos_test = wBeta_Cos_tmp >> 15;/* Speed PI regulator */hOutput = Speed_PI(pHandle,(s16)(wAlfa_Sin_tmp >> 15), (s16)(wBeta_Cos_tmp >> 15));return (hOutput);
}/**
* @brief 將電機旋轉速度存儲數組中
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @param hRotor_Speed 要存儲的電機旋轉速度
* @return 無返回值
*/
void Store_Rotor_Speed(STO_luenberger *pHandle,s16 hRotor_Speed)
{static s32 start_flag;pHandle->STO_Speed.hSpeed_Buffer[pHandle->STO_Speed.bSpeed_Buffer_Index] = hRotor_Speed;pHandle->STO_Speed.speed_sum += pHandle->STO_Speed.hSpeed_Buffer[pHandle->STO_Speed.bSpeed_Buffer_Index];if(++(pHandle->STO_Speed.bSpeed_Buffer_Index) >= BUFFER_SIZE) //16{pHandle->STO_Speed.bSpeed_Buffer_Index = 0;start_flag = 1;}if(start_flag == 0){pHandle->hRotor_avSpeed = pHandle->STO_Speed.speed_sum / pHandle->STO_Speed.bSpeed_Buffer_Index;}else{pHandle->hRotor_avSpeed = pHandle->STO_Speed.speed_sum >> BUF_POW2;pHandle->STO_Speed.speed_sum -= pHandle->STO_Speed.hSpeed_Buffer[pHandle->STO_Speed.bSpeed_Buffer_Index];}pHandle->STO_Speed.hRotor_Speed_dpp = pHandle->hRotor_avSpeed;
/*bSpeed_Buffer_Index++;if (bSpeed_Buffer_Index == BUFFER_SIZE) //64{bSpeed_Buffer_Index = 0;STO_Calc_Speed();}hSpeed_Buffer[bSpeed_Buffer_Index] = hRotor_Speed;
*/
}/**
* @brief 獲取電機旋轉速度
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @return 返回電機旋轉速度
*/
s16 STO_Get_Speed(STO_luenberger *pHandle)
{return (pHandle->hRotor_avSpeed);
}/**
* @brief 獲取電機轉子的電角度
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @return 返回電機電角位
*/
s16 STO_Get_Electrical_Angle(STO_luenberger *pHandle)
{return (pHandle->hRotor_El_Angle);
}/**
* @brief 設置電機轉子的電角度
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @param eiAngle 設置的電機電角位
* @return 無返回值
*/
void STO_Set_Electrical_Angle(STO_luenberger *pHandle,s16 eiAngle)
{pHandle->hRotor_El_Angle = eiAngle;
}/**
* @brief 計算觀測速度
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @return 無返回值
*/
void STO_Calc_Speed(STO_luenberger *pHandle)
{s32 wAverage_Speed = 0;s32 wError;s32 wAverageQuadraticError = 0;u8 i;for (i = 0; i < BUFFER_SIZE; i++){wAverage_Speed += pHandle->STO_Speed.hSpeed_Buffer[i];}wAverage_Speed = wAverage_Speed >> BUF_POW2;pHandle->STO_Speed.hRotor_Speed_dpp = (s16)(wAverage_Speed);for (i = 0; i < BUFFER_SIZE; i++){wError = pHandle->STO_Speed.hSpeed_Buffer[i] - wAverage_Speed;wError = (wError * wError);wAverageQuadraticError += (u32)(wError);}//It computes the measurement variancewAverageQuadraticError= wAverageQuadraticError >> BUF_POW2;//The maximum variance acceptable is here calculated as ratio of average speedwAverage_Speed = (s32)(wAverage_Speed * wAverage_Speed);wAverage_Speed = (wAverage_Speed >> 7) * pHandle->STO_Speed.hPercentageFactor;#if 0 // for debug onlyQuadraticError = wAverageQuadraticError;AverageSpeed = wAverage_Speed;
#endifif (wAverageQuadraticError > wAverage_Speed){pHandle->STO_Speed.Is_Speed_Reliable = FALSE;}else{pHandle->STO_Speed.Is_Speed_Reliable = TRUE;}
}/**
* @brief 觀測器觀測電角度
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲控制器的狀態信息
* @param pfoc 指向FOCVars_t結構體的指針,用于存儲電壓和電流信息
* @param hBusVoltage 輸入電壓
* @return 無返回值
*/
void STO_CalcElAngle(STO_luenberger *pHandle,FOCVars_t *pfoc, s16 hBusVoltage)
{s32 wIalfa_est_Next, wIbeta_est_Next;s32 wBemf_alfa_est_Next, wBemf_beta_est_Next;s16 hValfa, hVbeta;s16 hIalfa_err, hIbeta_err;s32 bDirection;s16 hRotor_Speed;if (pHandle->STO_Observer.wBemf_alfa_est > (s32)(S16_MAX * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_alfa_est = S16_MAX * pHandle->STO_Observer.hF2;}else if (pHandle->STO_Observer.wBemf_alfa_est <= (s32)(S16_MIN * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_alfa_est = -S16_MAX * pHandle->STO_Observer.hF2;}if (pHandle->STO_Observer.wBemf_beta_est > (s32)(S16_MAX * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_beta_est = S16_MAX * pHandle->STO_Observer.hF2;}else if (pHandle->STO_Observer.wBemf_beta_est <= (s32)(S16_MIN * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_beta_est = -S16_MAX * pHandle->STO_Observer.hF2;}if (pHandle->STO_Observer.wIalfa_est > (s32)(S16_MAX * pHandle->STO_Observer.hF1)){pHandle->STO_Observer.wIalfa_est = S16_MAX * pHandle->STO_Observer.hF1;}else if (pHandle->STO_Observer.wIalfa_est <= (s32)(S16_MIN * pHandle->STO_Observer.hF1)){pHandle->STO_Observer.wIalfa_est = -S16_MAX * pHandle->STO_Observer.hF1;}if (pHandle->STO_Observer.wIbeta_est > S16_MAX * pHandle->STO_Observer.hF1){pHandle->STO_Observer.wIbeta_est = S16_MAX * pHandle->STO_Observer.hF1;}else if (pHandle->STO_Observer.wIbeta_est <= S16_MIN * pHandle->STO_Observer.hF1){pHandle->STO_Observer.wIbeta_est = -S16_MAX * pHandle->STO_Observer.hF1;}hIalfa_err = (s16)((pHandle->STO_Observer.wIalfa_est >> F1LOG)- pfoc->Ialphabeta.alpha);hIbeta_err = (s16)((pHandle->STO_Observer.wIbeta_est >> F1LOG)- pfoc->Ialphabeta.beta);hValfa = (s16)((pfoc->Valphabeta.alpha * hBusVoltage) >> 15); //�������?�??���?����?�?������??��?��hVbeta = (s16)((pfoc->Valphabeta.beta * hBusVoltage) >> 15); //�������?�??���?����?�?������??��?��/*alfa axes observer*/wIalfa_est_Next = (s32)(pHandle->STO_Observer.wIalfa_est - (s32)(pHandle->STO_Observer.hC1 * (s16)(pHandle->STO_Observer.wIalfa_est >> F1LOG))+(s32)(pHandle->STO_Observer.hC2 * hIalfa_err)+(s32)(pHandle->STO_Observer.hC5 * hValfa)-(s32)(pHandle->STO_Observer.hC3 * (s16)(pHandle->STO_Observer.wBemf_alfa_est >> F2LOG)));//I(n+1)=I(n)-rs*T/Ls*I(n)+K1*(I(n)-i(n))+T/Ls*V-T/Ls*emfwBemf_alfa_est_Next = (s32)(pHandle->STO_Observer.wBemf_alfa_est + (s32)(pHandle->STO_Observer.hC4 * hIalfa_err)+(s32)(pHandle->STO_Observer.hC6 * pHandle->STO_Speed.hRotor_Speed_dpp * (pHandle->STO_Observer.wBemf_beta_est / (pHandle->STO_Observer.hF2 * pHandle->STO_Observer.hF3))));//(wBemf_beta_est>>20)));//emf(n+1)=emf(n)+K2*(I(n)-i(n))+p*w*emfb*T/*beta axes observer*/wIbeta_est_Next = (s32)(pHandle->STO_Observer.wIbeta_est - (s32)(pHandle->STO_Observer.hC1 * (s16)(pHandle->STO_Observer.wIbeta_est >> F1LOG))+(s32)(pHandle->STO_Observer.hC2 * hIbeta_err)+(s32)(pHandle->STO_Observer.hC5 * hVbeta)-(s32)(pHandle->STO_Observer.hC3 * (s16)(pHandle->STO_Observer.wBemf_beta_est >> F2LOG)));wBemf_beta_est_Next = (s32)(pHandle->STO_Observer.wBemf_beta_est + (s32)(pHandle->STO_Observer.hC4 * hIbeta_err)-(s32)(pHandle->STO_Observer.hC6 * pHandle->STO_Speed.hRotor_Speed_dpp * (pHandle->STO_Observer.wBemf_alfa_est / (pHandle->STO_Observer.hF2 * pHandle->STO_Observer.hF3))));//(wBemf_alfa_est>>20)));/* Extrapolation of present rotation direction, necessary for PLL */if (pHandle->STO_Speed.hRotor_Speed_dpp >= 0){bDirection = -1;}else{bDirection = 1;}/*Calls the PLL blockset*/pHandle->STO_Observer.hBemf_alfa_est = pHandle->STO_Observer.wBemf_alfa_est >> F2LOG;pHandle->STO_Observer.hBemf_beta_est = pHandle->STO_Observer.wBemf_beta_est >> F2LOG;pHandle->hRotor_Speed = Calc_Rotor_Speed(pHandle,(s16)(pHandle->STO_Observer.hBemf_alfa_est * bDirection),(s16)(-pHandle->STO_Observer.hBemf_beta_est * bDirection));if(pfoc->Vqd.q > 0){if(pHandle->hRotor_Speed < 0){pHandle->hRotor_Speed = -pHandle->hRotor_Speed;}}else //MotorCtrl.Dir == CCW{if(pHandle->hRotor_Speed > 0){pHandle->hRotor_Speed = -pHandle->hRotor_Speed;}}Store_Rotor_Speed(pHandle,pHandle->hRotor_Speed);pHandle->hRotor_El_Angle = (s16)(pHandle->hRotor_El_Angle + pHandle->hRotor_Speed);/*storing previous values of currents and bemfs*/pHandle->STO_Observer.wIalfa_est = wIalfa_est_Next;pHandle->STO_Observer.wBemf_alfa_est = wBemf_alfa_est_Next;pHandle->STO_Observer.wIbeta_est = wIbeta_est_Next;pHandle->STO_Observer.wBemf_beta_est = wBemf_beta_est_Next;
}
/**** END OF FILE ****/
3.2 三段式啟動狀態機模塊
3.2.1 mc_statemachine.h
mc_statemachine.h定義相關變量和結構體以及函數申明:
/********************************************************************************* @file mc_statemachine.h* @author hlping* @version V1.0.0* @date 2022-11-28* @brief ******************************************************************************* @attention********************************************************************************/#ifndef __MC_STATEMACHINE_H
#define __MC_STATEMACHINE_H/* *INDENT-OFF* */
#ifdef __cplusplus
extern "C"
{
#endif/* Includes -----------------------------------------------------------------*/
#include <types.h>
#include "Luenberger.h"/* Macros -------------------------------------------------------------------*//* Typedefs -----------------------------------------------------------------*/
#define OPEN_LOOP 0
#define CLOSE_LOOP 1
#define IDLE_STATE 2
#define CLOSE_SWITCH 3#define OPENLOOPTIMEINSEC 8.0typedef enum{MOTOR_STOP=0, MOTOR_INIT=1, MOTOR_START=2, MOTOR_RUN=3, MOTOR_FAULT=4, MOTOR_BRAKE=5
} MCStatus_t;//typedef struct
//{
// MCStatus_t Status;
// u32 StatusMacCnt;
// u32 Dir;
// bool_t DirChangeFlag;
// bool_t StartupFlag; // s16 SpdRampRef;
// s16 SpdRef;
//}mc_control_t;typedef struct
{u32 State;u32 Angle;s16 LockCnt;u16 pole;u32 pwm_frequency;float looptimeinsec;//u32 time;u32 locktime;u16 initialSpeedinRpm;u16 start_iq;u16 start_iq_max;u16 min_iq;u16 start_vq;u16 endSpeedOpenloop;u16 inc_iq;u32 ramp_time;u32 delta_startup_ramp;s16 ElangleError;bool_t speed_loop_enable;bool_t current_loop_enable;
}mc_openloop_t;/* Function declarations ----------------------------------------------------*/
void mc_statemachine_init(mc_openloop_t *ploop);
void mc_statemachine_process(mc_openloop_t *popen,STO_luenberger *pHandle,FOCVars_t *pfoc,s16 hBusVoltage);
u16 mc_get_state(mc_openloop_t *ploop);
bool_t mc_get_speed_loop_enable(mc_openloop_t *ploop);
void mc_set_speed_loop_enable(mc_openloop_t *ploop,bool_t state);
bool_t mc_get_current_loop_enable(mc_openloop_t *ploop);
void mc_set_current_loop_enable(mc_openloop_t *ploop,bool_t state);
void mc_set_vf_iqRef(FOCVars_t *pfoc,int16_t piqref);
void mc_parameter_init(mc_openloop_t *ploop);/* *INDENT-OFF* */
#ifdef __cplusplus
}
#endif
/* *INDENT-ON* */#endif /* __MC_STATEMACHINE_H *//**** END OF FILE ****/
3.2.2 mc_statemachine.c
/********************************************************************************* @file mc_statemachine.c* @author hlping* @version V1.0.0* @date 2023-01-08* @brief ******************************************************************************* @attention********************************************************************************//* Includes ------------------------------------------------------------------*/
#include "mc_statemachine.h"
#include "public_global.h"/* Variable definitions ------------------------------------------------------*/
/**
* @brief 初始化電機啟動控制器狀態機
* @param ploop 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @return 無返回值
*/
void mc_statemachine_init(mc_openloop_t *ploop)
{ploop->speed_loop_enable = FALSE;ploop->current_loop_enable = FALSE;
// locktime = ploop->time;
// ploop->Angle = 0;
// ploop->State = IDLE_STATE;
// ploop->LockCnt = 0;
// endSpeedOpenloop = ploop->endSpeedOpenloop;ploop->looptimeinsec = 1/(float)ploop->pwm_frequency;ploop->inc_iq = 32767 * (ploop->start_iq_max - ploop->start_iq)/ploop->locktime;ploop->ramp_time = (u32)(ploop->endSpeedOpenloop * ploop->pole * 65536 * ploop->looptimeinsec * 65536 /60 );ploop->delta_startup_ramp = (u32)((ploop->ramp_time/OPENLOOPTIMEINSEC)/(float)ploop->pwm_frequency);
}/**
* @brief 處理電機啟動控制器狀態機
* @param popen 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @param pHandle 指向STO_luenberger結構體的指針,用于存儲觀測器狀態
* @param pfoc 指向FOCVars_t結構體的指針,用于存儲FOC相關變量
* @param hBusVoltage 輸入電壓,單位為V
* @return 無返回值
*/
void mc_statemachine_process(mc_openloop_t *popen,STO_luenberger *pHandle,FOCVars_t *pfoc,s16 hBusVoltage)
{if(popen->LockCnt >= popen->locktime && popen->State != IDLE_STATE)STO_CalcElAngle(pHandle,pfoc, hBusVoltage);if(popen->State == OPEN_LOOP){if(popen->LockCnt < popen->locktime) //LOCK{static s32 iq_ref_temp = 0;if (popen->LockCnt == 0)iq_ref_temp = 0;popen->LockCnt++;// if( FOCVars.Vqd.q > 0) //��?
// iq_ref_temp += inc_iq;
// else if(FOCVars.Vqd.q < 0) //��?
// iq_ref_temp -= inc_iq;
// else //??
// iq_ref_temp = 0;iq_ref_temp += popen->inc_iq;FOCVars.Iqdref.q = iq_ref_temp >> 15;}else if(popen->Angle < popen->ramp_time) //SPEED RAMP{if (popen->Angle == 0){ //FOCVars.Vqd.q = popen->start_vq * 32767 ;FOCVars.Vqd.q = popen->start_vq ;FOCVars.Vqd.d = 0 ;}popen->Angle += popen->delta_startup_ramp; if( FOCVars.Vqd.q > 0) //正轉FOCVars.hElAngle += (popen->Angle >> 16);else if(FOCVars.Vqd.q < 0) //反轉FOCVars.hElAngle -= (popen->Angle >> 16); }else{popen->State = CLOSE_LOOP;
//#ifndef NDEBUG
// OpenLoopSpeed = STO_Get_Speed(); // for test only
//#endif#if 0 //just for test,openloop for observation anglepopen->speed_loop_enable = FALSE;popen->current_loop_enable = FALSE;#elsepopen->speed_loop_enable = TRUE;popen->current_loop_enable = TRUE;#endifpopen->ElangleError = STO_Get_Electrical_Angle(pHandle) - FOCVars.hElAngle;//FOCVars.hElAngle = STO_Get_Electrical_Angle(pHandle);}}else if(popen->State == CLOSE_LOOP){FOCVars.hElAngle = STO_Get_Electrical_Angle(pHandle) - popen->ElangleError;
// FOCVars.hElAngle = STO_Get_Electrical_Angle(pHandle) - (popen->ElangleError>>2);
// //s16 err = popen->ElangleError>>2;
// if(popen->ElangleError > 0)
// popen->ElangleError--;
// else if(popen->ElangleError < 0)
// popen->ElangleError++;}
}/**
* @brief 初始化電機控制器參數
* @param ploop 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @return 無返回值
*/
void mc_parameter_init(mc_openloop_t *ploop)
{ploop->State = OPEN_LOOP; //開環啟動ploop->Angle = 0; //如等于RAMP_TIME就意味著跳過RAMP階段,直接速度閉環ploop->LockCnt = 0;ploop->speed_loop_enable = FALSE; ploop->current_loop_enable = FALSE;
}/**
* @brief 設置電機控制器的中間變量
* @param pfoc 指向FOCVars_t結構體的指針,用于存儲電機控制器的中間變量
* @param piqref 輸入的iq參考值
* @return 無返回值
*/
void mc_set_vf_iqRef(FOCVars_t *pfoc,int16_t piqref)
{pfoc->Iqdref.q = piqref;pfoc->Iqdref.d = 0;
}/**
* @brief 獲取電機控制器的狀態
* @param ploop 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @return 返回控制器的狀態
*/
u16 mc_get_state(mc_openloop_t *ploop)
{return (ploop->State);
}/**
* @brief 獲取電機控制器是否啟用速度環
* @param ploop 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @return 返回true或false,表示是否啟用速度環
*/
bool_t mc_get_speed_loop_enable(mc_openloop_t *ploop)
{return (ploop->speed_loop_enable);
}/**
* @brief 設置電機控制器是否啟用速度環
* @param ploop 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @param state 設置為true或false,表示是否啟用速度環
* @return 無返回值
*/
void mc_set_speed_loop_enable(mc_openloop_t *ploop,bool_t state)
{ploop->speed_loop_enable = state;
}/**
* @brief 獲取電機控制器是否啟用電流環
* @param ploop 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @return 返回true或false,表示是否啟用電流環
*/
bool_t mc_get_current_loop_enable(mc_openloop_t *ploop)
{return (ploop->current_loop_enable);
}/**
* @brief 設置電機控制器是否啟用電流環
* @param ploop 指向mc_openloop_t結構體的指針,用于存儲控制器的狀態信息
* @param state 設置為true或false,表示是否啟用電流環
* @return 無返回值
*/
void mc_set_current_loop_enable(mc_openloop_t *ploop,bool_t state)
{ploop->current_loop_enable = state;
}/**** END OF FILE ****/
3.3 初始化及函數調用
定義全局變量:
STO_luenberger STO_LBG; //龍伯格觀測器相關變量
mc_openloop_t mc_openloop; //三段式啟動相關變量
3.3.1 初始化
當編碼器類型為ENCODER_TYPE_UNKNOWN時為無感運行模式:
if(sensor_peripheral.Encoder_Sensor.encType == ENCODER_TYPE_UNKNOWN)//just for sensorless{sensor_peripheral.Encoder_Sensor.encRes=65535;/* init Luenberger parameter */STO_Parameter_t Parameter={.Rs = 55, //0.055 pMotorParSet.tBasePar.resist[A_AXIS].Rs_factor = 1000,.Ls = 21, //2.1e-4 pMotorParSet.tBasePar.inductance[A_AXIS].Ls_factor = 100000,.Pole = pMotorParSet.tBasePar.poles[A_AXIS], //4.pwm_frequency = SAMPLE_FREQUENCY, //10000//.max_speed_rpm = pMotorParSet.tBasePar.ratedVel[A_AXIS]*60/sensor_peripheral.Encoder_Sensor.encRes,//rpm.max_speed_rpm = 3000,//rpm.max_voltage = (s16)(ProtectPar.regenOn/1000), //36000 mV//.max_current = pMotorParSet.tBasePar.maxPhaseCurr[A_AXIS]/1000, //A.max_current = 31, //A.motor_voltage_constant = 4, //4v/1000rpm.motor_voltage_constant_factor = 1,};memcpy(&STO_LBG.STO_Parameter,&Parameter,sizeof(STO_Parameter_t));STO_Init(&STO_LBG);STO_Set_k1k2(&STO_LBG,-24225,25925);STO_Gains_Init(&STO_LBG);STO_PLL_Set_Gains(&STO_LBG,638,45); mc_openloop_t openloop={.State= IDLE_STATE,.Angle = 0,.LockCnt = 0,.pole = pMotorParSet.tBasePar.poles[A_AXIS],.pwm_frequency = SAMPLE_FREQUENCY,.looptimeinsec = 1/(float)SAMPLE_FREQUENCY,.locktime = SAMPLES_PER_50MSECOND,.start_iq = 0, //Q軸啟動電流.start_iq_max = 3000, //mA.endSpeedOpenloop = 300, //rpm.start_vq = 4000, //VF啟動電壓};memcpy(&mc_openloop,&openloop,sizeof(mc_openloop_t));mc_statemachine_init(&mc_openloop);}
3.3.2 反饋速度處理
void AxisVelocityCalc()
{float ftempll; if(sensor_peripheral.Encoder_Sensor.encType != ENCODER_TYPE_UNKNOWN){pAxisPar.vel[A_AXIS][2] = sensor_peripheral.Encoder_Sensor.deltaPos * SAMPLE_FREQUENCY; //TODO: ftempll = (float) pAxisPar.vel[A_AXIS][2];ftempll =_filterPar._velFdk(&ftempll,&_filterPar);//250 point 1.5uspAxisPar.vel[A_AXIS][1] = (long) ftempll;pAxisPar.vel[A_AXIS][0] = pAxisPar.vel[A_AXIS][1];}else{pAxisPar.vel[A_AXIS][2] = STO_Get_Speed(&STO_LBG)*sensor_peripheral.Encoder_Sensor.encRes/(pMotorParSet.tBasePar.poles[A_AXIS] * 2 * PI);//we->rpm->plus; //TODO: ftempll = (float) pAxisPar.vel[A_AXIS][2];ftempll =_filterPar._velFdk(&ftempll,&_filterPar);//250 point 1.5uspAxisPar.vel[A_AXIS][1] = (long) ftempll;pAxisPar.vel[A_AXIS][0] = pAxisPar.vel[A_AXIS][1];}
}
3.3.3 FOC模塊處理
/*** @brief FOC function* @param None* @retval None
**/
void FOC_Model(void)
{FOCVars.Iqdref.q = pMotorParSet.currRef[A_AXIS];//*1.414; // RMS resultFOCVars.Iqdref.d = 0;
// FOC_Cal(&FOCVars);FOCVars.Ialphabeta = Clark(FOCVars.Iab);FOCVars.Iqd = Park(FOCVars.Ialphabeta, FOCVars.hElAngle); FOCVars.IqdErr.d = FOCVars.Iqdref.d - FOCVars.Iqd.d;FOCVars.IqdErr.q = FOCVars.Iqdref.q - FOCVars.Iqd.q;if(sensor_peripheral.Encoder_Sensor.encType != ENCODER_TYPE_UNKNOWN){FOCVars.Vqd.d = PI_Controller(&PidIdHandle, FOCVars.IqdErr.d);PidIqHandle.hKpGain = PidIdHandle.hKpGain;PidIqHandle.hKiGain = PidIdHandle.hKiGain;FOCVars.Vqd.q = PI_Controller(&PidIqHandle, FOCVars.IqdErr.q);}else{if(mc_get_current_loop_enable(&mc_openloop) == TRUE){FOCVars.Vqd.d = PI_Controller(&PidIdHandle, FOCVars.IqdErr.d);PidIqHandle.hKpGain = PidIdHandle.hKpGain;PidIqHandle.hKiGain = PidIdHandle.hKiGain;FOCVars.Vqd.q = PI_Controller(&PidIqHandle, FOCVars.IqdErr.q);}//mc_statemachine_process(&mc_openloop,&STO_LBG,&FOCVars,sensor_peripheral.pVbusPar.glVBus);//放在這里主要是可以重置FOCVars.Vqd.qmc_statemachine_process(&mc_openloop,&STO_LBG,&FOCVars,24000);//放在這里主要是可以重置FOCVars.Vqd.q}#if 1//STO_CalcElAngle(&FOCVars,sensor_peripheral.pVbusPar.glVBus);glDebugTestD[12] = STO_Get_Speed(&STO_LBG)*sensor_peripheral.Encoder_Sensor.encRes/(pMotorParSet.tBasePar.poles[A_AXIS] * 2 * PI);//we->rpm->plusglDebugTestD[13] = STO_Get_Electrical_Angle(&STO_LBG);glDebugTestD[16] = FOCVars.hElAngle;
#endifFOCVars.Vqd = Circle_LimitationFunc(&FOCVars.CircleLimitationFoc, FOCVars.Vqd); //340 pointFOCVars.Valphabeta = Rev_Park(FOCVars.Vqd, FOCVars.hElAngle); //TODO: USING COSA COSB calc infront.....FOCVars.DutyCycle = SVPWM_3ShuntCalcDutyCycles(&FOCVars); pMotorParSet.va[A_AXIS] = MID_PWM_CLK_PRD - FOCVars.DutyCycle.CntPhA;pMotorParSet.vb[A_AXIS] = MID_PWM_CLK_PRD - FOCVars.DutyCycle.CntPhB;pMotorParSet.vc[A_AXIS] = MID_PWM_CLK_PRD - FOCVars.DutyCycle.CntPhC;
}
實際效果(約100rpm)
目前代碼還有優化空間:實現正反轉(反轉先降速切開環,反向開環拖動,最后切閉環)