手搓FOC-环路激励的实现

前言

本文是手搓FOC功能里面的一种-环路激励功能：

• 主要用于环路PID参数调节
• 支持方波和正弦波
• 模块化调用

另外还有一些核心功能：

• 高速数据传输
• 实时动态示波器（10KHz）
• 速度JOG
• 位置JOG（仿汇川）
• 转矩JOG

伺服上位机展示

直接上代码：
refinject.h

/********************************************************************************* @file    refinject.h* @author  hlping* @version V1.0.0* @date    2025-07-23* @brief   ******************************************************************************* @attention********************************************************************************/ #ifndef __REFINJECT_H
#define __REFINJECT_H/* *INDENT-OFF* */
#ifdef __cplusplus
extern "C"
{
#endif
/* *INDENT-ON* *//* Includes -----------------------------------------------------------------*/
#include "main.h"/* Defines -------------------------------------------------------------------*/
/* Injections of signals into the control loop */
#define INJECT_TYPE_NONE             0x00
#define INJECT_TYPE_SIN_DIRECT       0x01
#define INJECT_TYPE_SQUARE_DIRECT    0x02
#define INJECT_TYPE_LAST             0x03#define INJECT_POINT_CUR             0x00
#define INJECT_POINT_VEL             0x01
#define INJECT_POINT_POS             0x02
#define INJECT_POINT_LAST            0x03#define LOOP_CALC_NONE               0x00
#define LOOP_CALC_ACR_SET            0x01
#define LOOP_CALC_ASR_SET            0x02
#define LOOP_CALC_APR_SET            0x04
#define LOOP_CALC_ALL                0x07/* Typedefs -------------------------------------------------------------------*/
typedef struct 
{uint8_t swEn;                      //开关uint8_t injectType;                //激励类型uint16_t injectFreq;               //频率float injectAmp;                //幅值float fInjectValue;float fAmpCoef;float fInjectInit;
}ref_loop_t;typedef struct 
{uint8_t loop_type;                //环路类型 0：cur  1:vel  2:posuin16_t freq;    float fSamptime; ref_loop_t cur_loop;ref_loop_t vel_loop;ref_loop_t pos_loop;   uint16_t loopMask;    float  fAmpMax;float  fInjectPhase;float  fPhaseCycle;
}refinject_t;/* *INDENT-OFF* */
#ifdef __cplusplus
extern "C"
{
#endif
/* *INDENT-ON* */#endif /* __REFINJECT_H *//**** Copyright (C)2021 xxx Tech Co.Ltd. All Rights Reserved **** END OF FILE ****/

refinject.c

/********************************************************************************* @file    refinject.c* @author  hlping* @version V1.0.0* @date    2025-07-23* @brief   ******************************************************************************* @attention********************************************************************************//* Includes ------------------------------------------------------------------*/
#include "refinject.h"
#include "global.h"
/* Private Defines ---------------------------------------------------------*/
refinject_t refinject;/* Function prototypes -----------------------------------------------------*/
void RefinJectInit(void)
{refinject.cur_loop.fInjectValue = 0;refinject.vel_loop.fInjectValue = 0;refinject.pos_loop.fInjectValue = 0;refinject.cur_loop.injectAmp = 0.1f;refinject.vel_loop.injectAmp = 100;refinject.pos_loop.injectAmp = 8000;refinject.cur_loop.swEn = 0;refinject.vel_loop.swEn = 0;refinject.pos_loop.swEn = 0;refinject.cur_loop.injectType = INJECT_TYPE_SIN_DIRECT;refinject.vel_loop.injectType = INJECT_TYPE_SIN_DIRECT;refinject.pos_loop.injectType = INJECT_TYPE_SIN_DIRECT;refinject.fAmpMax = 6.28f;//2pirefinject.fSamptime = SAMPLE_TIME;refinject.fInjectPhase = 0;refinject.fPhaseCycle = 0;refinject.loop_type = INJECT_POINT_CUR;
}void RefinJectReset(refinject_t *pOv)
{pOv->fInjectPhase = 0;pOv->loopMask = LOOP_CALC_ALL;pOv->cur_loop.fInjectValue = 0;pOv->vel_loop.fInjectValue = 0;pOv->pos_loop.fInjectValue = 0;pOv->cur_loop.swEn = 0;pOv->vel_loop.swEn = 0;pOv->pos_loop.swEn = 0;pOv->cur_loop.injectType = INJECT_TYPE_SIN_DIRECT;pOv->vel_loop.injectType = INJECT_TYPE_SIN_DIRECT;pOv->pos_loop.injectType = INJECT_TYPE_SIN_DIRECT;pOv->cur_loop.fInjectInit = 0;pOv->vel_loop.fInjectInit = 0;pOv->pos_loop.fInjectInit = motor.motor_sensor.encoder.pos;
}void RefinJectUpdate(void *pData)
{refinject_t *pOv = (refinject_t *)pData;if (pOv->loop_type == INJECT_POINT_CUR && pOv->cur_loop.swEn){pOv->fPhaseCycle = CUR_LOOP_CNT*pOv->fAmpMax * pOv->fSamptime *pOv->cur_loop.injectFreq;pOv->fInjectPhase +=pOv->fPhaseCycle;if (pOv->fInjectPhase > pOv->fAmpMax){pOv->fInjectPhase -= pOv->fAmpMax;}pOv->cur_loop.fInjectValue = RefinJectCalc(pOv->cur_loop);pOv->loopMask = LOOP_CALC_ACR_SET;}else if(pOv->loop_type == INJECT_POINT_VEL && pOv->vel_loop.swEn){pOv->fPhaseCycle = VEL_LOOP_CNT*pOv->fAmpMax * pOv->fSamptime *pOv->vel_loop.injectFreq;pOv->fInjectPhase +=pOv->fPhaseCycle;if (pOv->fInjectPhase > pOv->fAmpMax){pOv->fInjectPhase -= pOv->fAmpMax;}pOv->vel_loop.fInjectValue = RefinJectCalc(pOv);pOv->loopMask = LOOP_CALC_ASR_SET;}else if(pOv->loop_type == INJECT_POINT_POS && pOv->pos_loop.swEn){pOv->fPhaseCycle = POS_LOOP_CNT*pOv->fAmpMax * pOv->fSamptime *pOv->pos_loop.injectFreq;pOv->fInjectPhase +=pOv->fPhaseCycle;if (pOv->fInjectPhase > pOv->fAmpMax){pOv->fInjectPhase -= pOv->fAmpMax;}pOv->pos_loop.fInjectValue = RefinJectCalc(pOv);pOv->loopMask = LOOP_CALC_APR_SET;}else{RefinJectReset(pOv);}
}float RefinJectCalc(ref_loop_t *pOv)
{float output = 0.0f;switch (pOv->injectType){case INJECT_TYPE_NONE:{refinject.fInjectPhase = 0.0fif (refinject.loop_type == INJECT_POINT_POS){refinject.InjectInit = refinject.pos_loop.fInjectValue;}}break;case INJECT_TYPE_SIN_DIRECT:{pOv->fInjectValue = pOv->injectAmp * sinf(refinject.fInjectPhase);output = pOv->fInjectValue + pOv->fInjectInit;}break;case INJECT_TYPE_SQUARE_DIRECT:{if (pOv->fInjectInit < refinject.fAmpMax * 0.5f){pOv->fInjectValue = pOv->injectAmp * 0.5f;}else{pOv->fInjectValue = - pOv->injectAmp * 0.5f;}output = pOv->fInjectValue + pOv->fInjectInit;}break;default:break;}}void RefinJectSwitch(refinject_t *pOv,bool sw)
{if (pOv->loop_type == INJECT_POINT_CUR){pOv->cur_loop.sw = sw;pOv->vel_loop.sw = G_FLASE;pOv->pos_loop.sw = G_FLASE;}if (pOv->loop_type == INJECT_POINT_VEL){pOv->cur_loop.sw = G_FLASE;pOv->vel_loop.sw = sw;pOv->pos_loop.sw = G_FLASE;}if (pOv->loop_type == INJECT_POINT_POS){pOv->cur_loop.sw = G_FLASE;pOv->vel_loop.sw = G_FLASE;pOv->pos_loop.sw = sw;}pOv->cur_loop.fInjectInit = 0;pOv->vel_loop.fInjectInit = 0;pOv->pos_loop.fInjectInit = motor.motor_sensor.encoder.pos;
}void RefinjectSetLoopType(uint8_t type)
{if (type>3)refinject->loop_type=INJECT_POINT_POS;refinject->loop_type=type;
}uint8_t RefinjectGetLoopType(void)
{return refinject->loop_type;
}void RefinjectSetDirectType(uint8_t type)
{if(type>=3)type=INJECT_TYPE_SQUARE_DIRECT;if (refinject.loop_type == INJECT_POINT_CUR){refinject.cur_loop.injectType=type;}if (refinject.loop_type == INJECT_POINT_VEL){refinject.vel_loop.injectType=type;}if (refinject.loop_type == INJECT_POINT_POS){refinject.pos_loop.injectType=type;}}void RefinjectSetAmp(float amp)
{if (refinject.loop_type == INJECT_POINT_CUR){refinject.cur_loop.injectAmp=amp;}if (refinject.loop_type == INJECT_POINT_VEL){refinject.vel_loop.injectAmp=amp;}if (refinject.loop_type == INJECT_POINT_POS){refinject.pos_loop.injectAmp=amp;}
}void RefinjectSetFreq(uint16_t freq)
{if (refinject.loop_type == INJECT_POINT_CUR){refinject.cur_loop.injectFreq=freq;}if (refinject.loop_type == INJECT_POINT_VEL){refinject.vel_loop.injectFreq=freq;}if (refinject.loop_type == INJECT_POINT_POS){refinject.pos_loop.injectFreq=freq;}}float RefinjectGetRefValue(void)
{float res;if (refinject.loop_type == INJECT_POINT_CUR){res = refinject.cur_loop.fInjectValue;}if (refinject.loop_type == INJECT_POINT_VEL){res = refinject.vel_loop.fInjectValue;}if (refinject.loop_type == INJECT_POINT_POS){res = refinject.pos_loop.fInjectValue;}return res;
}/**** Copyright (C)2025 xxx Tech Co.Ltd. All Rights Reserved **** END OF FILE ****/