#define	_IN_STM32MOTOS_C
#include "stm32motos.h"
#include "inout.h"
#include "shell.h"

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

void TestStm32Motos(char * para1, char * para2);		//终端调试接口

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

typedef struct
{
	// 配置参数
	u32 	pulseMode;					// 脉冲类型		0:cw/ccw		!=0:pulse/dir
	u32 	startPPS;					// 启动速度
	u32 	runPPS;						// 运动速度
	u32 	addPPSG;					// 启停加速度
	u32 	brkPPSG;					// 刹车加速度

	// 	运动中参数
	s32		realpos;					// 电机绝对坐标
	int 	runState;					// 运行状态
	int 	dir;						// 运行方向
	s32 	currentSpd;					// 当前速度
	u32 	currentAcc;					// 当前加速度
	s32 	totalAcc;					// 累加加速度
	u32 	currentPos;					// 当前位置
	u32 	totalPos;					// 总位移
	u32 	decPos;						// 减速点位移
	int 	pulseState;					// 脉冲状态 0:当前输出为0, !=当前输出为1
	int 	startrunflag;				// 启动运行标志

	// 函数指针
	u32 	funTestTime;				// 运行过程中外部函数检测间隔时间 		单位:0.1ms
	u32 	funTestCount;				// 运行过程中外部函数检测间隔时间计数器
	int 	syncList[6];				// 同步移动选择
	
	int 	(*GetNormalStop)(u32,u32);		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按启停加速度降速）
	int 	(*GetQuickStop)(u32,u32);		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度降速）
	u32 	condPara1, condPara2;			// 条件函数参数

	int 	blockRunflag;					// 阻塞运行标志
	void 	(*ExecWhenRun)(void);			// 电机运行过程中执行的函数
	void	(*ExecWhenStart)(void);			// 电机启动时执行的函数
	void	(*ExecWhenStop)(void);			// 电机停止时执行的函数

}TimerCtrl;


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

#ifndef 	SPEED_PSC
#define		SPEED_PSC			72
#endif

#define		SPEED_TIME			100		// 0.1ms

//---------------------------------------------------------------------------------------------
// TIM1:	异步控制速度控制(0.1毫秒中断)

#ifndef 	SPEED_TIM
#define		SPEED_TIM					TIM1
#endif

#ifndef 	MOTO1_TIM
#define		MOTO1_TIM					TIM2
#endif

#ifndef 	MOTO2_TIM
#define		MOTO2_TIM					TIM3
#endif

#ifndef 	MOTO3_TIM
#define		MOTO3_TIM					TIM4
#endif

#ifndef 	MOTO4_TIM
#define		MOTO4_TIM					TIM5
#endif

#ifndef 	MOTO5_TIM
#define		MOTO5_TIM					TIM6
#endif

#ifndef 	MOTO6_TIM
#define		MOTO6_TIM					TIM7
#endif

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

// 电机 定时器时钟，对应管脚，中断配置
#ifndef MOOT_TIM_PSC
#define	MOOT_TIM_PSC	120			// 预分频
#endif

#define	MOTO_FIN		((SYSCLK/MOOT_TIM_PSC))		// 时钟频率 = ((RTE_SYSCLK / MOOT_TIM_PSC)/(@25k时校准系数))

#define	SPD_MIN			10			// 最慢脉冲频率 单位: 脉冲/秒 = (MOTO_FIN / 65535)
#define	SPD_MAX			50000		// 最快脉冲频率 单位: 脉冲/秒 = (MOTO_FIN / 10)

#define	ADD_MIN			1			// 最小加速度
#define	ADD_MAX			100000		// 最大加速度 	单位: pps/s

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


OutFunc SoftPwmProc 	= SetOutputNull;

OutFunc Moto1PulseOn 	= SetOutputNull;
OutFunc Moto1PulseOff 	= SetOutputNull;
OutFunc Moto1SignOn 	= SetOutputNull;
OutFunc Moto1SignOff 	= SetOutputNull;

OutFunc  Moto2PulseOn 	= SetOutputNull;
OutFunc  Moto2PulseOff 	= SetOutputNull;
OutFunc  Moto2SignOn 	= SetOutputNull;
OutFunc  Moto2SignOff 	= SetOutputNull;

OutFunc  Moto3PulseOn 	= SetOutputNull;
OutFunc  Moto3PulseOff 	= SetOutputNull;
OutFunc  Moto3SignOn 	= SetOutputNull;
OutFunc  Moto3SignOff 	= SetOutputNull;

OutFunc  Moto4PulseOn 	= SetOutputNull;
OutFunc  Moto4PulseOff 	= SetOutputNull;
OutFunc  Moto4SignOn 	= SetOutputNull;
OutFunc  Moto4SignOff 	= SetOutputNull;

OutFunc  Moto5PulseOn 	= SetOutputNull;
OutFunc  Moto5PulseOff 	= SetOutputNull;
OutFunc  Moto5SignOn 	= SetOutputNull;
OutFunc  Moto5SignOff 	= SetOutputNull;

OutFunc  Moto6PulseOn 	= SetOutputNull;
OutFunc  Moto6PulseOff 	= SetOutputNull;
OutFunc  Moto6SignOn 	= SetOutputNull;
OutFunc  Moto6SignOff 	= SetOutputNull;

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

// 脉冲 开
void SetMotoPulseOn(int curAux, int syncList[6])
{
	if ((curAux == 1) || (syncList[1-1] == 1))
	{
		Moto1PulseOn();
	}
	if ((curAux == 2) || (syncList[2-1] == 1))
	{
		Moto2PulseOn();
	}
	if ((curAux == 3) || (syncList[3-1] == 1))
	{
		Moto3PulseOn();
	}
	if ((curAux == 4) || (syncList[4-1] == 1))
	{
		Moto4PulseOn();
	}
	if ((curAux == 5) || (syncList[5-1] == 1))
	{
		Moto5PulseOn();
	}
	if ((curAux == 6) || (syncList[6-1] == 1))
	{
		Moto6PulseOn();
	}
}

// 脉冲 关
void SetMotoPulseOff(int curAux, int syncList[6])
{
	if ((curAux == 1) || (syncList[1-1] == 1))
	{
		Moto1PulseOff();
	}
	if ((curAux == 2) || (syncList[2-1] == 1))
	{
		Moto2PulseOff();
	}
	if ((curAux == 3) || (syncList[3-1] == 1))
	{
		Moto3PulseOff();
	}
	if ((curAux == 4) || (syncList[4-1] == 1))
	{
		Moto4PulseOff();
	}
	if ((curAux == 5) || (syncList[5-1] == 1))
	{
		Moto5PulseOff();
	}
	if ((curAux == 6) || (syncList[6-1] == 1))
	{
		Moto6PulseOff();
	}
}

// 方向 开
void SetMotoSignOn(int curAux, int syncList[6])
{
	if ((curAux == 1) || (syncList[1-1] == 1))
	{
		Moto1SignOn();
	}
	if ((curAux == 2) || (syncList[2-1] == 1))
	{
		Moto2SignOn();
	}
	if ((curAux == 3) || (syncList[3-1] == 1))
	{
		Moto3SignOn();
	}
	if ((curAux == 4) || (syncList[4-1] == 1))
	{
		Moto4SignOn();
	}
	if ((curAux == 5) || (syncList[5-1] == 1))
	{
		Moto5SignOn();
	}
	if ((curAux == 6) || (syncList[6-1] == 1))
	{
		Moto6SignOn();
	}
}

// 方向 关
void SetMotoSignOff(int curAux, int syncList[6])
{
	if ((curAux == 1) || (syncList[1-1] == 1))
	{
		Moto1SignOff();
	}
	if ((curAux == 2) || (syncList[2-1] == 1))
	{
		Moto2SignOff();
	}
	if ((curAux == 3) || (syncList[3-1] == 1))
	{
		Moto3SignOff();
	}
	if ((curAux == 4) || (syncList[4-1] == 1))
	{
		Moto4SignOff();
	}
	if ((curAux == 5) || (syncList[5-1] == 1))
	{
		Moto5SignOff();
	}
	if ((curAux == 6) || (syncList[6-1] == 1))
	{
		Moto6SignOff();
	}
}

//---------------------------------------
// 电机控制宏定义

#define	USE_MOTO(x)	\
TimerCtrl g_moto##x##TimerCtrl; \
 \
/*---------------------------------------*/ \
void InitStm32Moto##x##OutFunc(OutFunc stepOn, OutFunc StepOff, OutFunc dirOn, OutFunc dirOff) \
{ \
	Moto##x##PulseOn = stepOn; \
	Moto##x##PulseOff = StepOff; \
	Moto##x##SignOn = dirOn; \
	Moto##x##SignOff = dirOff; \
}\
\
void InitSTM32Moto##x##Ctrl(void) \
{ \
	LL_TIM_ClearFlag_UPDATE(MOTO##x##_TIM);				/* 清除溢出中断标志 */ \
	LL_TIM_DisableIT_UPDATE(MOTO##x##_TIM);				/* 关闭中断 */ \
	LL_TIM_DisableCounter(MOTO##x##_TIM);				/* 关闭定时器 */ \
\
	memset(&g_moto##x##TimerCtrl, 0, sizeof(TimerCtrl)); \
	g_moto##x##TimerCtrl.pulseMode = MODE_STEPDIR;		/* 脉冲模式 */ \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 中断处理 */ \
void STM32Moto##x##IntProc(void) \
{ \
	if (LL_TIM_IsActiveFlag_UPDATE(MOTO##x##_TIM) != RESET)			/* 检查指定的TIM中断发生与否 */ \
	{ \
		LL_TIM_ClearFlag_UPDATE(MOTO##x##_TIM);						/* 清除TIMx的中断待处理位 */ \
 \
		if ((g_moto##x##TimerCtrl.currentPos >= g_moto##x##TimerCtrl.totalPos) ||	/* 发送脉冲完成 */ \
			(g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP) ||					/* 停止信号 */ \
			0	) \
		{ \
			SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList);		/* 关闭脉冲输出 */ \
			SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); 		/* 关闭方向输出 */ \
			LL_TIM_DisableCounter(MOTO##x##_TIM);					/* 关闭定时器 */ \
			if (g_moto##x##TimerCtrl.runState > STM32MOTO_STOP) \
			{ \
				g_moto##x##TimerCtrl.runState = STM32MOTO_STOP;		/* 设置运行状态 */ \
				g_moto##x##TimerCtrl.startrunflag = 0;				/* 关闭运行标志 */ \
				if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
				{ \
					g_moto##x##TimerCtrl.ExecWhenStop(); \
				} \
			} \
		} \
		else \
		{ \
			LL_TIM_SetAutoReload(MOTO##x##_TIM, (u16)((MOTO_FIN/g_moto##x##TimerCtrl.currentSpd)/2)); 	 	/* 设置计数值 */ \
 \
			if (g_moto##x##TimerCtrl.pulseState == 0) \
			{ \
				g_moto##x##TimerCtrl.pulseState = 1; \
				g_moto##x##TimerCtrl.currentPos++;								/* 计数值递增 */ \
				g_moto##x##TimerCtrl.realpos += g_moto##x##TimerCtrl.dir; \
 \
				if (g_moto##x##TimerCtrl.pulseMode == MODE_CWCCW)				/* 正反脉冲 */ \
				{ \
					if (g_moto##x##TimerCtrl.dir == 1) \
					{ \
						SetMotoPulseOn(x,g_moto##x##TimerCtrl.syncList); 	/*SetMoto##x##PulseOn(); */\
					} \
					else \
					{ \
						SetMotoSignOn(x,g_moto##x##TimerCtrl.syncList); 	/*SetMoto##x##SignOn(); */\
					} \
				} \
				else \
				{ \
					SetMotoPulseOn(x,g_moto##x##TimerCtrl.syncList); 	/*SetMoto##x##PulseOn(); */\
				} \
			} \
			else \
			{ \
				g_moto##x##TimerCtrl.pulseState = 0; \
				if (g_moto##x##TimerCtrl.pulseMode == MODE_CWCCW) \
				{ \
					SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList);		/*SetMoto##x##PulseOff();*/ \
					SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); 		/*SetMoto##x##SignOff();*/ \
				} \
				else \
				{ \
					SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList);		/*SetMoto##x##PulseOff();*/ \
				} \
			} \
 \
			/* 加速中 */ \
			if (g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) \
			{ \
				if (g_moto##x##TimerCtrl.currentPos >= (g_moto##x##TimerCtrl.totalPos>>1)) \
				{/* 当前位移大于总位移的一半,进入降速状态 */ \
					g_moto##x##TimerCtrl.runState = STM32MOTO_DEC; \
					g_moto##x##TimerCtrl.decPos = g_moto##x##TimerCtrl.currentPos;		/* 减速点位移 */ \
					/*printf("enter dec, spd=%d, pos=%d\r\n", g_moto##x##TimerCtrl.currentSpd, g_moto##x##TimerCtrl.decPos);*/ \
				} \
			} \
 \
			/* 匀速中 */ \
			if 	(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) \
			{ \
				if (g_moto##x##TimerCtrl.currentPos >= g_moto##x##TimerCtrl.decPos) \
				{ \
					g_moto##x##TimerCtrl.runState = STM32MOTO_DEC; \
					g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.addPPSG;		/* 当前加速度 */ \
				} \
			} \
		} \
	} \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：电机速度控制 */ \
/* 速度控制中断内响应 */ \
void STM32Moto##x##Speed (void) \
{ \
	/* 停止中 */ \
	if (g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP || g_moto##x##TimerCtrl.startrunflag == 0) \
	{ \
		return; \
	} \
	if ((g_moto##x##TimerCtrl.blockRunflag == 0) && (g_moto##x##TimerCtrl.funTestTime != 0))		/* 阻塞运行标志 */ \
	{ \
		g_moto##x##TimerCtrl.funTestCount++; \
		if ((g_moto##x##TimerCtrl.funTestCount >= g_moto##x##TimerCtrl.funTestTime)) \
		{ \
			g_moto##x##TimerCtrl.funTestCount = 0; \
			if (g_moto##x##TimerCtrl.GetNormalStop != NULL)					/* 一般停止 */ \
			{ \
				if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) ||		/* 加速 */ \
					(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) ||		/* 匀速 */ \
					(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) ||		/* 计数降速 */ \
					0	) \
				{ \
					if (g_moto##x##TimerCtrl.GetNormalStop(g_moto##x##TimerCtrl.condPara1, g_moto##x##TimerCtrl.condPara2) != 0) \
					{ \
						g_moto##x##TimerCtrl.runState = STM32MOTO_NSTOP;		/* 条件停车 */ \
					} \
				} \
			} \
			/* 判断快速停止 */ \
			if (g_moto##x##TimerCtrl.GetQuickStop != NULL)		/* 快速停止 */ \
			{ \
				if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) ||		/* 加速 */ \
					(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) ||		/* 匀速 */ \
					(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) ||		/* 计数降速 */ \
					(g_moto##x##TimerCtrl.runState == STM32MOTO_NSTOP) ||	/* 条件停车 */ \
					0	) \
				{ \
					if (g_moto##x##TimerCtrl.GetQuickStop(g_moto##x##TimerCtrl.condPara1, g_moto##x##TimerCtrl.condPara2) != 0) \
					{ \
						g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.brkPPSG;	/* 当前加速度 */ \
						g_moto##x##TimerCtrl.runState = STM32MOTO_EMC;				/* 条件停车 */ \
					} \
				} \
			} \
			if (g_moto##x##TimerCtrl.ExecWhenRun != NULL)		/* 电机运行过程中执行的函数*/ \
			{ \
				g_moto##x##TimerCtrl.ExecWhenRun(); \
			} \
		} \
	} \
 \
	/* 加速中 */ \
	if (g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) \
	{ \
		g_moto##x##TimerCtrl.totalAcc += g_moto##x##TimerCtrl.currentAcc;		/* 累加速度 */ \
		do\
		{\
			if (g_moto##x##TimerCtrl.totalAcc >= 10000)\
			{\
				g_moto##x##TimerCtrl.currentSpd += 1;		/* 当前速度 */ \
				g_moto##x##TimerCtrl.totalAcc -= 10000;		/* 累加速度 */ \
			}\
			else\
			{\
				break;\
			}\
		}while(1);\
		if (g_moto##x##TimerCtrl.currentSpd >= (s32)g_moto##x##TimerCtrl.runPPS) \
		{/* 当前速度大于运行速度,进入匀速状态 */ \
			g_moto##x##TimerCtrl.runState = STM32MOTO_RUN; \
			g_moto##x##TimerCtrl.currentSpd = g_moto##x##TimerCtrl.runPPS; \
			g_moto##x##TimerCtrl.decPos = (g_moto##x##TimerCtrl.totalPos - g_moto##x##TimerCtrl.currentPos);		/* 减速点位移 */ \
			/*printf("enter run, spd=%d, decpos=%d\r\n", g_moto##x##TimerCtrl.currentSpd, g_moto##x##TimerCtrl.decPos);*/ \
		} \
	} \
 \
	/* 减速中 */ \
	if (g_moto##x##TimerCtrl.runState == STM32MOTO_DEC)  					/* 计数降速 */ \
	{ \
		g_moto##x##TimerCtrl.totalAcc -= g_moto##x##TimerCtrl.currentAcc;		/* 累加速度 */ \
		do\
		{\
			if ((g_moto##x##TimerCtrl.totalAcc < 0) && (g_moto##x##TimerCtrl.currentSpd > 0))\
			{\
				g_moto##x##TimerCtrl.currentSpd -= 1;		/* 当前速度 */ \
				g_moto##x##TimerCtrl.totalAcc += 10000;		/* 累加速度 */ \
			}\
			else\
			{\
				break;\
			}\
		}while(1);\
		if (g_moto##x##TimerCtrl.currentSpd <= (s32)g_moto##x##TimerCtrl.startPPS) \
		{ \
			g_moto##x##TimerCtrl.currentSpd = g_moto##x##TimerCtrl.startPPS; \
		} \
	} \
 \
	/* 条件停车中 */ \
	if ((g_moto##x##TimerCtrl.runState == STM32MOTO_NSTOP) ||			/* 条件停车 */ \
		(g_moto##x##TimerCtrl.runState == STM32MOTO_EMC) ||			/* 条件刹车 */ \
		0) \
	{ \
		g_moto##x##TimerCtrl.totalAcc -= g_moto##x##TimerCtrl.currentAcc;		/* 累加速度 */ \
		do\
		{\
			if ((g_moto##x##TimerCtrl.totalAcc < 0) && (g_moto##x##TimerCtrl.currentSpd > 0))\
			{\
				g_moto##x##TimerCtrl.currentSpd -= 1;		/* 当前速度 */ \
				g_moto##x##TimerCtrl.totalAcc += 10000;		/* 累加速度 */ \
			}\
			else\
			{\
				break;\
			}\
		}while(1);\
		if (g_moto##x##TimerCtrl.currentSpd <= (s32)g_moto##x##TimerCtrl.startPPS) \
		{/* 当前速度小于启动速度,进入停止状态 */ \
			SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList);		/*SetMoto##x##PulseOff();*/ \
			SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); 		/*SetMoto##x##SignOff();*/ \
			LL_TIM_DisableCounter(MOTO##x##_TIM);					/* 关闭定时器 */ \
			g_moto##x##TimerCtrl.runState = STM32MOTO_STOP;		/* 设置运行状态 */ \
			g_moto##x##TimerCtrl.startrunflag = 0;				/* 关闭运行标志 */ \
			if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
			{ \
				g_moto##x##TimerCtrl.ExecWhenStop(); \
			} \
		} \
	} \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：启动电机 */ \
int STM32Moto##x##Start(MotoCtrl * pCtrl) \
{ \
	int runflag = 1;\
	int rslt = 0;\
	if (g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP)			/* 停车/报警状态允许启动电机 */ \
	{ \
		if (pCtrl->movement == 0) /* 电机运行完毕 */\
		{ \
			runflag = 0;\
		} \
		if (pCtrl->GetNormalStop != NULL)						/* 一般停止 */ \
		{\
			rslt = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2); \
			if (rslt != 0) \
			{ \
				runflag = 0;\
			} \
		} \
		if (pCtrl->GetQuickStop != NULL)		/* 快速停止 */ \
		{ \
			rslt = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);	\
			if (rslt != 0) \
			{ \
				runflag = 0;\
			} \
		} \
		if (runflag == 0)	\
		{ \
			g_moto##x##TimerCtrl.runState = STM32MOTO_STOP; \
			g_moto##x##TimerCtrl.startrunflag = 0;				/* 关闭运行标志 */ \
			return rslt; \
		} \
		if (pCtrl->ExecWhenStart != NULL)			/* 启动执行 */ \
		{ \
			pCtrl->ExecWhenStart(); \
		} \
		/* 位移配置 */ \
		g_moto##x##TimerCtrl.currentPos = 0;							/* 当前位移 */ \
		g_moto##x##TimerCtrl.pulseState = 0;							/* 当前输出为 0 */ \
		g_moto##x##TimerCtrl.totalPos = abs(pCtrl->movement);			/* 总位移 */ \
		g_moto##x##TimerCtrl.decPos = g_moto##x##TimerCtrl.totalPos; 		/* 减速位置 */ \
		if (pCtrl->movement > 0) \
		{/* 方向 */ \
			g_moto##x##TimerCtrl.dir = 1; \
		} \
		else \
		{ \
			g_moto##x##TimerCtrl.dir = -1; \
		} \
 \
		/* 速度配置 */ \
		g_moto##x##TimerCtrl.runState = STM32MOTO_ADD;				/* 加速中 */ \
		g_moto##x##TimerCtrl.startPPS = pCtrl->startPPS; 			/* 启停速度 */ \
		g_moto##x##TimerCtrl.runPPS  = pCtrl->runPPS; 				/* 运行速度 */ \
		g_moto##x##TimerCtrl.addPPSG = pCtrl->addPPSG; 				/* 启停加速度 */ \
		g_moto##x##TimerCtrl.brkPPSG = pCtrl->brkPPSG; 				/* 刹车加速度 */ \
		g_moto##x##TimerCtrl.totalAcc = 0; 							/* 累加加速度 */ \
 \
		g_moto##x##TimerCtrl.funTestTime = pCtrl->funTestTime; 		/* 运行过程中外部函数检测间隔时间 */ \
		g_moto##x##TimerCtrl.funTestCount = 0; 						/* 运行过程中外部函数检测间隔时间计数器 */ \
\
		g_moto##x##TimerCtrl.syncList[0] = pCtrl->syncList[0]; 		/* 同步移动选择 */ \
		g_moto##x##TimerCtrl.syncList[1] = pCtrl->syncList[1]; 		/* 同步移动选择 */ \
		g_moto##x##TimerCtrl.syncList[2] = pCtrl->syncList[2]; 		/* 同步移动选择 */ \
		g_moto##x##TimerCtrl.syncList[3] = pCtrl->syncList[3]; 		/* 同步移动选择 */ \
		g_moto##x##TimerCtrl.syncList[4] = pCtrl->syncList[4]; 		/* 同步移动选择 */ \
		g_moto##x##TimerCtrl.syncList[5] = pCtrl->syncList[5]; 		/* 同步移动选择 */ \
 \
		g_moto##x##TimerCtrl.GetNormalStop = pCtrl -> GetNormalStop;		/* 一般停止 */ \
		g_moto##x##TimerCtrl.GetQuickStop = pCtrl -> GetQuickStop;			/* 快速停止 */ \
		g_moto##x##TimerCtrl.condPara1 = pCtrl -> condPara1;				/* 参数1 */ \
		g_moto##x##TimerCtrl.condPara2 = pCtrl -> condPara2;				/* 参数2 */ \
 \
		g_moto##x##TimerCtrl.blockRunflag = pCtrl -> blockRunflag;			/* 阻塞运行标志 */ \
		g_moto##x##TimerCtrl.ExecWhenRun = pCtrl -> ExecWhenRun;			/* 电机运行过程中执行的函数 */ \
		g_moto##x##TimerCtrl.ExecWhenStart = pCtrl -> ExecWhenStart;		/* 电机启动时执行的函数 */ \
		g_moto##x##TimerCtrl.ExecWhenStop = pCtrl -> ExecWhenStop;			/* 电机停止时执行的函数 */ \
 \
		/* 输入速度校验 */ \
		if (g_moto##x##TimerCtrl.startPPS > SPD_MAX) \
		{ \
			g_moto##x##TimerCtrl.startPPS = SPD_MAX; \
		} \
		if (g_moto##x##TimerCtrl.startPPS < SPD_MIN) \
		{ \
			g_moto##x##TimerCtrl.startPPS = SPD_MIN; \
		} \
		if (g_moto##x##TimerCtrl.runPPS > SPD_MAX) \
		{ \
			g_moto##x##TimerCtrl.runPPS = SPD_MAX; \
		} \
		if (g_moto##x##TimerCtrl.runPPS < SPD_MIN) \
		{ \
			g_moto##x##TimerCtrl.runPPS = SPD_MIN; \
		} \
		if (g_moto##x##TimerCtrl.startPPS > g_moto##x##TimerCtrl.runPPS) \
		{ \
			g_moto##x##TimerCtrl.startPPS = g_moto##x##TimerCtrl.runPPS; \
		} \
 \
		/* 输入加速度校验 */ \
		if (g_moto##x##TimerCtrl.addPPSG > ADD_MAX) \
		{ \
			g_moto##x##TimerCtrl.addPPSG = ADD_MAX; \
		} \
		if (g_moto##x##TimerCtrl.addPPSG <= ADD_MIN) \
		{ \
			g_moto##x##TimerCtrl.addPPSG = ADD_MIN; \
		} \
		if (g_moto##x##TimerCtrl.brkPPSG > ADD_MAX) \
		{ \
			g_moto##x##TimerCtrl.brkPPSG = ADD_MAX; \
		} \
		if (g_moto##x##TimerCtrl.brkPPSG <= ADD_MIN) \
		{ \
			g_moto##x##TimerCtrl.brkPPSG = ADD_MIN; \
		} \
		g_moto##x##TimerCtrl.currentSpd = g_moto##x##TimerCtrl.startPPS;		/* 当前速度 */ \
		g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.addPPSG;			/* 当前加速度 */ \
 \
		/* 输出配置 */ \
		SetMotoPulseOff(x,g_moto##x##TimerCtrl.syncList);			/*SetMoto##x##PulseOff();*/ \
		if (g_moto##x##TimerCtrl.pulseMode != MODE_CWCCW) \
		{ \
			if (g_moto##x##TimerCtrl.dir == 1) \
			{ \
				SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); 	/*SetMoto##x##SignOff();*/ \
			} \
			else \
			{ \
				SetMotoSignOn(x,g_moto##x##TimerCtrl.syncList); 	/*SetMoto##x##SignOn(); */\
			} \
		} \
		else \
		{ \
			SetMotoSignOff(x,g_moto##x##TimerCtrl.syncList); 		/*SetMoto##x##SignOff();*/ \
		} \
 \
		LL_TIM_DisableIT_UPDATE(MOTO##x##_TIM);				/* 关闭中断 */ \
		LL_TIM_SetAutoReload(MOTO##x##_TIM, (u16)((MOTO_FIN/g_moto##x##TimerCtrl.currentSpd)/2)); 	/* 设置计数值 */ \
		LL_TIM_EnableIT_UPDATE(MOTO##x##_TIM);				/* 开中断 */ \
		LL_TIM_EnableCounter(MOTO##x##_TIM);				/* 开定时器 */ \
		g_moto##x##TimerCtrl.startrunflag = 1;				/* 设置启动运行标志 */ \
 \
		return STM32Moto##x##Run(pCtrl); \
	} \
	else \
	{ \
		printf("moto is running, start failed\r\n"); \
		return -1;											/* 电机正在运行 */ \
	} \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：停止电机  */ \
void STM32Moto##x##Stop(void) \
{ \
	if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) ||			/* 加速 */ \
		(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) ||			/* 匀速 */ \
		(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) ||			/* 降速 */ \
		0	) \
	{ \
		g_moto##x##TimerCtrl.runState = STM32MOTO_NSTOP;				/* 进入降速停车状态 */ \
		if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
		{ \
			g_moto##x##TimerCtrl.ExecWhenStop(); \
		} \
	} \
} \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：电机运行  */ \
int STM32Moto##x##Run(MotoCtrl * pCtrl) \
{ \
	/* 判断一般停止 */ \
	do \
	{ \
		if (pCtrl == NULL || \
			g_moto##x##TimerCtrl.runState <= STM32MOTO_STOP)			/*  */ \
		{ \
			break; \
		} \
		if (pCtrl->GetNormalStop != NULL)						/* 一般停止 */ \
		{ \
			if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) ||		/* 加速 */ \
				(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) ||		/* 匀速 */ \
				(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) ||		/* 计数降速 */ \
				0	) \
			{ \
				if (pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2) != 0) \
				{ \
					g_moto##x##TimerCtrl.runState = STM32MOTO_NSTOP;		/* 条件停车 */ \
					if (g_moto##x##TimerCtrl.ExecWhenStop != NULL) \
					{ \
						g_moto##x##TimerCtrl.ExecWhenStop(); \
					} \
				} \
			} \
		} \
		/* 判断快速停止 */ \
		if (pCtrl->GetQuickStop != NULL)		/* 快速停止 */ \
		{ \
			if ((g_moto##x##TimerCtrl.runState == STM32MOTO_ADD) ||		/* 加速 */ \
				(g_moto##x##TimerCtrl.runState == STM32MOTO_RUN) ||		/* 匀速 */ \
				(g_moto##x##TimerCtrl.runState == STM32MOTO_DEC) ||		/* 计数降速 */ \
				(g_moto##x##TimerCtrl.runState == STM32MOTO_NSTOP) ||	/* 条件停车 */ \
				0	) \
			{ \
				if (pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2) != 0) \
				{ \
					g_moto##x##TimerCtrl.currentAcc = g_moto##x##TimerCtrl.brkPPSG;	/* 当前加速度 */ \
					g_moto##x##TimerCtrl.runState = STM32MOTO_EMC;				/* 条件停车 */ \
				} \
			} \
		} \
		if (pCtrl->ExecWhenRun != NULL)		/* 电机运行过程中执行的函数*/ \
		{ \
			pCtrl->ExecWhenRun(); \
		} \
		if (pCtrl->blockRunflag == 0)		/* 阻塞运行标志 */ \
		{ \
			break; \
		} \
	}while(1); \
	return g_moto##x##TimerCtrl.runState; \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：得到电机运行状态  */ \
int GetSTM32Moto##x##State(void) \
{ \
	return	g_moto##x##TimerCtrl.runState; \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：得到电机当前坐标  */ \
s32 GetSTM32Moto##x##RealPos(void) \
{ \
	return	g_moto##x##TimerCtrl.realpos; \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：设置电机当前坐标  */ \
void  SetSTM32Moto##x##RealPos(s32 pos) \
{ \
	g_moto##x##TimerCtrl.realpos = pos; \
} \
 \
/*-----------------------------------------------------------------------------------*/ \
/* 功能：设置电机输出脉冲模式  */ \
void  SetSTM32Moto##x##Mode(int mode) \
{ \
	g_moto##x##TimerCtrl.pulseMode = mode; \
} \
/*-----------------------------------------------------------------------------------*/ \

//----------------------------------------------------------------------
// 宏定义电机

#if (STM32_MOTOS_NUM >= 1)
	USE_MOTO(1);
#endif
#if (STM32_MOTOS_NUM >= 2)
	USE_MOTO(2);
#endif
#if (STM32_MOTOS_NUM >= 3)
	USE_MOTO(3);
#endif
#if (STM32_MOTOS_NUM >= 4)
	USE_MOTO(4);
#endif
#if (STM32_MOTOS_NUM >= 5)
	USE_MOTO(5);
#endif
#if (STM32_MOTOS_NUM >= 6)
	USE_MOTO(6);
#endif

//----------------------------------------------------------------------
// 初始化速度控制定时器中断
void InitSTM32Speed(void)
{
#if (STM32_MOTOS_NUM >= 1 || STM32_SOFT_PWM > 0)
	LL_TIM_ClearFlag_UPDATE(SPEED_TIM);			// 清除溢出中断标志
	LL_TIM_EnableIT_UPDATE(SPEED_TIM);			// 开启溢出中断
	LL_TIM_EnableCounter(SPEED_TIM);			// 使能定时器
#endif
}

/*-----------------------------------------------------------------------------------*/
// 速度控制中断处理
void STM32MotoSpeedIntProc(void)
{
#if (STM32_MOTOS_NUM >= 1 || STM32_SOFT_PWM > 0)

	if (LL_TIM_IsActiveFlag_UPDATE(SPEED_TIM) != RESET)
	{
		LL_TIM_ClearFlag_UPDATE(SPEED_TIM);
	#if (STM32_SOFT_PWM > 0)
		SoftPwmProc();
	#endif
	#if (STM32_MOTOS_NUM >= 1)
		STM32Moto1Speed();
	#endif
	#if (STM32_MOTOS_NUM >= 2)
		STM32Moto2Speed();
	#endif
	#if (STM32_MOTOS_NUM >= 3)
		STM32Moto3Speed();
	#endif
	#if (STM32_MOTOS_NUM >= 4)
		STM32Moto4Speed();
	#endif
	#if (STM32_MOTOS_NUM >= 5)
		STM32Moto5Speed();
	#endif
	#if (STM32_MOTOS_NUM >= 6)
		STM32Moto6Speed();
	#endif
	}
#endif
}

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

void InitSTM32SoftPwm(OutFunc pwmProc)
{
#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
	InitSTM32Speed();
#endif
	SoftPwmProc = pwmProc;
}

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

void InitSTM32Motos(void)
{
#if (STM32_MOTOS_NUM > 0)
	InitSTM32Speed();
#endif

#if (STM32_MOTOS_NUM >= 1)
	InitSTM32Moto1Ctrl();
#endif

#if (STM32_MOTOS_NUM >= 2)
	InitSTM32Moto2Ctrl();
#endif

#if (STM32_MOTOS_NUM >= 3)
	InitSTM32Moto3Ctrl();
#endif

#if (STM32_MOTOS_NUM >= 4)
	InitSTM32Moto4Ctrl();
#endif

#if (STM32_MOTOS_NUM >= 5)
	InitSTM32Moto5Ctrl();
#endif

#if (STM32_MOTOS_NUM >= 6)
	InitSTM32Moto6Ctrl();
#endif

//	AddShellCmd("TMR", "test stm32 moto run", TestStm32Motos);
}

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

//终端调试接口
void TestStm32Motos(char * para1, char * para2)
{
#if (STM32_MOTOS_NUM >= 1)
	int p1, p2;
	MotoCtrl 	mtcfg;				// 配置参数

	memset(&mtcfg, 0, sizeof(MotoCtrl));

	if (para1 == NULL || para2 == NULL)
	{
		return;
	}

	printf("para1=%s, para2=%s\r\n", para1, para2);

	p1 = 0;
	p2 = 0;
	if (strcmp(para1, "") != 0)
	{
		p1 = atoi(para1);
		if (strcmp(para2, "") != 0)
		{
			p2 = atoi(para2);
		}
	}

	if (p1 == 0)
	{//读取电机状态
		if (p2 == 0)
		{
			g_moto1TimerCtrl.realpos = 0;
		}
		else if (p2 == 1)
		{
			printf("moto1pos=%ld\r\n", g_moto1TimerCtrl.realpos);
		}

		if (p2 == 100)
		{// 电机状态
			printf("moto1 state =%d\r\n",GetSTM32Moto1State());
		}
	}

	if (p1 == 1)
	{//moto1
		if (p2 == 0)
		{// 停止电机
			STM32Moto1Stop();
		}
		else
		{
			mtcfg.movement = p2;		// 位移量
			mtcfg.startPPS = 300;
			mtcfg.runPPS = 	1000;
			mtcfg.addPPSG = 10;
			mtcfg.brkPPSG = 50;
			mtcfg.GetNormalStop = NULL;
			mtcfg.GetQuickStop = NULL;
			mtcfg.condPara1 = 0;
			mtcfg.condPara2 = 0;
			STM32Moto1Start (&mtcfg);
		}
	}
#if (STM32_MOTOS_NUM >= 2)
	if (p1 == 2)
	{//moto2
		if (p2 == 0)
		{// 停止电机
			STM32Moto2Stop();
		}
		else
		{
			mtcfg.movement = p2;		// 位移量
			mtcfg.startPPS = 300;
			mtcfg.runPPS = 	1000;
			mtcfg.addPPSG = 10;
			mtcfg.brkPPSG = 50;
			mtcfg.GetNormalStop = NULL;
			mtcfg.GetQuickStop = NULL;
			mtcfg.condPara1 = 0;
			mtcfg.condPara2 = 0;
			STM32Moto2Start (&mtcfg);
		}
	}
#endif
#if (STM32_MOTOS_NUM >= 3)
	if (p1 == 3)
	{//moto3
		if (p2 == 0)
		{// 停止电机
			STM32Moto3Stop();
		}
		else
		{
			mtcfg.movement = p2;		// 位移量
			mtcfg.startPPS = 50;
			mtcfg.runPPS = 	7000;
			mtcfg.addPPSG = 5;
			mtcfg.brkPPSG = 50;
			mtcfg.GetNormalStop = NULL;
			mtcfg.GetQuickStop = NULL;
			mtcfg.condPara1 = 0;
			mtcfg.condPara2 = 0;
			STM32Moto3Start (&mtcfg);
		}
	}
#endif
#if (STM32_MOTOS_NUM >= 4)
	if (p1 == 4)
	{//moto4
		if (p2 == 0)
		{// 停止电机
			STM32Moto4Stop();
		}
		else
		{
			mtcfg.movement = p2;		// 位移量
			mtcfg.startPPS = 50;
			mtcfg.runPPS = 	4000;
			mtcfg.addPPSG = 5;
			mtcfg.brkPPSG = 50;
			mtcfg.GetNormalStop = NULL;
			mtcfg.GetQuickStop = NULL;
			mtcfg.condPara1 = 0;
			mtcfg.condPara2 = 0;
			STM32Moto4Start (&mtcfg);
		}
	}
#endif
#if (STM32_MOTOS_NUM >= 5)
	if (p1 == 5)
	{//moto5
		if (p2 == 0)
		{// 停止电机
			STM32Moto5Stop();
		}
		else
		{
			mtcfg.movement = p2;		// 位移量
			mtcfg.startPPS = 100;
			mtcfg.runPPS = 	2000;
			mtcfg.addPPSG = 50;
			mtcfg.brkPPSG = 50;
			mtcfg.GetNormalStop = NULL;
			mtcfg.GetQuickStop = NULL;
			mtcfg.condPara1 = 0;
			mtcfg.condPara2 = 0;
			STM32Moto5Start (&mtcfg);
		}
	}
#endif
#if (STM32_MOTOS_NUM >= 6)
	if (p1 == 6)
	{//moto5
		if (p2 == 0)
		{// 停止电机
			STM32Moto6Stop();
		}
		else
		{
			mtcfg.movement = p2;		// 位移量
			mtcfg.startPPS = 100;
			mtcfg.runPPS = 	2000;
			mtcfg.addPPSG = 50;
			mtcfg.brkPPSG = 50;
			mtcfg.GetNormalStop = NULL;
			mtcfg.GetQuickStop = NULL;
			mtcfg.condPara1 = 0;
			mtcfg.condPara2 = 0;
			STM32Moto6Start (&mtcfg);
		}
	}

#endif
	
#endif

}