FOC 位置环S曲线之定点查表算法

对于curve_init函数的参数：

• num生成数组的个数。
  

  越大越平缓，电机调整速度越慢；越小越陡峭，电机调整速度快

• range:在[-range,range]范围内取值
  • 注意：range取7的时候值域已接近于(0,1),故建议range<=7,若太大则后面太平缓,几乎没有改变

2、位置环结构体

typedef struct
{
	PID_Handle_t *PIPos;
	int16_t  hSetPosition;   //设定机械位置
	int16_t  hStartPosition; //起始机械位置
	int16_t  hCurPosition;   //当前机械位置
	int16_t  hError;         //初始机械位置与目标位置的差
	int16_t  hMaxTorque;     //最大力矩
	uint16_t hCurIndex;      //当前S曲线表的索引位置
	uint16_t hStep;          //S曲线调整的时间
	uint8_t  bIsChanging;    //正在调整位置
} PosControl_Handle_t;

3、位置控制函数

int16_t Pos_CalcTorqueReference( PosControl_Handle_t * pHandle,SpeednPosFdbk_Handle_t * SpeedPos)
{
  int16_t hError;
  int16_t hTorqueReference;
  
  hError = SpeedPos->hMecPosition - pHandle->hCurPosition;

  hTorqueReference = PID_Controller( pHandle->PIPos,(int32_t )hError);
  if(hTorqueReference < 0)
  {	
     if(hTorqueReference < -pHandle->hMaxTorque)
			 hTorqueReference = -pHandle->hMaxTorque;
  }
  else
  {
     if(hTorqueReference > pHandle->hMaxTorque)
			 hTorqueReference = pHandle->hMaxTorque;
  };
  return hTorqueReference;
}

#define NEXT_STEP_ERR 50
void Pos_Control(PosControl_Handle_t * pHandle,SpeednPosFdbk_Handle_t * SpeedPos)
{
	static uint16_t POSCNT = 0;
	int16_t delta;
	if(pHandle->bIsChanging)
	{
		delta = ENCODER_M._Super.hMecPosition - pHandle->hCurPosition;
		/* 每hStep次中断更改一次位置 */
		POSCNT = (POSCNT+1) % pHandle->hStep;
		if(POSCNT == 0 || delta < NEXT_STEP_ERR)
		{
			/* 计算下一次调整的中间位置 */
			pHandle->hCurPosition = pHandle->hStartPosition + ((S_PROFILE[pHandle->hCurIndex]*pHandle->hError)>>16);
			/* 调整位置总共需要S_TOTAL次 */
			pHandle->hCurIndex = (pHandle->hCurIndex+1) % S_TOTAL;
			/* 调整结束,设置位置为目标位置 */
			if(pHandle->hCurIndex == 0)
			{
				pHandle->hError = pHandle->bIsChanging = 0;
				//pHandle->hCurPosition = pHandle->hSetPosition;
			}
		}
	}

	FOCVars.hTeref = Pos_CalcTorqueReference(pHandle,SpeedPos);//计算参考转矩
}

• Pos_Control函数在中频任务中被调用
• NEXT_STEP_ERR为预期位置和实际位置的误差，小于这个值就认为位置已经收敛，进入S曲线的下一个step。
  

  否则在S曲线的一些平缓的位置，位置可能提前收敛，电机就会轻微地来回抖动。
  

4、位置位置API

void POS_SetPos(int16_t pos)
{
	PosCtrlM.hSetPosition = pos;
	PosCtrlM.hStartPosition = ENCODER_M._Super.hMecPosition;
	PosCtrlM.hError = PosCtrlM.hSetPosition - PosCtrlM.hStartPosition;
	PosCtrlM.bIsChanging = 1;
	PosCtrlM.hCurIndex = 0;
}

        这个方法的弊端就是无法精确控制旋转的时间，有最低速度的限制。

整体来说没什么难度，参考一下就行了，一般不用在实际工业控制中。

        后续将介绍一种恒定Jerk(加加速度，即)的S曲线算法，已经实现，但是由于与毕设内容相关，暂不发布。

此时速度-时间曲线为马鞍形，角度-时间曲线为S型，这种方法可以精确控制位置环的时间。