

#include "drivermotion.h"

#if (DEV_MOTIONS_NUM > 0)

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

// 积分计算的运动控制的计算

#ifndef CALC_TIMES_PER_SEC
#define CALC_TIMES_PER_SEC		(10000)		// 每秒钟计算次数
#endif

#ifndef USE_FLOAT_CALC
#define USE_FLOAT_CALC	1		//  0, 整数运算,  1, 浮点数运算
#endif

#if (USE_FLOAT_CALC == 1)
#define	CALC_TYPE		float
#define CALC_MUTI		1.0f
#else

#define	CALC_TYPE		s32
#define CALC_MUTI		10000

#endif


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



typedef struct
{
	// 输入参数
	CALC_TYPE startSpd;				// 启动停止速度
	CALC_TYPE runSpd;				// 运动速度
	CALC_TYPE runAdd;				// 启停加速度
	CALC_TYPE brkAdd;				// 刹车加速度

	u32 funTestTime;			// 运行过程中外部函数检测间隔时间 0:不检测外部函数 (单位:0.1ms)

	// 函数指针
	int (*GetNormalStop)(u32,u32);		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按启停加速度降速）
	int (*GetQuickStop)(u32,u32);		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度降速）
	u32 condPara1, condPara2;			// 条件函数参数

	int blockRunflag;					// 阻塞运行标志, =1阻塞运行,同步运动	=0,非阻塞,异步运动
	void (*ExecWhenRun)(void);			// 电机运行过程中执行的函数
	void (*ExecWhenStart)(void);		// 电机启动时执行的函数
	void (*ExecWhenStop)(void);			// 电机停止时执行的函数
	void (*DelayWhenBlock)(u32);		// 阻塞时的Delay函数

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

	// 状态参数
	int 	runState;		// 运行状态
	s32 	realpos;		// 绝对坐标
	int 	startrunflag;	// 启动运行标志
	int 	stopflag;		// 停止标志

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

	// 计算控制变量
	int 	rundir;			// 运行方向
	u32		totalNums;		// 运动总脉冲数
	u32 	pulseCount;		// 当前脉冲计数
	u32 	decPos;			// 减速点位置

	u32 	funTestCount;	// 运行过程中外部函数检测间隔时间计数器

	CALC_TYPE 	currentSpd;		// 当前速度
	CALC_TYPE 	currentAcc;		// 当前加速度
	CALC_TYPE 	currentPos;		// 当前位置
	//---------------------

} DriverMotionCtrl;

DriverMotionCtrl g_drvMotionsCtrlList[DEV_MOTIONS_NUM];

//----------------------------------------------------------------
// 积分计算过程

void IntegrationMotionCalc(DriverMotionCtrl * pCtrl)
{
	if (pCtrl == NULL ||
		0	)
	{
		return;
	}

	if (pCtrl->runState <= DRV_MOTION_STOP ||
		pCtrl->startrunflag <= 0)				// 停止状态
	{
		return;		// 不执行
	}

	if (pCtrl->stopflag != 0)
	{
		if (pCtrl->stopflag == 1)
		{
			pCtrl->currentAcc = pCtrl->runAdd;		// 当前加速度
			pCtrl->runState = DRV_MOTION_NSTOP;
		}
		else
		{
			pCtrl->currentAcc = pCtrl->brkAdd;		// 当前加速度
			pCtrl->runState = DRV_MOTION_ESTOP;
		}
	}

	if (pCtrl->funTestTime > 0)		// 条件检测
	{
		pCtrl->funTestCount++;		// 计数增加
		if ((pCtrl->funTestCount >= pCtrl->funTestTime) &&		// 到了测试时间
			1	)
		{
			pCtrl->funTestCount = 0;
			if (pCtrl->GetNormalStop != NULL)			// 一般停止条件
			{
				if ((pCtrl->runState == DRV_MOTION_ADD) ||		// 加速
					(pCtrl->runState == DRV_MOTION_RUN) ||		// 匀速
					(pCtrl->runState == DRV_MOTION_DEC) ||		// 计数降速
					0	)
				{
					if (pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2) != 0)
					{
						pCtrl->currentAcc = pCtrl->runAdd;		// 当前加速度
						pCtrl->runState = DRV_MOTION_NSTOP;		// 普通条件停车
					}
				}
			}
			if (pCtrl->GetQuickStop != NULL)			// 快速停止条件
			{
				if ((pCtrl->runState == DRV_MOTION_ADD) ||		// 加速
					(pCtrl->runState == DRV_MOTION_RUN) ||		// 匀速
					(pCtrl->runState == DRV_MOTION_DEC) ||		// 计数降速
					(pCtrl->runState == DRV_MOTION_ESTOP) ||	// 条件停车
					0	)
				{
					if (pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2) != 0)
					{
						pCtrl->currentAcc = pCtrl->brkAdd;		// 当前加速度
						pCtrl->runState = DRV_MOTION_ESTOP;				// 急停条件停车
					}
				}
			}
			if (pCtrl->ExecWhenRun != NULL)		// 电机运行过程中执行的函数
			{
				pCtrl->ExecWhenRun();
			}
		}
	}

	// 加速中
	if (pCtrl->runState == DRV_MOTION_ADD)
	{
		pCtrl->currentSpd += pCtrl->currentAcc;			// 改变当前速度
		if (pCtrl->currentSpd >= pCtrl->runSpd)			// 当前速度大于运行速度,进入匀速状态
		{
			pCtrl->runState = DRV_MOTION_RUN;
			pCtrl->currentSpd = pCtrl->runSpd;
			pCtrl->decPos = (pCtrl->totalNums - pCtrl->pulseCount);		// 减速点位移
		}
	}

	// 减速中
	if (pCtrl->runState == DRV_MOTION_DEC)  			// 计数降速
	{
		pCtrl->currentSpd -= pCtrl->currentAcc;			// 当前速度
		if (pCtrl->currentSpd <= pCtrl->startSpd)
		{
			pCtrl->currentSpd = pCtrl->startSpd;
		}
	}

	//  条件停车中
	if ((pCtrl->runState == DRV_MOTION_NSTOP) ||			// 条件停车
		(pCtrl->runState == DRV_MOTION_ESTOP) ||			// 条件刹车
		0	)
	{
		pCtrl->currentSpd -= pCtrl->currentAcc;			// 当前速度
		if (pCtrl->currentSpd <= pCtrl->startSpd)		// 当前速度小于启动速度,进入停止状态
		{
			pCtrl->currentSpd = pCtrl->startSpd;
			pCtrl->runState = DRV_MOTION_STOPING;
		}
	}

	// 位置计算
	{
		pCtrl->currentPos += pCtrl->currentSpd * pCtrl->rundir;		// 速度积分
		if (pCtrl->currentPos > CALC_MUTI || pCtrl->currentPos < -CALC_MUTI)	// 位置移出
		{
			int ofst = (int)(pCtrl->currentPos/CALC_MUTI);	// 计算位置偏差
			pCtrl->currentPos -= ofst*CALC_MUTI;
			pCtrl->realpos += ofst;				// 位置改变
			pCtrl->pulseCount += abs(ofst);		// 计数值增加
		}
	}

	// 停止处理
	{
		if ((pCtrl->pulseCount >= pCtrl->totalNums) 	||	// 发送脉冲完成
			(pCtrl->runState <= DRV_MOTION_STOP) 		||	// 停止信号
			(pCtrl->runState == DRV_MOTION_STOPING)		||	// 停止中
			0	)
		{
			if (pCtrl->runState > DRV_MOTION_STOP)			// 运行状态
			{
				pCtrl->runState = DRV_MOTION_STOP;	// 设置为关闭状态
				pCtrl->startrunflag = 0;					// 关闭运行标志
				if (pCtrl->ExecWhenStop != NULL)	// 执行关闭时的函数
				{
					pCtrl->ExecWhenStop();
				}
			}
		}
		else
		{
			if (	(pCtrl->runState == DRV_MOTION_ADD) &&				// 加速状态中
					(pCtrl->pulseCount >= (pCtrl->totalNums>>1)) &&		// 大于位移的一半
					1	)
			{
				pCtrl->runState = DRV_MOTION_RUN;
				pCtrl->currentAcc = pCtrl->runAdd;		// 当前加速度
				pCtrl->decPos = pCtrl->pulseCount;
#if (0)
#if (USE_FLOAT_CALC == 0)
				printf("enter dec, spd=%d\r\n", pCtrl->currentSpd);
#else
#endif
#endif
			}

			if (	(pCtrl->runState == DRV_MOTION_RUN) &&		// 匀速中
					(pCtrl->pulseCount >= pCtrl->decPos) &&		// 大于降速点
					1	)
			{
				pCtrl->runState = DRV_MOTION_DEC;
				pCtrl->currentAcc = pCtrl->runAdd;				// 当前加速度
			}
		}
	}
}

CALC_TYPE pps_to_ppg(s32 pps)
{
	return pps*CALC_MUTI / CALC_TIMES_PER_SEC;
}

//----------------------------------------------------------------
// 启动运动控制（停止状态时可用）
int DrvMotionStart(int driverIdx, DrvMotionPara * pPara)
{
	DriverMotionCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
	{
		printf("DrvMotionStart fail, driverIdx error\r\n");
		return -1;
	}

	pCtrl = &(g_drvMotionsCtrlList[driverIdx]);

	if (pPara == NULL)
	{
		printf("DrvMotionStart fail, pPara is NULL\r\n");
		return -1;
	}

	if (pCtrl->runState <= DRV_MOTION_STOP)			// 停车/报警状态允许启动电机
	{
		pCtrl->startrunflag = 0;				// 关闭运行标志

		if (pPara->movement == 0)	// 电机运行完毕
		{
			pCtrl->runState = DRV_MOTION_STOP;
			printf("driverIdx=%d, movement=0\r\n", driverIdx);
			pCtrl->rundir = 0;
			return 0;
		}

		if (pPara->startPPS < 10)
		{
			pPara->startPPS = 10;
		}
		if (pPara->runPPS < 10)
		{
			pPara->runPPS = 10;
		}
		if (pPara->startPPS > pPara->runPPS)
		{
			pPara->startPPS = pPara->runPPS;
		}
		if (pPara->addPPSS < 1)
		{
			pPara->addPPSS = 1;
		}

		// 输入速度参数计算
		pCtrl->startSpd	= pps_to_ppg(pPara->startPPS);		// 启动停止速度
		pCtrl->runSpd	= pps_to_ppg(pPara->runPPS);		// 运动速度
		pCtrl->runAdd	= pps_to_ppg(pPara->addPPSS);		// 启停加速度
		pCtrl->brkAdd	= pps_to_ppg(pPara->brkPPSS);		// 刹车加速度

		pCtrl->currentSpd = pCtrl->startSpd;				// 当前速度
		pCtrl->currentAcc = pCtrl-> runAdd;					// 当前加速度
		pCtrl->currentPos = 1;								// 当前位置, 先输出一个脉冲
		pCtrl->runState = DRV_MOTION_ADD;					// 运行状态, 加速中

		pCtrl->funTestTime = pPara->funTestTime;
		pCtrl->funTestCount = 0; 							// 运行过程中外部函数检测间隔时间计数器

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

		pCtrl->blockRunflag		= pPara->blockRunflag;		// 阻塞运行标志
		pCtrl->ExecWhenStart 	= pPara->ExecWhenStart;		// 启动时执行的函数
		pCtrl->ExecWhenRun 		= pPara->ExecWhenRun;		// 运行过程中执行的函数
		pCtrl->ExecWhenStop 	= pPara->ExecWhenStop;		// 停止时执行的函数
		pCtrl->DelayWhenBlock 	= pPara->DelayWhenBlock;	// 阻塞时执行的函数

		if (pCtrl->ExecWhenStart != NULL)				// 启动前执行
		{
			pCtrl->ExecWhenStart();
		}
		//---------------------
		// 位移配置
		pCtrl->rundir = (pPara->movement > 0) ? 1:-1;	// 方向
		pCtrl->totalNums = abs(pPara->movement);		// 总位移
		pCtrl->pulseCount = 0;							// 脉冲计数
		pCtrl->decPos = pCtrl->totalNums; 				// 减速位置

		// 输出配置
		pCtrl->stopflag = 0;
		pCtrl->startrunflag = 1;						// 设置启动运行标志

		return DrvMotionRun(driverIdx);
	}
	else
	{
		printf("DrvMotionStart failed, drv%d is running\r\n", driverIdx);
		return -2;											// 电机正在运行
	}
}

// 停止运动控制（非阻塞运动时用到）
void DrvMotionStop(int driverIdx)
{
	DriverMotionCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
	{
		return;
	}

	pCtrl = &(g_drvMotionsCtrlList[driverIdx]);

	pCtrl->stopflag = 1;
}

// 停止运动控制（非阻塞运动时用到）
void DrvMotionBreak(int driverIdx)
{
	DriverMotionCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
	{
		return;
	}

	pCtrl = &(g_drvMotionsCtrlList[driverIdx]);

	pCtrl->stopflag = 2;
}


int DrvMotionRun(int driverIdx)
{
	DriverMotionCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
	{
		return -1;
	}

	pCtrl = &(g_drvMotionsCtrlList[driverIdx]);

	// 运行循环
	do
	{
		if (pCtrl->runState <= DRV_MOTION_STOP)
		{
			break;
		}

		if (pCtrl->blockRunflag == 0)		// 阻塞运行标志
		{
			break;
		}

		if (pCtrl->DelayWhenBlock != NULL)
		{
			pCtrl->DelayWhenBlock(0);
		}

	}while(1);

	return pCtrl->runState;
}

// 得到运动状态（非阻塞运动时用到）
int  GetDrvMotionState(int driverIdx)
{
	DriverMotionCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
	{
		return 0;
	}

	pCtrl = &(g_drvMotionsCtrlList[driverIdx]);

	return pCtrl->runState;
}

// 得到当前运动位置（非阻塞运动时用到）
s32  GetDrvMotionRealPos(int driverIdx)
{
	DriverMotionCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
	{
		return 0;
	}

	pCtrl = &(g_drvMotionsCtrlList[driverIdx]);

	return pCtrl->realpos;
}

// 设置当前运动位置（停止状态时可用）
// 禁止应用层使用此代码,若需重置电机坐标,需使用ResetDriverPos
void SetDrvMotionRealPos(int driverIdx, s32 pos)
{
	DriverMotionCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DEV_MOTIONS_NUM)
	{
		return ;
	}

	pCtrl = &(g_drvMotionsCtrlList[driverIdx]);

	pCtrl->realpos = pos;
//	printf("set drv%d realpos to %d\r\n", driverIdx+1, pos);
}

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

// 初始化
void InitDrvMotions(void)
{
	memset(&g_drvMotionsCtrlList, 0, sizeof(DriverMotionCtrl)*DEV_MOTIONS_NUM);

	// 测试命令
}

// 计算过程
void DrvMotionCalcProc(void)
{
	DriverMotionCtrl * pCtrl;
	int i;
	for (i = 0; i < DEV_MOTIONS_NUM; i++)
	{
		pCtrl = &(g_drvMotionsCtrlList[i]);
		IntegrationMotionCalc(pCtrl);
	}
}

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





#endif

