devbox
菜鸟追梦旅行
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

永磁同步电机无感FOC（龙伯格观测器）算法技术总结-实战篇_电机算法观测器

作者：菜鸟追梦旅行 | 2024-05-27 05:03:40

赞

踩

电机算法观测器

文章目录

龙伯格观测+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 ****/

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138

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 procedure
	pHandle->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/s
	while (htempk != 0)
	{
		htempk /= 2;
		pHandle->STO_Observer.hF3 *= 2;
	}
	pHandle->STO_Observer.hC6 = (s16)((F2 * pHandle->STO_Observer.hF3 * 2 * PI)/65536);//10000
	
	pHandle->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;
#endif
	wSpeed_PI_proportional_term = pHandle->STO_Speed.hSpeed_P_Gain * wSpeed_PI_error;  // !!!p
	wSpeed_PI_integral_term = pHandle->STO_Speed.hSpeed_I_Gain * wSpeed_PI_error;      // !!!i

	if ( (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 variance
	wAverageQuadraticError= wAverageQuadraticError >> BUF_POW2;

	//The maximum variance acceptable is here calculated as ratio of average speed
	wAverage_Speed = (s32)(wAverage_Speed * wAverage_Speed);
	wAverage_Speed = (wAverage_Speed >> 7) * pHandle->STO_Speed.hPercentageFactor;

#if 0 // for debug only
	QuadraticError = wAverageQuadraticError;
	AverageSpeed = wAverage_Speed;
#endif

	if (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*emf
	wBemf_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 ****/

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268
  • 269
  • 270
  • 271
  • 272
  • 273
  • 274
  • 275
  • 276
  • 277
  • 278
  • 279
  • 280
  • 281
  • 282
  • 283
  • 284
  • 285
  • 286
  • 287
  • 288
  • 289
  • 290
  • 291
  • 292
  • 293
  • 294
  • 295
  • 296
  • 297
  • 298
  • 299
  • 300
  • 301
  • 302
  • 303
  • 304
  • 305
  • 306
  • 307
  • 308
  • 309
  • 310
  • 311
  • 312
  • 313
  • 314
  • 315
  • 316
  • 317
  • 318
  • 319
  • 320
  • 321
  • 322
  • 323
  • 324
  • 325
  • 326
  • 327
  • 328
  • 329
  • 330
  • 331
  • 332
  • 333
  • 334
  • 335
  • 336
  • 337
  • 338
  • 339
  • 340
  • 341
  • 342
  • 343
  • 344
  • 345
  • 346
  • 347
  • 348
  • 349
  • 350
  • 351
  • 352
  • 353
  • 354
  • 355
  • 356
  • 357
  • 358
  • 359
  • 360
  • 361
  • 362
  • 363
  • 364
  • 365
  • 366
  • 367
  • 368
  • 369
  • 370
  • 371
  • 372
  • 373
  • 374
  • 375
  • 376
  • 377
  • 378
  • 379
  • 380
  • 381
  • 382
  • 383
  • 384
  • 385
  • 386
  • 387
  • 388
  • 389
  • 390
  • 391
  • 392
  • 393
  • 394
  • 395
  • 396
  • 397
  • 398
  • 399
  • 400
  • 401
  • 402
  • 403
  • 404
  • 405
  • 406
  • 407
  • 408
  • 409
  • 410
  • 411
  • 412
  • 413
  • 414
  • 415
  • 416
  • 417
  • 418
  • 419
  • 420
  • 421
  • 422
  • 423
  • 424
  • 425
  • 426
  • 427
  • 428
  • 429
  • 430
  • 431
  • 432
  • 433
  • 434
  • 435
  • 436
  • 437
  • 438
  • 439
  • 440
  • 441
  • 442
  • 443
  • 444
  • 445
  • 446
  • 447
  • 448
  • 449
  • 450
  • 451
  • 452
  • 453
  • 454
  • 455
  • 456
  • 457
  • 458
  • 459
  • 460
  • 461
  • 462
  • 463
  • 464
  • 465
  • 466
  • 467
  • 468
  • 469
  • 470
  • 471
  • 472
  • 473
  • 474
  • 475
  • 476
  • 477
  • 478
  • 479
  • 480
  • 481
  • 482
  • 483
  • 484
  • 485
  • 486
  • 487
  • 488
  • 489

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.0

typedef 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 ****/

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104

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 angle
			popen->speed_loop_enable = FALSE;
			popen->current_loop_enable = FALSE;
			#else
			popen->speed_loop_enable = TRUE;
			popen->current_loop_enable = TRUE;
			#endif

			popen->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 ****/

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201

3.3 初始化及函数调用

定义全局变量：

STO_luenberger        STO_LBG;     //龙伯格观测器相关变量
mc_openloop_t         mc_openloop; //三段式启动相关变量

  • 1
  • 2

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);
	}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41

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.5us
	    pAxisPar.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.5us
	    pAxisPar.vel[A_AXIS][1] = (long) ftempll;
	    pAxisPar.vel[A_AXIS][0] = pAxisPar.vel[A_AXIS][1];
	
	}
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

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 result
    FOCVars.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.q
				mc_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->plus
    glDebugTestD[13] = STO_Get_Electrical_Angle(&STO_LBG);
    glDebugTestD[16] = FOCVars.hElAngle;
#endif
	  
		FOCVars.Vqd = Circle_LimitationFunc(&FOCVars.CircleLimitationFoc, FOCVars.Vqd); //340 point
		FOCVars.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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54

实际效果（约100rpm）
在这里插入图片描述
目前代码还有优化空间：实现正反转（反转先降速切开环，反向开环拖动，最后切闭环）

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/菜鸟追梦旅行/article/detail/629994
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号