optical/EMBOS/Users/EmbBase/motos.h

#ifndef __MOTOS_H__
#define __MOTOS_H__

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

#include "embfpga.h"

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

#define		AXIS_NUM		6		// 控制轴的个数

//-------------------------------------------------------------------------------
// 电机配置结构
typedef struct
{
	u16 axisConfig;		// 实轴输出配置
	u16 poutType;		// 脉冲输出模式(CW/CCW，PULSE/DIR)
	u16 spdSource;		// 实轴速度控制(虚轴1，虚轴2，虚轴3，编码器)
	u16 datSource;		// 实轴数据获取(硬件插补，编码器随动，编码器映射)
}AxisConfig;


#ifdef _IN_MOTOS_C



#endif
//-------------------------------------------------------------------------------
//---------------------------------------------------------------


typedef struct
{
	int enFlag;
	s32 motosPos[AXIS_NUM];

}MotosPara;

#ifdef _IN_MOVECTRL_C

MotosPara g_motosPara;

#else

extern MotosPara g_motosPara;

#endif

//---------------------------------------------------------------
// 实轴输出配置
/*
	---------------------------------------------------------------------------------------------------------------------------------
	|		b0		|		b1		|		b2		|		b3		|		b4		|		b5		|		b6		|		b7		|
	---------------------------------------------------------------------------------------------------------------------------------
	| 脉冲输出允许	| 	A输出控制	|	B输出控制	| 	电机伺服ON	|	报警停车允许	| 	报警电平选择	|				| 				|
	---------------------------------------------------------------------------------------------------------------------------------
	---------------------------------------------------------------------------------------------------------------------------------
	|		b8		|		b9		|		b10		|		b11		|		b12		|		b13		|		b14		|		b15		|
	---------------------------------------------------------------------------------------------------------------------------------
	|				| 				|				| 				|				| 				|				| 	配置允许		|
	---------------------------------------------------------------------------------------------------------------------------------

	// 其中 某个位为1，是输出相应的信号
*/

#define	POUT_EN			0x0001		// 输出允许
#define	POA_EN			0x0002		// A允许输出
#define	POB_EN			0x0004		// B允许输出
#define	SON_EN			0x0008		// 伺服ON允许

#define	ALM_EN			0x0010		// 报警停车允许
#define	ALM_LEVH		0x0020		// 报警电平为高

#define	CONFIG_EN		0x8000		// 配置允许

#define	DEFAULT_CFG		(CONFIG_EN|SON_EN|ALM_LEVH)		// 默认配置

#define	IO_CTRL_CFG		(CONFIG_EN|ALM_EN|ALM_LEVH)		// IO控制电机配置

// 无报警
#define	MOTOEN_CFG_NALM		(CONFIG_EN|POUT_EN|SON_EN|ALM_LEVH)						// 通用电机配置
#define	MOTOENA_CFG_NALM	(CONFIG_EN|POUT_EN|POA_EN|SON_EN|ALM_LEVH)				// 通用电机配置
#define	MOTOENB_CFG_NALM	(CONFIG_EN|POUT_EN|POB_EN|SON_EN|ALM_LEVH)				// 通用电机配置
#define	MOTOENAB_CFG_NALM	(CONFIG_EN|POUT_EN|POA_EN|POB_EN|SON_EN|ALM_LEVH)		// 通用电机配置

// 有报警
#define	MOTOEN_CFG			(CONFIG_EN|POUT_EN|SON_EN|ALM_EN|ALM_LEVH)					// 通用电机配置
#define	MOTOENA_CFG			(CONFIG_EN|POUT_EN|POA_EN|SON_EN|ALM_EN|ALM_LEVH)			// 通用电机配置
#define	MOTOENB_CFG			(CONFIG_EN|POUT_EN|POB_EN|SON_EN|ALM_EN|ALM_LEVH)			// 通用电机配置
#define	MOTOENAB_CFG		(CONFIG_EN|POUT_EN|POA_EN|POB_EN|SON_EN|ALM_EN|ALM_LEVH)	// 通用电机配置

// 有报警(低电平报警)
#define	MOTOEN_CFG_LALM			(CONFIG_EN|POUT_EN|SON_EN|ALM_EN)					// 通用电机配置
#define	MOTOENA_CFG_LALM		(CONFIG_EN|POUT_EN|POA_EN|SON_EN|ALM_EN)			// 通用电机配置
#define	MOTOENB_CFG_LALM		(CONFIG_EN|POUT_EN|POB_EN|SON_EN|ALM_EN)			// 通用电机配置
#define	MOTOENAB_CFG_LALM		(CONFIG_EN|POUT_EN|POA_EN|POB_EN|SON_EN|ALM_EN)		// 通用电机配置

// 三输出电机 无报警
#define	MOTO3EN_CFG_NALM		(CONFIG_EN|POUT_EN|ALM_LEVH)						// 通用电机配置
#define	MOTO3ENA_CFG_NALM		(CONFIG_EN|POUT_EN|POB_EN|ALM_LEVH)					// 通用电机配置
#define	MOTO3ENB_CFG_NALM		(CONFIG_EN|POUT_EN|POA_EN|ALM_LEVH)					// 通用电机配置
#define	MOTO3ENC_CFG_NALM		(CONFIG_EN|POUT_EN|POA_EN|POB_EN|SON_EN|ALM_LEVH)	// 通用电机配置
#define	MOTO3ENABC_CFG_NALM		(CONFIG_EN|POUT_EN|SON_EN|ALM_LEVH)	// 通用电机配置

// 三输出电机 有报警
#define	MOTO3EN_CFG				(CONFIG_EN|POUT_EN|ALM_EN|ALM_LEVH)					// 通用电机配置
#define	MOTO3ENA_CFG			(CONFIG_EN|POUT_EN|POB_EN|ALM_EN|ALM_LEVH)			// 通用电机配置
#define	MOTO3ENB_CFG			(CONFIG_EN|POUT_EN|POA_EN|ALM_EN|ALM_LEVH)			// 通用电机配置
#define	MOTO3ENC_CFG			(CONFIG_EN|POUT_EN|POA_EN|POB_EN|SON_EN|ALM_EN|ALM_LEVH)			// 通用电机配置
#define	MOTO3ENABC_CFG			(CONFIG_EN|POUT_EN|SON_EN|ALM_EN|ALM_LEVH)			// 通用电机配置


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

#define	POUTA_EN		MOUT_SEL_HIGH	// 输出脉冲A允许为高电平
#define	POUTA_DIS		MOUT_SEL_LOW	// 输出脉冲A禁止为低电平
#define	POUTB_EN		MOUT_EN_HIGH	// 输出脉冲B允许为高电平
#define	POUTB_DIS		MOUT_EN_LOW		// 输出脉冲B禁止为低电平
#define	SERVO_OFF		MOUT_SON_HIGH	// 伺服OFF为高电平
#define	SERVO_ON		MOUT_SON_LOW	// 伺服ON为低电平

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

// 电机配置结构
typedef struct
{
	int newConfig;		// 重新配置标志，= 0，不需要重新写配置，使用已有配置;  != 0，是新的配置，需要重新写入，同时刷新记录配置

	// 电机报警配置
	AxisConfig axisConfig[AXIS_NUM];

}MotosConfig;

// 电机位置结构
typedef struct
{
	s32 movement[AXIS_NUM];		// 各个轴的运动量，0，无运动；其他，运动长度，单位p
	u32 extraRepeat;			// 多出的重复次数，0，无重复，其他，多出的重复次数（按照movement的运动直线延伸）
	u32 interLong;				// 插补长轴位移，必须大于或等于movement分量中的最大值
	u32 pulsePerSegment;		// 每段脉冲数

}MovementCtrl;


// 电机速度结构
typedef struct
{
	u32 startPPS;			// 启动速度
	u32 stopPPS;			// 停止速度
	u32 runPPS;				// 运动速度
	u32 slowPPS;			// 慢速速度
	u32 addPPSG;			// 升速加速度
	u32 decPPSG;			// 降速加速度
	u32 brkPPSG;			// 刹车加速度
}SpeedCtrl;

//---------------------------------------------------------------
// 直线插补运动控制结构
typedef struct
{
	u32 vAxisId;				// 使用虚轴ID

	MotosConfig motosConfig;	// 参与电机配置

	MovementCtrl mvmtPara;		// 位移控制参数

	SpeedCtrl spdPara;			// 速度控制参数

	int stopMode;				// 停止运动方式
									// 0，降速到停止速度以下停止。
									// 1，降速，到整表切换时停止。

	int errInfo;				// 错误信息

	// 回调函数及参数
	void (*Delay)(u32);			// 延时回调函数

	void (*RefreshPosition)(u32, s32*);	// 坐标刷新回调函数
	u32 refPosPara;						// 坐标刷新参数

	int (*WorkBeforeRun)(u32, u32);		// 运行前准备工作回调函数
	int (*WorkAfterRun)(u32, u32);		// 运行后准备工作回调函数
	u32 workPara1, workPara2;			// 工作参数

	int (*GetSlowDown)(u32,u32);		// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	int (*GetNormalStop)(u32,u32);		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	int (*GetQuickStop)(u32,u32);		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	int (*GetEmergencyStop)(u32,u32);	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	u32 condPara1, condPara2;			// 条件函数参数

}InterMoveCtrl;

// 最长支持插补数据
#define	MAX_INTER_SIZE			S32_MAX

#define	STOP_MODE_SPD			0		// 速度低于最低速度则停止
#define	STOP_MODE_TAB			1		// 在表运行完成后停止



#define	TOSLOWMUTI			5
//------------------------------------------------------------------------------------------------

#define	SUPPORT_SEGMENT		15		// 支持1段数据, 最多支持16段

typedef struct
{
	u32 interLong;								// 插补长轴位移，必须大于或等于movement分量中的最大值
				
	u16 sineOut[AXIS_NUM];						// 正弦输出标志. 0, 非正弦输出, 1, 正弦输出
	u16 segmnetNum[AXIS_NUM];					// 有效段数，支持0--100
	u32 startEsc[AXIS_NUM];						// 起始跳过脉冲数
	u32 vppSegment[AXIS_NUM];					// 虚轴对应每段脉冲数
	u16 movement[AXIS_NUM][SUPPORT_SEGMENT+1];	// 各个轴的运动数据

}MovementDat;

// 数据交换结构
typedef struct
{
	// 位移控制参数
	s32 fillCount;			// 填充计数器（写入FPGA中的数据计数）
	s32 execCount;			// 执行计数器（已经执行完成的数据计数，也可以认为是正在执行的数据idx）
	s32 fillLimit;			// 填充限制,在刹车过程中允许填充的数据个数
	int bufsta;				// 缓冲区状态

	u32 lastEndPPS;			// 上段结束时的速度，也就是本段（正在执行段）的起始速度（每次表结束时设置）
	u32 lastrunPPS;			// 上段的运行速度

	int runstat;			// 运行状态，= 0，正常运行状态。 = 1，进入停止状态。 = 2，进入刹车状态
	int errInfo;			// 错误信息
	int waitNoData;			// 无数据时等待标志, = 0，不等待。= 1，等待
	int emptyCount;
	int dataBegIndex;		// 数据索引起始
	int exeingIdx;			// 正在执行数据的索引
	//------------
	u32 mvmtStatus;			// 运动数据状态，0，无效，1，有效

	MovementDat mvmtdat;	// 位移数据

	// 速度控制参数
	u32 lowPPS;				// 本段启动速度
	u32 runPPS;				// 本段运行目标速度
	u32 calcGap;			// 本段加速度

}MoveSwapData;

#define	CRST_NORMAL		0	// 正常运行状态
#define	CRST_NSTOP		1	// 停止状态
#define	CRST_QSTOP		2	// 停止状态
#define	CRST_BREAK		3	// 刹车状态


// 多轴连续直线插补运动控制结构
typedef struct
{
	u32 vAxisId;						// 使用虚轴ID

	MotosConfig motosConfig;			// 参与电机配置
	MoveSwapData mvData;				// 位移和速度刷新结构

	s32 dataBegIndex;

	// 停车速度控制参数
	u32 stopPPS;			// 停车速度
	u32 nstopPPSG;			// 普通停止加速度
	u32 qstopPPSG;			// 快速停止加速度

	// 回调函数及参数
	void (*Delay)(u32);					// 延时回调函数

	// 数据填充函数
	int (*RefreshData)(u32, u32);		// 数据刷新函数
	u32 refDatPara;

	void (*RefreshPosition)(u32, s32*);	// 坐标刷新回调函数
	u32 refPosPara;						// 坐标刷新参数

	int (*WorkBeforeRun)(u32, u32);		// 运行前准备工作回调函数
	int (*WorkAfterRun)(u32, u32);		// 运行前准备工作回调函数
	u32 workPara1, workPara2;			// 工作参数

	int (*GetSlowDown)(u32,u32);		// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	int (*GetNormalStop)(u32,u32);		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	int (*GetQuickStop)(u32,u32);		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	int (*GetEmergencyStop)(u32,u32);	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	u32 condPara1, condPara2;			// 条件函数参数

}ContinuousInterMoveCtrl;

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


// 初始化电机
void InitMotos(void);

// 读取电机位置
int GetMotosCounter(s32 * position);

// 设置电机位置
void SetMotosCounter(s32 * position);

// 刷新电机位置
void RefreshMotosPos(s32 * poslist);

// 读取电机计数器
s32 GetMotoCounter(int axisIdx);
void SetMotoCounter(int axisIdx, s32 pos);

void MotoServoCtrl(int axisIdx, int onoff);

void SetMotosConfig(MotosConfig * pConfig);					// 设置电机配置


// 多轴插补运动，最多支持6轴同步
int InterpolationMotion(InterMoveCtrl * pCtrl);

// 多轴连续直线插补运动，最多支持6轴同步
int ContinuousInterpolation(ContinuousInterMoveCtrl * pCtrl);

u32 CalcDisplacement(u32 pps1, u32 pps2, u32 calcTime);		// 计算加减速需要的位移
u32 CalcGapTime(u32 disp, u32 pps1, u32 pps2);				// 计算某位移下速度变化需要的时间间隔参数
u32 CalcEndSpd(u32 disp, u32 pps1, s32 addgap);


#endif