#define	_IN_MOVECTRL_C

#include "movectrl.h"

//----------------------------------------------------------------------------------------------------

#include "delay.h"
#include "trigger.h"
#include "inout.h"
#include "shell.h"
#include "error.h"
#include "workctrl.h"

#ifndef IsStopButton
#define	IsStopButton	GetInputNullOff
#endif


//----------------------------------------------------------------------------------------------------

__weak void NormalRefPosition(u32 para, s32 * posilist)
{
}

//----------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------

// 紧急停止
int GetCommonEms(void)
{
	int err = ERR_NONE;

	TasksWhenDelay();

	return err;
}

// 快速停止
int GetCommonQStop(void)
{
	int err = ERR_NONE;

	return err;
}

// 普通停止
int GetCommonNStop(void)
{
	int err = ERR_NONE;

#if (0)
	if (err == ERR_NONE && 
		IsConsoleCancel() != 0 &&				// 终端输入停止
		1	)
	{
		printf("get console cancel\r\n");
		err = STA_NORMAL_STOP;
	}
#endif

#if (1)
	if (err == ERR_NONE && 
		IsStopButton() == SENSOR_ON &&			// 检测暂停按钮按下
		1	)
	{
		printf("get stop button down\r\n");
		err = STA_NORMAL_STOP;
	}
#endif

	return err;
}


//----------------------------------------------------------------------------------------------------

// 周期性运动的零位检测（归零）
#if (1)

#define	ADJ_RUN_MVMT(cm)	((cm)*3)
#define	ADJ_FAN_MVMT(cm)	((cm)*36/360)
#define	CANC_MVMT(cm)		((cm)*288/360)

#define	ADJ_WAVE_MVMT(cm)	((cm)*10/360)
#define	ADJ_WVC_MVMT(cm)	((cm)*1/360)


// 急停控制
int RSensorEmsScan(u32 para1, u32 para2)
{
	int rslt = GetCommonEms();

	return rslt;
}

// 快速停止控制
int RSensorQStopScan(u32 para1, u32 para2)
{
	int rslt;
	RSenserCheck * pchk;

	rslt = GetCommonQStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}

	pchk = (RSenserCheck *)para1;
	if (pchk == NULL)
	{
		return STA_NORMAL_STOP;
	}

	if (pchk->zeroflag == 0)				// 如果没有经过传感器位置
	{
		pchk->cansflag = 0;
		if (pchk->sensorscan != NULL)
		{
			if (pchk->sensorscan() == pchk->val)	// 检测到传感器位置
			{
//				printf("1. first in sensor area when run\r\n");
				pchk->zeroflag = 1;		// 设置经过传感器标志
				pchk->canscount = g_motosPara.motosPos[pchk->axisIdx];
			}
		}
	}
	else
	{
		if (pchk->cansflag == 0)			// 如果没有跳过传感器位置
		{
			if (pchk->sensorscan != NULL)
			{
				if (pchk->sensorscan() != pchk->val)	// 检测非传感器位置
				{
					if (abs(g_motosPara.motosPos[pchk->axisIdx] - pchk->canscount) > pchk->cansmvmt)
					{
//						printf("1. first out zero area when run\r\n");
						pchk->cansflag = 1;
					}
				}
			}
		}
		else
		{
			if (pchk->sensorscan != NULL)
			{
				if (pchk->sensorscan() == pchk->val)	// 检测传感器位置
				{
//					printf("1. in sensor area when run\r\n");
					rslt = STA_NORMAL_STOP;
				}
			}
		}
	}

	return rslt;
}

// 普通停止控制
int RSensorNStopScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}

	return rslt;
}

//------------------------------------------------------------------

// 控制流程,运动到传感器
int RoundRunToSensor(RSenserCheck * pck)
{
	return RoundRunToSensorBase(pck, 0);
}

// 运动到传感器边缘
int RoundRunToSensorEdge(RSenserCheck * pck)
{
	return RoundRunToSensorBase(pck, 1);
}

// 运动到传感器中心
int RoundRunToSensorCenter(RSenserCheck * pck)
{
	return RoundRunToSensorBase(pck, 2);
}

// 控制流程
// flag,找零成功后转动到哪个位置	0,无动作		1,转动到边缘位置		2,转动到中心位置
int RoundRunToSensorBase(RSenserCheck * pck, int flag)
{
	int rslt, err, i;
	InterMoveCtrl lmctrl;
	u16 axisIdx;
	u32 interLong;

	if (pck == NULL)
	{
		return -1;
	}

	axisIdx = pck->axisIdx;
	assert_param(axisIdx < AXIS_NUM);

	memset(&lmctrl, 0, sizeof(InterMoveCtrl));

	lmctrl.vAxisId = pck->vAxisId;			// 使用虚轴
	lmctrl.motosConfig.newConfig = pck->newCfg;								// 需要更新电机配置
	pck->cansmvmt = CANC_MVMT(pck->circleNum);

	if (1	&&
		pck->newCfg == 0 && 
		pck->syncflag != 0 &&
		1	)
	{
		for (i = 0; i < AXIS_NUM; i++)
		{
			if (pck->syncList[i] != 0)
			{
				lmctrl.mvmtPara.movement[i] = ADJ_RUN_MVMT(pck->circleNum);
			}
		}
	}
	else
	{
		lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pck->axisCfg;		// 实轴输出配置
		lmctrl.motosConfig.axisConfig[axisIdx].poutType = pck->axisOtp;			// 脉冲输出模式(CW/CCW，PULSE/DIR)
		lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
		lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)

		lmctrl.mvmtPara.movement[axisIdx] = ADJ_RUN_MVMT(pck->circleNum);		// 各个轴的运动量，0，无运动；其他，运动长度，单位p
	}

	interLong = abs(ADJ_RUN_MVMT(pck->circleNum));
	lmctrl.mvmtPara.extraRepeat = 0;							// 延续重复次数
	lmctrl.mvmtPara.interLong = interLong;						// 插补长轴位移，必须大于或等于movement分量中的最大值
	lmctrl.mvmtPara.pulsePerSegment = abs(pck->circleNum);		// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pck->rspd.stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pck->rspd.stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pck->rspd.runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pck->rspd.runSpd;		// 降速速度
	lmctrl.spdPara.addPPSG = pck->rspd.spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pck->rspd.spdAdd;		// 降速加速度
	lmctrl.spdPara.brkPPSG = pck->rspd.brkAdd;		// 刹车加速度

	lmctrl.stopMode = STOP_MODE_SPD;				// 停止运动方式

	// 回调函数及参数
	lmctrl.Delay = DelayRef;						// 延时回调函数
	lmctrl.RefreshPosition = NormalRefPosition;		// 坐标刷新回调函数
	lmctrl.refPosPara = 0;							// 坐标刷新参数
	lmctrl.WorkBeforeRun = NULL;					// 运行前工作回调函数
	lmctrl.WorkAfterRun = NULL;						// 运行后工作回调函数
	lmctrl.workPara1 = 0;
	lmctrl.workPara2 = 0;							// 工作参数
	lmctrl.GetSlowDown = NULL;						// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.GetNormalStop = RSensorNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	lmctrl.GetQuickStop = RSensorQStopScan;			// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetEmergencyStop = RSensorEmsScan;		// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.condPara1 = (u32)(pck);;
	lmctrl.condPara2 = 0;							// 条件函数参数

	rslt = InterpolationMotion(&lmctrl);			// 运动控制

	if (rslt == 3)		// 条件停车
	{
		DelayRef(100);	// 等待机械稳定

		// 重新检查传感器位置
		if (pck->sensorscan != NULL)
		{
			if (pck->sensorscan() != pck->val)	// 如果传感器不在位置，那么反向慢速转动到位置
			{
				printf("not at sensor\r\n");
				if (
					pck->newCfg == 0 && 
					pck->syncflag != 0	)
				{
					for (i = 0; i < AXIS_NUM; i++)
					{
						if (pck->syncList[i] != 0)
						{
							lmctrl.mvmtPara.movement[i] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
						}
					}
				}
				else
				{
					lmctrl.mvmtPara.movement[axisIdx] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
				}

				lmctrl.motosConfig.newConfig = 0;

				rslt = InterpolationMotion(&lmctrl);				// 运动控制
			}
		}
	}

	err = GetMoveResultError(rslt, lmctrl.errInfo);	

#if (1)
	if (flag != 0)
	{
		// 摆动找到中心位置
		if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
				pck->sensorscan() == pck->val &&	// 在传感器位置
				1	)
		{
//			printf("move to edge\r\n");
			int gapcount, pcount;

			// 取反
			if (pck->val == SENSOR_ON)
			{
				pck->val = SENSOR_OFF;
			}
			else
			{
				pck->val = SENSOR_ON;
			}

			// 速度控制参数,使用慢速速度
			lmctrl.spdPara.startPPS = pck->rspd.stspSpd/2;					// 启动速度
			lmctrl.spdPara.stopPPS = pck->rspd.stspSpd/2;					// 停止速度
			lmctrl.spdPara.runPPS = pck->rspd.runSpd/2;						// 运行速度
			lmctrl.spdPara.slowPPS = pck->rspd.runSpd/2;					// 降速速度
			lmctrl.spdPara.addPPSG = pck->rspd.spdAdd;		// 升速加速度
			lmctrl.spdPara.decPPSG = pck->rspd.spdAdd;		// 降速加速度
			lmctrl.spdPara.brkPPSG = pck->rspd.brkAdd;		// 刹车加速度

			if (
					pck->newCfg == 0 &&
					pck->syncflag != 0	)
			{
				for (i = 0; i < AXIS_NUM; i++)
				{
					if (pck->syncList[i] != 0)
					{
						lmctrl.mvmtPara.movement[i] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
					}
				}
			}
			else
			{
				lmctrl.mvmtPara.movement[axisIdx] = ADJ_FAN_MVMT(pck->circleNum)*(-1);
			}

			pck->cansflag = 0;
			pck->cansmvmt = ADJ_WVC_MVMT(pck->circleNum);

			lmctrl.motosConfig.newConfig = 0;

			rslt = InterpolationMotion(&lmctrl);				// 运动控制, 运动到边缘
			err = GetMoveResultError(rslt, lmctrl.errInfo);	

#if (1)
			if (flag == 2)
			{
				if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
						pck->sensorscan() == pck->val &&
						1	)
				{
					pcount = GetMotoCounter(axisIdx);		// 读取位置

					// 正向
					if (
							pck->newCfg == 0 &&
							pck->syncflag != 0)
					{
						for (i = 0; i < AXIS_NUM; i++)
						{
							if (pck->syncList[i] != 0)
							{
								lmctrl.mvmtPara.movement[i] = ADJ_FAN_MVMT(pck->circleNum);
							}
						}
					}
					else
					{
						lmctrl.mvmtPara.movement[axisIdx] = ADJ_FAN_MVMT(pck->circleNum);
					}

					pck->cansflag = 0;
					pck->cansmvmt = ADJ_WVC_MVMT(pck->circleNum);

					rslt = InterpolationMotion(&lmctrl);				// 运动控制, 运动到另外的边缘
					err = GetMoveResultError(rslt, lmctrl.errInfo);

					if ((err == STA_NORMAL_STOP || err == ERR_NONE) &&
							pck->sensorscan() == pck->val &&
							1	)
					{
						gapcount = abs(GetMotoCounter(axisIdx) - pcount);		// 读取位置
						gapcount /= 2;

						if (gapcount != 0)
						{
							if (
									pck->newCfg == 0 &&
									pck->syncflag != 0	)
							{
								for (i = 0; i < AXIS_NUM; i++)
								{
									if (pck->syncList[i] != 0)
									{
										lmctrl.mvmtPara.movement[i] = gapcount*(-1);
									}
								}
							}
							else
							{
								lmctrl.mvmtPara.movement[axisIdx] = gapcount*(-1);
							}

							lmctrl.GetNormalStop = NULL;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
							lmctrl.GetQuickStop = NULL;			// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
							lmctrl.stopMode = STOP_MODE_TAB;	// 停止运动方式

							lmctrl.spdPara.startPPS = pck->rspd.runSpd;	// 启动速度
							lmctrl.spdPara.stopPPS = pck->rspd.runSpd;	// 停止速度
							lmctrl.spdPara.runPPS = pck->rspd.runSpd;	// 运行速度

							rslt = InterpolationMotion(&lmctrl);				// 运动控制
							err = GetMoveResultError(rslt, lmctrl.errInfo);
						}
					}
				}
			}
#endif
		}
	}
#endif

	return err;
}

//------------------------------------------------------------------

// 急停控制
int RMotoRunEmsScan(u32 para1, u32 para2)
{
	RMotoRun * pmr;
	int rslt = GetCommonEms();
	pmr = (RMotoRun *)para1;

	if (pmr->RRunProc != NULL)
	{
		pmr->RRunProc();
	}

	return rslt;
}

// 快速停止控制
int RMotoRunQStopScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonQStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}

	return rslt;
}

// 普通停止控制
int RMotoRunNStopScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}

	return rslt;
}

// 按偏移量运动
int RoundRunAsOffset(RMotoRun * pmr)
{
	int rslt, err, i;
	InterMoveCtrl lmctrl;
	int axisIdx;
	int movmt;

	if (pmr == NULL)
	{
		return ERR_MV_PARA;
	}

	movmt = 0;

	memset(&lmctrl, 0, sizeof(InterMoveCtrl));

	lmctrl.vAxisId = pmr->vAxisId;					// 使用虚轴
	lmctrl.motosConfig.newConfig = pmr->newCfg;		// 需要更新电机配置
	for (i = 0; i < AXIS_NUM; i++)
	{
		axisIdx = pmr->config[i].axisIdx;
		if (axisIdx >= 0)
		{
			assert_param(axisIdx < AXIS_NUM);

			lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmr->config[i].axisCfg;			// 实轴输出配置
			lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmr->config[i].poutType;			// 脉冲输出模式(CW/CCW，PULSE/DIR)
			lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
			lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
			lmctrl.mvmtPara.movement[axisIdx] = pmr->config[i].movement;						// 各个轴的运动量，0，无运动；其他，运动长度，单位p
			if (pmr->config[i].movement != 0)
			{
				movmt = pmr->config[i].movement;
			}
		}
	}

//	printf("round run as offset, movmt=%d\r\n", movmt);
	
	if (movmt == 0)		// 无运动量
	{
		return ERR_NONE;
	}

	lmctrl.mvmtPara.extraRepeat = pmr->mrepeat;				// 延续重复次数
	lmctrl.mvmtPara.interLong = pmr->interLong;				// 插补长轴位移，必须大于或等于movement分量中的最大值
	lmctrl.mvmtPara.pulsePerSegment = pmr->segmentNum;		// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pmr->rspd.stspSpd;
	lmctrl.spdPara.stopPPS = pmr->rspd.stspSpd;
	lmctrl.spdPara.slowPPS = pmr->rspd.stspSpd;
	lmctrl.spdPara.runPPS = pmr->rspd.runSpd;
	lmctrl.spdPara.addPPSG = pmr->rspd.spdAdd;
	lmctrl.spdPara.decPPSG = pmr->rspd.spdAdd;
	lmctrl.spdPara.brkPPSG = pmr->rspd.brkAdd;		// 停止加速度

	lmctrl.stopMode = STOP_MODE_TAB;				// 停止运动方式

	// 回调函数及参数
	lmctrl.Delay = DelayRef;						// 延时回调函数
	lmctrl.RefreshPosition = NormalRefPosition;		// 坐标刷新回调函数
	lmctrl.refPosPara = 0;							// 坐标刷新参数
	lmctrl.WorkBeforeRun = NULL;					// 运行前工作回调函数
	lmctrl.WorkAfterRun = NULL;						// 运行后工作回调函数
	lmctrl.workPara1 = 0;
	lmctrl.workPara2 = 0;							// 工作参数
	lmctrl.GetSlowDown = NULL;						// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复

	if (pmr->escStop != 0)
	{
//		printf("stop condition is null\r\n");
		lmctrl.GetNormalStop = NULL;
		lmctrl.GetQuickStop = NULL;
	}
	else
	{
//		printf("stop condition is r move stop\r\n");
		lmctrl.GetNormalStop = RMotoRunNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
		lmctrl.GetQuickStop = RMotoRunQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	}

	lmctrl.GetEmergencyStop = RMotoRunEmsScan;		// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.condPara1 = (u32)(pmr);;
	lmctrl.condPara2 = 0;							// 条件函数参数

	rslt = InterpolationMotion(&lmctrl);		// 运动控制

	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}

#endif

//----------------------------------------------------------------------------------------------------

#if (1)

// 急停检测
int MoveToSensorEmsScan(u32 para1, u32 para2)
{
	int rslt;
	MTSCtrl * pmts;

	pmts = (MTSCtrl*)(para1);

	rslt = GetCommonEms();

	if (rslt != ERR_NONE || pmts == NULL)
	{
		return rslt;
	}

	if (pmts->emcstopscan != NULL)
	{
		rslt = pmts->emcstopscan();
	}

	if (rslt != ERR_NONE)
	{
		return rslt;
	}

	if (pmts->movement > 0)
	{
		if (pmts->limitpsensor != NULL)		// 检测正向限位
		{
			if (pmts->limitpsensor() == SENSOR_ON)
			{
				rslt = ERR_LMT_POSITIVE;	// 正向限位
			}
		}
	}
	else if (pmts->movement < 0)
	{
		if (pmts->limitnsensor != NULL)		// 检测反向限位
		{
			if (pmts->limitnsensor() == SENSOR_ON)
			{
				rslt = ERR_LMT_NEGATIVE;	// 反向限位
			}
		}
	}
	else
	{
		rslt = ERR_MTZ_ERROR;	// 归零错误
	}

	return rslt;
}

// 普通停止检测
int MoveToSensorNStopScan(u32 para1, u32 para2)
{
	int rslt;
	MTSCtrl * pmts;

	pmts = (MTSCtrl*)(para1);

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE || pmts == NULL)
	{
		return rslt;
	}

	return rslt;
}

// 快速停止检测
int MoveToSensorQStopScan(u32 para1, u32 para2)
{
	int rslt;
	MTSCtrl * pmts;

	pmts = (MTSCtrl*)(para1);

	rslt = GetCommonQStop();
	if (rslt != ERR_NONE || pmts == NULL)
	{
		return rslt;
	}

	if (rslt == ERR_NONE)
	{
		if (pmts->zerosensor != NULL)		// 检测零位
		{
			if (pmts->zerosensor() == pmts->zeroval)
			{
				rslt = STA_MTZ_SUCCESS;		// 归零成功
			}
		}
	}

	return rslt;
}

// 降速检测
int MoveToSensorSlowScan(u32 para1, u32 para2)
{
	int rslt;
	MTSCtrl * pmts;

	pmts = (MTSCtrl*)(para1);
	rslt = 0;

	if (pmts != NULL)
	{
		if (pmts->slowsensor != NULL)	// 检测降速
		{
			if (pmts->slowsensor() == SENSOR_ON)
			{
				pmts->slowsta = 1;
			}
		}
	}

	rslt = pmts->slowsta;	// 降速状态
	return rslt;
}

//------------------------------------------------
// 直线运动到传感器
int LineRunToSensorCtrl(MTSCtrl * pmtsc)
{
	int rslt, err, poismv;
	InterMoveCtrl lmctrl;
	u16 axisIdx;
	int i;

	if (pmtsc == NULL)
	{
		return -1;
	}

	axisIdx = pmtsc->axisIdx;
	assert_param(axisIdx < AXIS_NUM);

	memset(&lmctrl, 0, sizeof(InterMoveCtrl));

	lmctrl.vAxisId = pmtsc->vAxisId;			// 使用虚轴
	lmctrl.motosConfig.newConfig = pmtsc->newCfg;							// 需要更新电机配置

	if (
		pmtsc->newCfg == 0 && 
		pmtsc->syncflag != 0)
	{
		for (i = 0; i < AXIS_NUM; i++)
		{
			if (pmtsc->syncList[i] != 0)
			{
				lmctrl.mvmtPara.movement[i] = pmtsc->movement;	// 各个轴的运动量，0，无运动；其他，运动长度，单位p
			}
		}
	}
	else
	{
		lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmtsc->axisCfg;		// 实轴输出配置
		lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmtsc->poutType;		// 脉冲输出模式(CW/CCW，PULSE/DIR)
		lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
		lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)

		lmctrl.mvmtPara.movement[axisIdx] = pmtsc->movement;	// 各个轴的运动量，0，无运动；其他，运动长度，单位p
	}

	lmctrl.mvmtPara.extraRepeat = 0;						// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(pmtsc->movement);		// 插补长轴位移，必须大于或等于movement分量中的最大值
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;	// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pmtsc->lspd.stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pmtsc->lspd.stspSpd;	// 停止速度
	lmctrl.spdPara.runPPS = pmtsc->lspd.runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pmtsc->lspd.stspSpd;	// 降速速度
	lmctrl.spdPara.addPPSG = pmtsc->lspd.spdAdd;	// 升速加速度
	lmctrl.spdPara.decPPSG = pmtsc->lspd.spdAdd/2;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pmtsc->lspd.brkAdd;	// 刹车加速度

	lmctrl.stopMode = STOP_MODE_SPD;				// 停止运动方式

	// 回调函数及参数
	lmctrl.Delay = DelayRef;						// 延时回调函数
	lmctrl.RefreshPosition = NormalRefPosition;		// 坐标刷新回调函数
	lmctrl.refPosPara = 0;							// 坐标刷新参数
	lmctrl.WorkBeforeRun = NULL;					// 运行工作回调函数
	lmctrl.WorkAfterRun = NULL;						// 运行后工作回调函数
	lmctrl.workPara1 = 0;
	lmctrl.workPara2 = 0;							// 工作参数
	lmctrl.GetSlowDown = MoveToSensorSlowScan;		// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.GetNormalStop = MoveToSensorNStopScan;	// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	lmctrl.GetQuickStop = MoveToSensorQStopScan;	// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetEmergencyStop = MoveToSensorEmsScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.condPara1 = (u32)(pmtsc);
	lmctrl.condPara2 = 0;							// 条件函数参数

	// 检测条件
	err = ERR_NONE;
	poismv = 0;

#if (1)
	if (pmtsc->syncflag == 0)
	{
		err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测急停条件
		if ((pmtsc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
			(pmtsc->movement > 0 && err == ERR_LMT_POSITIVE)	)	// 在限位
		{
			poismv = pmtsc->mvposi;		// 反向运动
		}

		err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测停止条件
		if (err == STA_MTZ_SUCCESS)							// 在零位位置
		{
			poismv = pmtsc->mvposi;		// 反向运动
		}
		else if (err == ERR_NONE)
		{
			rslt = MoveToSensorSlowScan(lmctrl.condPara1, lmctrl.condPara2);	// 在降速区域
			if (rslt != ERR_NONE)
			{
				poismv = pmtsc->mvposi;	// 反向运动
			}
		}
		else
		{
		}
	}
#endif

	do
	{
		// 运动
		if (poismv != 0)
		{
			if (pmtsc->moveasoffset != NULL)
			{
				err = pmtsc->moveasoffset(poismv, &(pmtsc->lspd));	 // 运动一段距离
			}

			if (err == ERR_NONE)
			{
				// 重新检测
				err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2);			// 检测急停条件
				if (err == ERR_NONE)
				{
					err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2);	// 检测停止条件
					if (err == ERR_NONE)
					{
						pmtsc->slowsta = 0;
					}
				}
			}
		}

		if (err == ERR_NONE)
		{
			s32 temp, val;
			temp = GetMotoCounter(axisIdx);				// 电机位置

			rslt = InterpolationMotion(&lmctrl);		// 运动控制

			if (poismv == 0 && (rslt == 3 || rslt == 2))
			{
				if (rslt == 3)
				{
					err = MoveToSensorQStopScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测停止条件
					if (err == STA_MTZ_SUCCESS && pmtsc->mvposi != 0)
					{
						val = GetMotoCounter(axisIdx);
						if (abs(val-temp) < abs(pmtsc->mvposi/2))				// 如果归零行走位移太小
						{
							poismv = pmtsc->mvposi;	// 反向运动
							continue;				// 重新执行一次归零
						}
					}
				}
				else if (rslt == 2)
				{
					err = MoveToSensorEmsScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测急停条件
					if ((pmtsc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
						(pmtsc->movement > 0 && err == ERR_LMT_POSITIVE)	)	// 在限位
					{
						poismv = pmtsc->mvposi;		// 反向运动
						continue;				// 重新执行一次归零
					}
				}
			}

			err = GetMoveResultError(rslt, lmctrl.errInfo);	
		}

		break;
	}while(1);

	return err;
}

//------------------------------------------------

#endif

//----------------------------------------------------------------------------------------------------

#if (1)

// 急停检测
int MoveBHEmsScan(u32 para1, u32 para2)
{
	int rslt;
	MVByHandCtrl * pmbhc;

	pmbhc = (MVByHandCtrl*)(para1);

	rslt = GetCommonEms();

	if (rslt != ERR_NONE || pmbhc == NULL)
	{
		return rslt;
	}

	if (pmbhc->movement > 0)
	{
		if (pmbhc->limitpsensor != NULL)		// 检测正向限位
		{
			if (pmbhc->limitpsensor(pmbhc->para) == SENSOR_ON)
			{
				rslt = ERR_LMT_POSITIVE;	// 正向限位
			}
		}
	}
	else if (pmbhc->movement < 0)
	{
		if (pmbhc->limitnsensor != NULL)		// 检测反向限位
		{
			if (pmbhc->limitnsensor(pmbhc->para) == SENSOR_ON)
			{
				rslt = ERR_LMT_NEGATIVE;	// 反向限位
			}
		}
	}
	else
	{
		rslt = ERR_MTZ_ERROR;	// 归零错误
	}

	return rslt;
}

// 普通停止检测
int MoveBHNStopScan(u32 para1, u32 para2)
{
	int rslt;
	MVByHandCtrl * pmbhc;

	pmbhc = (MVByHandCtrl*)(para1);

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE || pmbhc == NULL)
	{
		return rslt;
	}
	if (rslt == ERR_NONE && pmbhc->stopscan != NULL)
	{
		rslt = pmbhc->stopscan(pmbhc->para);
	}

	return rslt;
}

// 慢速检测
int MoveBHNSlowScan(u32 para1, u32 para2)
{
	int rslt;
	MVByHandCtrl * pmbhc;

	pmbhc = (MVByHandCtrl*)(para1);

	rslt = ERR_NONE;

	if (pmbhc->slowscan != NULL)
	{
		rslt = pmbhc->slowscan(para2);
	}

	return rslt;
}

// 快速停止检测
int MoveBHQStopScan(u32 para1, u32 para2)
{
	int rslt;
	MVByHandCtrl * pmbhc;

	pmbhc = (MVByHandCtrl*)(para1);

	rslt = GetCommonQStop();
	if (rslt != ERR_NONE || pmbhc == NULL)
	{
		return rslt;
	}

	return rslt;
}

// 手动移动控制
int MoveByHandCtrl(MVByHandCtrl * pmbhc)
{
	int rslt, err, i;
	u16 axisIdx;
	InterMoveCtrl lmctrl;

	if (pmbhc == NULL)
	{
		return ERR_NONE;
	}

	if (pmbhc->movement == 0)
	{
		return ERR_NONE;
	}

	// 先决条件和准备工作
	if (pmbhc->precondition != NULL)
	{
		err = pmbhc->precondition(pmbhc->para);
		if (err != ERR_NONE)
		{
			return err;
		}
	}

	axisIdx = pmbhc->axisIdx;
	assert_param(axisIdx < AXIS_NUM);

	memset(&lmctrl, 0, sizeof(InterMoveCtrl));

	lmctrl.vAxisId = pmbhc->vAxisId;			// 使用虚轴

	lmctrl.motosConfig.newConfig = pmbhc->newCfg;										// 需要更新电机配置

	if (
		pmbhc->newCfg == 0 && 
		pmbhc->syncflag != 0)
	{
		for (i = 0; i < AXIS_NUM; i++)
		{
			if (pmbhc->syncList[i] != 0)
			{
				lmctrl.mvmtPara.movement[i] = pmbhc->movement;	// 各个轴的运动量，0，无运动；其他，运动长度，单位p
			}
		}
	}
	else
	{
		lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmbhc->axisCfg;					// 实轴输出配置
		lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmbhc->poutType;					// 脉冲输出模式(CW/CCW，PULSE/DIR)
		lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
		lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
		lmctrl.mvmtPara.movement[axisIdx] = pmbhc->movement;	// 各个轴的运动量，0，无运动；其他，运动长度，单位p
	}

	lmctrl.mvmtPara.extraRepeat = 0;						// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(pmbhc->movement);		// 插补长轴位移，必须大于或等于movement分量中的最大值
	lmctrl.mvmtPara.pulsePerSegment = pmbhc->ppSegment;	// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pmbhc->hspd.stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pmbhc->hspd.stspSpd;	// 停止速度
	lmctrl.spdPara.runPPS = pmbhc->hspd.runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pmbhc->hspd.stspSpd;	// 降速速度
	lmctrl.spdPara.addPPSG = pmbhc->hspd.spdAdd;	// 升速加速度
	lmctrl.spdPara.decPPSG = pmbhc->hspd.spdAdd/2;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pmbhc->hspd.brkAdd;	// 刹车加速度

	lmctrl.stopMode = STOP_MODE_SPD;				// 停止运动方式

	// 回调函数及参数
	lmctrl.Delay = DelayRef;						// 延时回调函数
	lmctrl.RefreshPosition = NormalRefPosition;		// 坐标刷新回调函数
	lmctrl.refPosPara = 0;							// 坐标刷新参数
	lmctrl.WorkBeforeRun = NULL;					// 运行前工作回调函数
	lmctrl.WorkAfterRun = NULL;						// 运行后工作回调函数
	lmctrl.workPara1 = 0;
	lmctrl.workPara2 = 0;							// 工作参数
	lmctrl.GetSlowDown = MoveBHNSlowScan;			// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.GetNormalStop = MoveBHNStopScan;			// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	lmctrl.GetQuickStop = MoveBHQStopScan;			// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetEmergencyStop = MoveBHEmsScan;		// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.condPara1 = (u32)(pmbhc);;
	lmctrl.condPara2 = (u32)(pmbhc->movement<0?-1:1);	// 条件函数参数

	rslt = InterpolationMotion(&lmctrl);		// 运动控制

	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}

#endif

//------------------------------------------------

//-------------------------------------------------------------------------------
// 单轴异步运动
// 使用STM32计算速度&发送脉冲
int SeparateExerStart(SepMoveCtrl * pCtrl)	// 单轴异步运动 启动
{
	int rslt;
	MotoCtrl lmctrl;
	u16 axisIdx;
	u16 axisID;
	int i;
	
	if (pCtrl == NULL)
	{
		return -1;
	}
	
	axisIdx = pCtrl->axisIdx;
	assert_param(axisIdx < AXIS_NUM);
	
	memset(&lmctrl, 0, sizeof(MotoCtrl));
	
	lmctrl.movement = pCtrl->movement;				// 位移量
													
	lmctrl.startPPS = pCtrl->startPPS;				// 启动速度
	lmctrl.runPPS 	= pCtrl->runPPS;				// 运动速度
	lmctrl.addPPSG 	= pCtrl->addPPSG;				// 启停加速度
	lmctrl.brkPPSG 	= pCtrl->brkPPSG;				// 刹车加速度

	lmctrl.funTestTime = pCtrl->funTestTime;		// 运行过程中外部函数检测间隔时间
	for (i = 0; i < 6; i++)
	{
		lmctrl.syncList[i] = pCtrl->syncList[i];	// 同步移动选择
	}

	lmctrl.GetNormalStop = pCtrl->GetNormalStop;	// 一般停止条件函数
	lmctrl.GetQuickStop = pCtrl->GetQuickStop;		// 快速停止条件函数
	lmctrl.condPara1 = pCtrl->condPara1;			// 条件函数参数
	lmctrl.condPara2 = pCtrl->condPara2;			// 条件函数参数

	lmctrl.blockRunflag = pCtrl->blockRunflag;		// 阻塞运行标志
	lmctrl.ExecWhenRun = pCtrl->ExecWhenRun;		// 电机运行过程中执行的函数
	lmctrl.ExecWhenStart = pCtrl->ExecWhenStart;	// 电机启动前执行的函数
	lmctrl.ExecWhenStop = pCtrl->ExecWhenStop;		// 电机停止后执行的函数

	axisID = axisIdx + 1;
	if (pCtrl->newCfg != 0)
	{// 重新配置
		if ((pCtrl->axisCfg & CONFIG_EN) != 0)				// 配置允许
		{
			if ((pCtrl->axisCfg & POA_EN) != 0)				// A允许输出
			{
				SetAxisConfig(axisID, POUTA_EN);
			}
			else
			{
				SetAxisConfig(axisID, POUTA_DIS);
			}

			if ((pCtrl->axisCfg & POB_EN) != 0)				// B允许输出
			{
				SetAxisConfig(axisID, POUTB_EN);
			}
			else
			{
				SetAxisConfig(axisID, POUTB_DIS);
			}

			if ((pCtrl->axisCfg & SON_EN) != 0)				// 伺服ON允许
			{
				SetAxisConfig(axisID, SERVO_ON);
			}
			else
			{
				SetAxisConfig(axisID, SERVO_OFF);
			}

			if ((pCtrl->axisCfg & ALM_LEVH) != 0)			// 报警电平为高
			{
				SetAxisAlarmConfig(axisID, ALM_LEV_HIGH);
			}
			else
			{
				SetAxisAlarmConfig(axisID, ALM_LEV_LOW);
			}

			if ((pCtrl->axisCfg & ALM_EN) != 0)				// 报警停车允许
			{
				SetAxisAlarmConfig(axisID, ALM_STOP_EN);
			}
			else
			{
				SetAxisAlarmConfig(axisID, ALM_STOP_DIS);
			}

			if ((pCtrl->axisCfg & POUT_EN) != 0)			// 输出允许
			{
				SetAxisConfig(axisID, pCtrl->poutType);		// 脉冲输出模式 脉冲方式输出
			}
			else
			{
				SetAxisConfig(axisID, POUT_DISABLE);		// 禁止脉冲输出
			}
		}
	}
#if (STM32_MOTOS_NUM >= 1)
	if (((axisID == 1) || (lmctrl.syncList[1-1] == 1)) && (pCtrl->useOutPort == 0))
	{
		SetAxisConfig(1, POUT_ENABLE_BUSIO);			// 脉冲输出允许:输入为内部总线直接控制模式
		SetAxisConfig(1, DATI_BUS_IO);					// 内部总线直接控制模式
		SetSTM32Moto1Mode(MODE_STEPDIR);				// 脉冲模式为方向/脉冲形式
	}
#endif
#if (STM32_MOTOS_NUM >= 2)
	if (((axisID == 2) || (lmctrl.syncList[2-1] == 1)) && (pCtrl->useOutPort == 0))
	{
		SetAxisConfig(2, POUT_ENABLE_BUSIO);			// 脉冲输出允许:输入为内部总线直接控制模式
		SetAxisConfig(2, DATI_BUS_IO);					// 内部总线直接控制模式
		SetSTM32Moto2Mode(MODE_STEPDIR);				// 脉冲模式为方向/脉冲形式
	}
#endif
#if (STM32_MOTOS_NUM >= 3)
	if (((axisID == 3) || (lmctrl.syncList[3-1] == 1)) && (pCtrl->useOutPort == 0))
	{
		SetAxisConfig(3, POUT_ENABLE_BUSIO);			// 脉冲输出允许:输入为内部总线直接控制模式
		SetAxisConfig(3, DATI_BUS_IO);					// 内部总线直接控制模式
		SetSTM32Moto3Mode(MODE_STEPDIR);				// 脉冲模式为方向/脉冲形式
	}
#endif
#if (STM32_MOTOS_NUM >= 4)
	if (((axisID == 4) || (lmctrl.syncList[4-1] == 1)) && (pCtrl->useOutPort == 0))
	{
		SetAxisConfig(4, POUT_ENABLE_BUSIO);			// 脉冲输出允许:输入为内部总线直接控制模式
		SetAxisConfig(4, DATI_BUS_IO);					// 内部总线直接控制模式
		SetSTM32Moto4Mode(MODE_STEPDIR);				// 脉冲模式为方向/脉冲形式
	}
#endif
#if (STM32_MOTOS_NUM >= 5)
	if (((axisID == 5) || (lmctrl.syncList[5-1] == 1)) && (pCtrl->useOutPort == 0))
	{
		SetAxisConfig(5, POUT_ENABLE_BUSIO);			// 脉冲输出允许:输入为内部总线直接控制模式
		SetAxisConfig(5, DATI_BUS_IO);					// 内部总线直接控制模式
		SetSTM32Moto5Mode(MODE_STEPDIR);				// 脉冲模式为方向/脉冲形式
	}
#endif
#if (STM32_MOTOS_NUM >= 6)
	if (((axisID == 6) || (lmctrl.syncList[6-1] == 1)) && (pCtrl->useOutPort == 0))
	{
		SetAxisConfig(6, POUT_ENABLE_BUSIO);			// 脉冲输出允许:输入为内部总线直接控制模式
		SetAxisConfig(6, DATI_BUS_IO);					// 内部总线直接控制模式
		SetSTM32Moto6Mode(MODE_STEPDIR);				// 脉冲模式为方向/脉冲形式
	}
#endif

	switch (axisIdx)
	{
#if (STM32_MOTOS_NUM >= 1)
	case 0:
		rslt = STM32Moto1Start(&lmctrl);
		break;
#endif
#if (STM32_MOTOS_NUM >= 2)
	case 1:
		rslt = STM32Moto2Start(&lmctrl);
		break;
#endif
#if (STM32_MOTOS_NUM >= 3)
	case 2:
		rslt = STM32Moto3Start(&lmctrl);
		break;
#endif
#if (STM32_MOTOS_NUM >= 4)
	case 3:
		rslt = STM32Moto4Start(&lmctrl);
		break;
#endif
#if (STM32_MOTOS_NUM >= 5)
	case 4:
		rslt = STM32Moto5Start(&lmctrl);	
		break;		
#endif
#if (STM32_MOTOS_NUM >= 6)
	case 5:
		rslt = STM32Moto6Start(&lmctrl);
		break;		
#endif
	default:
		rslt = ERR_MV_PARA;		// 参数错误
		break;
	}
	
	return rslt;		 
}

//------------------------------------------------
// 单轴异步运动 停止
int SeparateExerStop(u16 axisIdx)		
{
	int rslt;

	assert_param(axisIdx < AXIS_NUM); 
	
	rslt = ERR_NONE;
	
	switch (axisIdx)
	{
	case 0:
#if (STM32_MOTOS_NUM >= 1)
		STM32Moto1Stop();
		break;
#endif
#if (STM32_MOTOS_NUM >= 2)
	case 1:			
		STM32Moto2Stop();
		break;
#endif
#if (STM32_MOTOS_NUM >= 3)
	case 2:				
		STM32Moto3Stop();
		break;
#endif
#if (STM32_MOTOS_NUM >= 4)
	case 3:
		STM32Moto4Stop();
		break;
#endif
#if (STM32_MOTOS_NUM >= 5)
	case 4:
		STM32Moto5Stop();		
		break;
#endif
#if (STM32_MOTOS_NUM >= 6)
	case 5:
		STM32Moto6Stop();
		break;
#endif
	default:	
		rslt = ERR_MV_PARA;		// 参数错误
		break;
	} 
	
	return	rslt;
}

//------------------------------------------------
int GetSeparateExerState(u16 axisIdx)		// 单轴异步运动 得到当前状态
{
	int rslt,err;
	
	assert_param(axisIdx < AXIS_NUM);
	
	rslt = ERR_NONE;
	
	switch (axisIdx)
	{
#if (STM32_MOTOS_NUM >= 1)
	case 0:
		rslt = GetSTM32Moto1State();
		break;
#endif
#if (STM32_MOTOS_NUM >= 2)
	case 1:
		rslt = GetSTM32Moto2State();
		break;
#endif
#if (STM32_MOTOS_NUM >= 3)
	case 2:
		rslt = GetSTM32Moto3State();
		break;
#endif
#if (STM32_MOTOS_NUM >= 4)
	case 3:
		rslt = GetSTM32Moto4State();
		break;
#endif
#if (STM32_MOTOS_NUM >= 5)
	case 4:
		rslt = GetSTM32Moto5State();		
		break;		
#endif
#if (STM32_MOTOS_NUM >= 6)
	case 5:
		rslt = GetSTM32Moto6State();
		break;		
#endif
	default:	
		rslt = ERR_MV_PARA;		// 参数错误
		break;
	} 
	
	if (rslt == STM32MOTO_STOP)
	{
		err = STA_PROCESS_FINISH;			// 执行完成
	}
	else
	{
		//err = STA_PROCESS_RUNNING;			// 执行过程中
		err = rslt;							// 返回运行状态
	}	 
	
	return	err;
}

//------------------------------------------------ 

//------------------------------------------------------------------------------- 
// 转化为 -180--180度之间的数
void ChangeRotToNP180(s32 * pval)
{
	while (*pval > (ROT_PULSE_PER_CIRCLE/2))
	{
		*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
	}

	while (*pval < ((s32)(ROT_PULSE_PER_CIRCLE/2) * -1))
	{
		*pval += (s32)(ROT_PULSE_PER_CIRCLE);
	}
}

// 转化为 -360--360度之间的数
void ChangeRotToNP360(s32 * pval)
{
	while (*pval < ((s32)(ROT_PULSE_PER_CIRCLE) * -1))
	{
		*pval += (s32)(ROT_PULSE_PER_CIRCLE);
	}

	while(*pval > ((s32)(ROT_PULSE_PER_CIRCLE)))
	{
		*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
	}
}

// 转化为 0--360度之间的数
void ChangeRotTo0To360(s32 * pval)
{
	while (*pval < 0)
	{
		*pval += (s32)(ROT_PULSE_PER_CIRCLE);
	}

	while(*pval > ((s32)(ROT_PULSE_PER_CIRCLE)))
	{
		*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
	}
}

// 转化为 -360--0度之间的数
void ChangeRotToN360To0(s32 * pval)
{
	while (*pval > 0)
	{
		*pval -= (s32)(ROT_PULSE_PER_CIRCLE);
	}

	while (*pval < ((s32)(ROT_PULSE_PER_CIRCLE) * -1))
	{
		*pval += (s32)(ROT_PULSE_PER_CIRCLE);
	}
}
//
int GetMoveResultError(int rslt, int errinfo)
{
	int err;
	switch (rslt)
	{
	case -1:		// 参数错误
		err = ERR_MV_PARA;
		break;
	case -2:		// 填充数据错误
		err = ERR_FILL_DATA;
		break;
	case -3:		// FPGA读写错误
		err = ERR_FPGA_RESET;		// FPGA复位错误
		break;
	case -4:		// FPGA版本错误
		err = ERR_FPGA_ERR;			// 运动控制芯片版本错误
		break;
	case 1:			// 驱动器报警
		err = ERR_DRIVER;
		break;
	case 0:			// 运动执行完成
	case 2:			// 紧急停止条件触发停车
	case 3:			// 快速停止条件触发停车
	case 4:			// 普通停车条件触发停车
		err = errinfo;
		break;
	case 5:
		err = ERR_FPGA_RESET;
		break;
	default:		// 正常结束或其他
		err = ERR_NONE;
		break;
	}
	return err;
}

//---------------------------------------------------

// 主轴角度转化
// 如果pdir == 0, 转化为 -180--180度之间的数,
// 如果pdir != 0  那么转化为 0-- 360之间的数
void ChangeMsToNP180(s32 * pval, int pdir)
{
	while (*pval < ((s32)(MS_PULSE_PER_CIRCLE/2) * -1))
	{
		*pval += (s32)(MS_PULSE_PER_CIRCLE);
	}

	while (*pval > ((s32)(MS_PULSE_PER_CIRCLE/2)))
	{
		*pval -= (s32)(MS_PULSE_PER_CIRCLE);
	}

	if (pdir != 0)	// 正向转动
	{
		if (*pval < 0)
		{
			*pval += MS_PULSE_PER_CIRCLE;
		}
	}
}

void ChangeMsToNP360(s32 * pval)
{
	*pval %= MS_PULSE_PER_CIRCLE;

	while (*pval < ((s32)(MS_PULSE_PER_CIRCLE) * -1))
	{
		*pval += (s32)(MS_PULSE_PER_CIRCLE);
	}

	while(*pval > ((s32)(MS_PULSE_PER_CIRCLE)))
	{
		*pval -= (s32)(MS_PULSE_PER_CIRCLE);
	}
}

void ChangeMsTo0To360(s32 * pval)
{
	*pval %= MS_PULSE_PER_CIRCLE;

	while (*pval < 0)
	{
		*pval += (s32)(MS_PULSE_PER_CIRCLE);
	}

	while(*pval > ((s32)(MS_PULSE_PER_CIRCLE)))
	{
		*pval -= (s32)(MS_PULSE_PER_CIRCLE);
	}
}

//--------------------------------------------------------------------------------
// 普通停止检测
int MoveToSensorAsMidPosNStopScan(u32 para1, u32 para2)
{
	int rslt;
	MTSAMPCtrl * pmts;

	pmts = (MTSAMPCtrl*)(para1);

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE || pmts == NULL)
	{
		return rslt;
	}

	return rslt;
}

// 快速停止检测
int MoveToSensorAsMidPosQStopScan(u32 para1, u32 para2)
{
	int rslt;
	MTSAMPCtrl * pmts;

	pmts = (MTSAMPCtrl*)(para1);

	rslt = GetCommonQStop();
	if (rslt != ERR_NONE || pmts == NULL)
	{
		return rslt;
	}

	if (rslt == ERR_NONE)
	{
		if (pmts->zerosensor != NULL)		// 检测零位
		{
			if (pmts->zerosensor() == pmts->zeroval)
			{
				rslt = STA_MTZ_SUCCESS;		// 归零成功
			}
		}
	}

	return rslt;
}

// 急停检测
int MoveToSensorAsMidPosEmsScan(u32 para1, u32 para2)
{
	int rslt;
	MTSAMPCtrl * pmts;

	pmts = (MTSAMPCtrl*)(para1);

	rslt = GetCommonEms();

	if (rslt != ERR_NONE || pmts == NULL)
	{
		return rslt;
	}

	if (pmts->emcstopscan != NULL)
	{
		rslt = pmts->emcstopscan();
	}

	if (rslt != ERR_NONE)
	{
		return rslt;
	}

	if (pmts->movement > 0)
	{
		if (pmts->limitpsensor != NULL)		// 检测正向限位
		{
			if (pmts->limitpsensor() == SENSOR_ON)
			{
				rslt = ERR_LMT_POSITIVE;	// 正向限位
			}
		}
	}
	else if (pmts->movement < 0)
	{
		if (pmts->limitnsensor != NULL)		// 检测反向限位
		{
			if (pmts->limitnsensor() == SENSOR_ON)
			{
				rslt = ERR_LMT_NEGATIVE;	// 反向限位
			}
		}
	}
	else
	{
		rslt = ERR_MTZ_ERROR;	// 归零错误
	}

	return rslt;
}

// 找挡片中心位置停止检测
int MoveToMidPosQStopScan(u32 para1, u32 para2)
{
	static int isckzero = 0;

	int rslt = 0;
	MTSAMPCtrl * pmts;

	pmts = (MTSAMPCtrl*)(para1);

	rslt = GetCommonQStop();
	if (rslt != ERR_NONE || pmts == NULL)
	{
		return rslt;
	}

	if (rslt == ERR_NONE)
	{
		if (pmts->zerosensor != NULL)		// 检测零位
		{
			if (isckzero == 1)
			{
				if (pmts->zerosensor() != pmts->zeroval)
				{
					isckzero = 0;
					rslt = 1;
				}
			}
			else
			{
				if (pmts->zerosensor() == pmts->zeroval)
				{
					isckzero = 1;
				}
			}
		}
	}

	return rslt;
}

// 直线运动到零位传感器且挡片在中心位置,特别说明,此种归零方式不能使用带有豁口的挡片,尤其是豁口不在中心位置
// return -1:参数错误
int LineRunToSensorAsMidPosCtrl(MTSAMPCtrl * pmtsampc)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 axisIdx;
	int mvnum = 0;				// 运动次数,最多运动两次,即正反限位各一次,若运动到正反限位还没有找到零位传感器,则归零失败

	if (pmtsampc == NULL)
	{
		return -1;
	}

	axisIdx = pmtsampc->axisIdx;
	assert_param(axisIdx < AXIS_NUM);

	memset(&lmctrl, 0, sizeof(InterMoveCtrl));

	lmctrl.vAxisId = pmtsampc->vAxisId;										// 使用虚轴
	lmctrl.motosConfig.newConfig = pmtsampc->newCfg;						// 需要更新电机配置

	err = ERR_NONE;

#if (1)
	//if (pmtsampc->syncflag == 0)
	{
		err = MoveToSensorAsMidPosEmsScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测急停条件
		if ((pmtsampc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
			(pmtsampc->movement > 0 && err == ERR_LMT_POSITIVE)	)	// 在限位
		{
			mvnum++;
			pmtsampc->movement = pmtsampc->movement * (-1);			// 在限位则向反向运动找零
		}

		err = MoveToSensorAsMidPosQStopScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测停止条件
		if (err == STA_MTZ_SUCCESS)							// 在零位位置
		{
			pmtsampc->movement = 0;							// 在零位则跳过找零位传感器过程,直接慢速移动找挡片中心位置
		}
	}
#endif

	if (pmtsampc->newCfg == 0 && pmtsampc->syncflag != 0)
	{
		for (int i = 0; i < AXIS_NUM; i++)
		{
			if (pmtsampc->syncList[i] != 0)
			{
				lmctrl.mvmtPara.movement[i] = pmtsampc->movement;	// 各个轴的运动量，0，无运动；其他，运动长度，单位p
			}
		}
	}
	else
	{
		lmctrl.motosConfig.axisConfig[axisIdx].axisConfig = pmtsampc->axisCfg;		// 实轴输出配置
		lmctrl.motosConfig.axisConfig[axisIdx].poutType = pmtsampc->poutType;		// 脉冲输出模式(CW/CCW，PULSE/DIR)
		lmctrl.motosConfig.axisConfig[axisIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
		lmctrl.motosConfig.axisConfig[axisIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)

		lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;	// 各个轴的运动量，0，无运动；其他，运动长度，单位p
	}

	lmctrl.mvmtPara.extraRepeat = 0;						// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(pmtsampc->movement);	// 插补长轴位移，必须大于或等于movement分量中的最大值
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;	// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pmtsampc->lspd.stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pmtsampc->lspd.stspSpd;	// 停止速度
	lmctrl.spdPara.runPPS = pmtsampc->lspd.runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pmtsampc->lspd.stspSpd;	// 降速速度
	lmctrl.spdPara.addPPSG = pmtsampc->lspd.spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pmtsampc->lspd.spdAdd/2;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pmtsampc->lspd.brkAdd;		// 刹车加速度

	lmctrl.stopMode = STOP_MODE_SPD;				// 停止运动方式

	// 回调函数及参数
	lmctrl.Delay = DelayRef;						// 延时回调函数
	lmctrl.RefreshPosition = NormalRefPosition;		// 坐标刷新回调函数
	lmctrl.refPosPara = 0;							// 坐标刷新参数
	lmctrl.WorkBeforeRun = NULL;					// 运行工作回调函数
	lmctrl.WorkAfterRun = NULL;						// 运行后工作回调函数
	lmctrl.workPara1 = 0;
	lmctrl.workPara2 = 0;							// 工作参数
	lmctrl.GetSlowDown = NULL;								// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.GetNormalStop = MoveToSensorAsMidPosNStopScan;	// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	lmctrl.GetQuickStop = MoveToSensorAsMidPosQStopScan;	// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetEmergencyStop = MoveToSensorAsMidPosEmsScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.condPara1 = (u32)(pmtsampc);;
	lmctrl.condPara2 = 0;							// 条件函数参数

	// 运动到零位传感器
	while(mvnum < 2 && pmtsampc->movement != 0)
	{
		rslt = InterpolationMotion(&lmctrl);		// 运动控制

		if (rslt == 3)
		{
			err = MoveToSensorAsMidPosQStopScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测停止条件
			break;
		}
		else if (rslt == 2)
		{
			err = MoveToSensorAsMidPosEmsScan(lmctrl.condPara1, lmctrl.condPara2);		// 检测急停条件
			if ( (pmtsampc->movement < 0 && err == ERR_LMT_NEGATIVE) ||
					(pmtsampc->movement > 0 && err == ERR_LMT_POSITIVE) ||		// 在限位
					0 )
			{
				mvnum++;
				pmtsampc->movement = pmtsampc->movement * (-1);		// 反向运动
				lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
			}
			else
			{
				break;
			}
		}
		else
		{
			break;
		}
	}

	if (mvnum >= 2)					// 正负限位都运动还未找到零位,归零错误
	{
		err = ERR_MTZ_ERROR;
		return err;
	}

	if (err == STA_MTZ_SUCCESS)		// 找零成功,开始移动到挡片中心位置
	{
		s32 pos1, pos2;		// 挡片两侧位置记录

		pmtsampc->movement = pmtsampc->bafflewidth + 1000;
		lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
		lmctrl.mvmtPara.interLong = abs(pmtsampc->movement);	// 插补长轴位移，必须大于或等于movement分量中的最大值

		lmctrl.spdPara.startPPS = pmtsampc->mposspd.stspSpd;	// 启动速度
		lmctrl.spdPara.stopPPS = pmtsampc->mposspd.stspSpd;	// 停止速度
		lmctrl.spdPara.runPPS = pmtsampc->mposspd.runSpd;		// 运行速度
		lmctrl.spdPara.slowPPS = pmtsampc->mposspd.stspSpd;	// 降速速度
		lmctrl.spdPara.addPPSG = pmtsampc->mposspd.spdAdd;	// 升速加速度
		lmctrl.spdPara.decPPSG = pmtsampc->mposspd.spdAdd/2;	// 降速加速度
		lmctrl.spdPara.brkPPSG = pmtsampc->mposspd.brkAdd;	// 刹车加速度

		lmctrl.GetQuickStop = MoveToMidPosQStopScan;	// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）

		err = ERR_NONE;
		rslt = InterpolationMotion(&lmctrl);		// 运动控制
		err = GetMoveResultError(rslt, lmctrl.errInfo);

		if (err == 1)
		{
			pos1 = GetMotoCounter(axisIdx);
			pmtsampc->movement = pmtsampc->movement * (-1);
			lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;

			err = ERR_NONE;
			rslt = InterpolationMotion(&lmctrl);		// 运动控制
			err = GetMoveResultError(rslt, lmctrl.errInfo);

			if (err == 1)
			{
				int dir;
				pos2 = GetMotoCounter(axisIdx);
				if (pmtsampc->movement > 0)
				{
					dir = -1;		// 跟前一次运动方向相反
				}
				else
				{
					dir = 1;
				}
				pmtsampc->movement = abs(pos1-pos2)/2;		// 20220317 BUG修复,单纯取绝对值电机只会单向运动,不会根据实际运动方向自切换
				pmtsampc->movement *= dir;
				lmctrl.mvmtPara.movement[axisIdx] = pmtsampc->movement;
				lmctrl.mvmtPara.interLong = abs(pmtsampc->movement);
				lmctrl.GetQuickStop = NULL;		// 20210812 因有些挡片带有豁口(带降速传感器时,实际此种归零方式完全不需要降速传感器)
												// 当找零运动到豁口时,个别情况下会存在传感器响应不正常的情况
												// 故此处移动到挡片中心位置时不再检测此停止条件

				err = ERR_NONE;
				rslt = InterpolationMotion(&lmctrl);		// 运动控制
				err = GetMoveResultError(rslt, lmctrl.errInfo);

				if (err == ERR_NONE)
				{
					err = STA_MTZ_SUCCESS;
				}
			}
		}
	}

	return err;
}