#define _IN_OPTICAL_C
#include "optical.h"

#if (CUR_MACHINE == MACH_OPTICAL)
#include "opticalpara.h"
#include "console.h"
#include "crc16.h"
#include "modbus_m.h"

//-------------------------------函数定义----------------------------------------------------
//初始化
void OpticalTask(void);												//
void OpticalAsyncTask(void);										// 异步执行任务
void OpticalTimerTask(void);										// 定时执行任务
int StartButtonTask(void);											// 开始按钮任务
int SendOpticalTask(void);											// 异步执行任务-自动送光纤任务
int sendOpticalTaskWhenRun(void);									// 布线过程中送光纤任务

void RefreshStatus(void);

//工作控制
int OpticalGotoOffsetPos(void);//去某个位置
int OpticalGotoFinishPos(void);

int isEmptyMaterialSta(u16 len);//无料检测

int IsSignalTimeout(u8 (* func)(void), int timeoutMs, int err); // 等气缸待信号是否超时
#define CYLINDER_TIMEOUT						5000			// 超时等待时间
#define SIGNAL_TIMEOUT_ENABLE					1				// 等待信号超时函数使能

int toActSpinLen(int theorySpin);//计算吐丝补偿

void getOptBorderWhenWork(void);

//----------------------------------------------电机相关-----------------------------------------------

void InitOpticalMotos(u32 delay);									// 初始化电机
void SetOpticalZeroPosValue(s32 * list);							// 将*list设置为零位坐标值
void GetMvBHSpdFromLevel(MotoSpd * pSpd, int spdLev);				// 根据手动移动速度等级获取手动移动速度


//---------------X电机----------------------------
#define	XYAxisMMToPulse(mm)				MM_TO_PULSE(mm)
#define	XYAxisMMPSToPPS(mmps)			MMPS_TO_PPS(mmps)
#define	XYAxisMMPSSToPPSG(mmpss)		MMPSS_TO_PPSG(mmpss)

#define	FXZ_MVMT		(2000)	// 2000 mm
#define	FXZ_FXMV		(20)	// 20 mm

#define	MVXBH_MVMT		(2000)	// 2000 mm

u8 MoveXLimitPosi(void);												// X正向限位(包含硬件限位和软限位)
u8 MoveXLimitNega(void);												// X反向限位(包含硬件限位和软限位)
u8 MoveXBHLimitPosi(u32 para);											// X移动正限位
u8 MoveXBHLimitNega(u32 para);											// X移动负限位
int MoveXBHPrec(u32 para);												// 先决条件
int MoveXBHStop(u32 para);												// 停止条件
int MoveXBHSlow(u32 para);												// 慢速条件
int XMoveConditions(void);												// X移动判断
int MoveXByHand(int dir, int spdLev);									// X手动移动
int MoveXAsOffset(s32 ofstx, MotoSpd * pSpd);							// X移动偏移量
int FindXZero(void);													// X电机找零

//---------------Y电机---------------
int MoveYByHand(int dir, int spdLev);
int FindYZero(int fzdir);
u8 MoveYBHLimitPosi(u32 para);
u8 MoveYBHLimitNega(u32 para);

//---------------XY--------------------
int MoveXYEmcScan(u32 para1, u32 para2);
int MoveXYQStopScan(u32 para1, u32 para2);
int MoveXYNStopScan(u32 para1, u32 para2);

//---------------升降电机---------------
int MoveU1ByHand(int dir, int mmps);
int MoveU1AsOffset(s32 ofstu1, MotoSpd * pSpd);
int MoveU1ToPos(s32 posu1, MotoSpd * pSpd);
int FindU1Zero(int fzdir);
void U1HighTest(void);
int MoveUToUpper(void);
int MoveZU1ToUpper(void);
#define	MoveHeadToUpper	MoveUToUpper

//---------------旋转电机---------------
s16 Rot001ToPitk(s32 ang001);
int MoveR1ByHand(int dir, int dps);
int MoveR1AsOffset(s32 ofstr1, MotoSpd * pSpd);
int MoveR1ToPos(s32 posr, MotoSpd * pSpd);
int FindR1Zero(void);

//---------------送料电机---------------
int MoveSToPos(s32 poss, MotoSpd * pSpd);
#define MTS1_IsSpin1PulseToS1Pulse(Pulse)  		Pulse*10//吐丝时：数据文件是1 pulse/mm，转换成电机脉冲当量（10pulse/mm）
#define MTS1_IsWire001PulseToS1Pulse(Pulse)  		Pulse/10//布线时：数据文件是100 pulse/mm，转换成电机脉冲当量（10pulse/mm）
//---------------拉料电机---------------
int MoveMTP1AsOffset(u8 mode, int offset, int block);
#define MTP_PULSE_MM		8.488		// 每毫米脉冲数
#define MTP_MMS_TO_PPS(mms)		(mms)*MT_PULSE_MM
#define MTP_MMSS_TO_MMSG(mmss,puls)	 (((((mmss)*MT_PULSE_MM)/puls) != 0)?(((mmss)*MT_PULSE_MM)/puls):1)

int MTP_1PulseToP1Pulse(s32 Pulse);//数据文件是1 pulse/mm，转换成步进电机脉冲当量

//---------------XYUR---------------
void XYMotoRunTo(char * para1, char * para2);
int MoveXYToPos(s32 posx, s32 posy, MotoSpd * pSpd);
int ReadXYPos(s32 posx, s32 posy);
int MoveXYAsOffset(s32 ofstx, s32 ofsty, MotoSpd * pSpd);
int MoveXYU1R1ToPos(s32 posx, s32 posy, s32 posu1, s32 posr1, MotoSpd * pSpd);
int MoveU1AndXYToPos(s32 posu, MotoSpd * pSpd);
int MoveU1S1AsOffset(s32 ofstu, MotoSpd * pSpd);
int MoveP1S1AsOffset(s32 ofstu, MotoSpd * pSpd);
int MoveXYRS1P1ToWorkPos(DataItem * pCurDat, MotoSpd *pSpd, int rToSpinPosFlag,int s1MotoRunFlag);

//------------------------------花版相关------------------------------
void ProcessNewFile(void);// 新文件处理
void XYCompenCalcuAfterTurning(DataItem datItem);//拐弯后xy补偿计算

//------------------------------调试------------------------------
void TestMotoRun(char * para1, char * para2);
void TestWorkRun(char * para1, char * para2);
void XMotoRunTo(char * para1, char * para2);
void YMotoRunTo(char * para1, char * para2);
void U1MotoRunTo(char * para1, char * para2);
void R1MotoRunTo(char * para1, char * para2);
void S1MotoRunTo(char * para1, char * para2);
void P1MoveAsOffset(char * para1, char * para2);

void ShowMotoInfo(char * para1, char * para2);
void ShowFileInfo(char * para1, char * para2);
void TestPara(char * para1, char * para2);
void Simulation(char * para1, char * para2);

void TestFile(char * para1, char * para2);
void OpttFile(char * para1, char * para2);
void Reset(char * para1, char * para2);

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

s32 g_ofstr = 0;  //角度
s16 SP = 0;    //画笔类型

//初始化
void InitAppCallback(void)
{
	//工作控制参数初始化

	memset(&g_optWorkCtrl, 0, sizeof(OptWorkCtrl));

	InitOpticalPara();

	SetInputNameString((void*)g_opticalInputName);
	SetOutputNameString((void*)g_opticalOutputName);

	g_workCtrl.WorkRunTask = OpticalTask;
	g_workCtrl.AsynchronousTask = OpticalAsyncTask;

	AddTriggerToList(60, NULL, OpticalTimerTask);// 600ms任务

	AddButtonToList(IsStartButton, SENSOR_ON, NULL, NULL,  StartButtonTask, NULL);// 启动按钮

	AddShellCmd("TESTMO", "test the moto ctrl", TestMotoRun);
	AddShellCmd("RUN", "test the moto ctrl", TestWorkRun);
	AddShellCmd("RMOVTO", "move r to pos", R1MotoRunTo);
	AddShellCmd("MOTOINFO", "show the info", ShowMotoInfo);
	AddShellCmd("RESET", "reset", Reset);
	AddShellCmd("XYMOVETO", "move xy to pos", XYMotoRunTo);
	AddShellCmd("FILEINFO", "ShowFileInfo", ShowFileInfo);
	AddShellCmd("DEBUG", "debug", TestPara);
	AddShellCmd("SIMU", "Simulation work", Simulation);

	printf("outCounter = %d \r\n",g_workCtrl.cursta.outCounter);

#if (OPTICAL_MC_TYPE == OPTICAL_MC_WIRE)
	g_workCtrl.cursta.runSteps = 0;
#endif
}

// 检测传感器和位置,自动归零
void CheckSensorAndPos(void)
{
	InitOpticalMotos(200);
	getOptBorderWhenWork();
}

void InitOpticalMotos(u32 delay)
{
	MotosConfig config;

	memset(&config, 0, sizeof(MotosConfig));		// 电机配置

// 配置电机
#if(MODULE_X != 0)
	config.axisConfig[MTX_IDX].axisConfig = MTX_CFG;					// 实轴输出配置
	config.axisConfig[MTX_IDX].poutType = MTX_OTP;						// 脉冲输出模式 脉冲方式输出
	config.axisConfig[MTX_IDX].spdSource = MAKE_POUT_CMD(VAXIS_ID1);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	config.axisConfig[MTX_IDX].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if(MODULE_Y != 0)
	config.axisConfig[MTY_IDX].axisConfig = MTY_CFG;					// 实轴输出配置
	config.axisConfig[MTY_IDX].poutType = MTY_OTP;						// 脉冲输出模式 脉冲方式输出
	config.axisConfig[MTY_IDX].spdSource = MAKE_POUT_CMD(VAXIS_ID1);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	config.axisConfig[MTY_IDX].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if(LIFT_U1 != 0)
	config.axisConfig[MTU1_IDX].axisConfig = MTU1_CFG;					// 实轴输出配置
	config.axisConfig[MTU1_IDX].poutType = MTU1_OTP;					// 脉冲输出模式 脉冲方式输出
	config.axisConfig[MTU1_IDX].spdSource = MAKE_POUT_CMD(VAXIS_ID1);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	config.axisConfig[MTU1_IDX].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if(TURN_R1 != 0)
	config.axisConfig[MTR1_IDX].axisConfig = MTR1_CFG;					// 实轴输出配置
	config.axisConfig[MTR1_IDX].poutType = MTR1_OTP;					// 脉冲输出模式 脉冲方式输出
	config.axisConfig[MTR1_IDX].spdSource = MAKE_POUT_CMD(VAXIS_ID1);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	config.axisConfig[MTR1_IDX].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if(MODULE_SEND != 0) //送料电机模块
	config.axisConfig[MTS1_IDX].axisConfig = MTS1_CFG;					// 实轴输出配置
	config.axisConfig[MTS1_IDX].poutType = MTS1_OTP;					// 脉冲输出模式 脉冲方式输出
	config.axisConfig[MTS1_IDX].spdSource = MAKE_POUT_CMD(VAXIS_ID1);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	config.axisConfig[MTS1_IDX].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if(MODULE_PULL != 0)  //拉料电机模块
	config.axisConfig[MTP1_IDX].axisConfig = MTP1_CFG;					// 实轴输出配置
	config.axisConfig[MTP1_IDX].poutType = MTP1_OTP;					// 脉冲输出模式 脉冲方式输出
	config.axisConfig[MTP1_IDX].spdSource = MAKE_POUT_CMD(VAXIS_ID1);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	config.axisConfig[MTP1_IDX].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

	config.newConfig = 1;

	SetMotosConfig(&config);

	DelayMs(delay);
}


void OpticalTask(void)
{
	ProcessNewFile();// 新文件处理
}

void ProcessNewFile(void)
{
	if (IsNewDatFile(0) == TRUE)		// 新文件
	{
		printf("get a new file\r\n");

		SetNewDatFlag(0, FALSE);
		g_workCtrl.cursta.workStatus &= ~(WORK_STA_EN_SCAN);	// 禁止扫码
		printf("Reset work sta en scan4\r\n");

		// 判断新文件是否是同一个文件
		if (g_datFile.pFileHead->fileHead.dataChecksum != g_workCtrl.savedpara.fileCheck 	||
			((g_datFile.pFileHead->fileHead.checkCrc + GetFileParaCheckCrc()) & 0xffff) != HIWORD(g_workCtrl.savedpara.fileID) ||
			0	)
		{
			g_workCtrl.savedpara.fileID &= 0x0000ffff;
			g_workCtrl.savedpara.fileID |= ((u32)(g_datFile.pFileHead->fileHead.checkCrc + GetFileParaCheckCrc()) << 16);

			g_workCtrl.savedpara.fileCheck = g_datFile.pFileHead->fileHead.dataChecksum;
			SaveWorkNvPara();

			printf("not the same file, set idx to 0\r\n");
			SetDatIdxToStart();
		}
		GetWorkPosFromIndex();
	//	printf("------ProcessNewFile, fileIdx=%d\r\n", g_workCtrl.cursta.fileIdx);

		SendStatus(1);				// 发送状态
	}
}
void OpticalAsyncTask(void)
{
	int rslt;
	rslt = ERR_NONE;

#if (MODULE_LOOSE == 1)			// 松料电机装置
	if (((g_workCtrl.cursta.workStatus & WORK_STA_SIMULATE) == 0) &&
			1 )		//  松料允许;非模拟状态
	{
		rslt = SendOpticalTask();			// 自动送光纤任务

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

	}
#endif
}

int SendOpticalTask(void)		// 自动送光纤任务
{
	int err;
	err = ERR_NONE;


	//光纤储料低位1触发：停止松料
	//光纤储料低位2触发：开始松料
	if(err == ERR_NONE)
	{
		if ((IsOptStoringLow2() == SENSOR_ON) ||
				  0)
		{
			LooseMotorRevOn();   //松料电机   转动

			//printf("LooseMotor run %d\r\n");
			//DelayRef(300);
		}
		else
		{
			if((IsOptStoringLow1() == SENSOR_ON) ||
				 0)
			{
				LooseMotorRevOff();		// 松料电机  停止
			}

		}
	}
	//工作中，光纤储料低位1没触发且送料电机转动时，启动松料电机，


   return err;
}

// 定时执行任务
void OpticalTimerTask(void)
{
	SendStatus(1);	// 发送状态
	OperatorTaskWhenRun(0);
}

int OpticalCloseAll(void)
{
	PreMatCylinderOff();
	IsBlowingCylinderOff();             // 放料气缸收回
	IsPullingCylinderOff();	            // 拉料气缸收回
	DownClipMatClipCylinderOff();		// 下夹料关
	TrimmingCylinderOff();           	// 剪线关
	//Fan1MotoOff();                      // 风机停止
	UpClipMatClipCylinderOff();			// 上夹料关
	//FixMatCylinderOff();				// 顶料气缸关

	int rslt = ERR_NONE;

	return rslt;
}
int sendOpticalTaskWhenRun(void)		// 布线过程中送光纤任务
{
	//光纤储料低位1触发：停止松料
	//光纤储料低位1/2触发：开始松料

	int err = ERR_NONE;

	if ((IsOptStoringLow2() == SENSOR_ON) ||
		(IsOptStoringLow1() != SENSOR_ON) ||
			  0)
	{
		LooseMotorRevOn();   //松料电机   转动

		//printf("LooseMotor run %d\r\n");
		//DelayRef(300);
	}
	else
	{
		if((IsOptStoringLow1() == SENSOR_ON) ||
			 0)
		{
			LooseMotorRevOff();		// 松料电机  停止
		}

	}
	//工作中，光纤储料低位1没触发且送料电机转动时，启动松料电机，

   return err;
}
#if(MATERIAL_SCAN == 1)

int isEmptyMaterialSta(u16 len)//无料检测
{
	if(g_pOpticalMcPara->materialFreeSwitch == 1)
	{
		if(g_optWorkCtrl.IsNoMaterialEn == 1)
		{
	//		//情况1
	//		if(IsEmptyMaterial() == SENSOR_ON)
	//		{
	//			g_optWorkCtrl.IsNoMaterialScanLen = TBDSCAN_NUM;//断线检测扫描次数复位
	//			g_optWorkCtrl.IsNoMaterialFlag = 2;
	//		}

			//情况2
			if((IsEmptyMaterial() == SENSOR_ON) )
			{
				g_optWorkCtrl.runSteps2 = 10;
				g_optWorkCtrl.IsNoMaterialScanLen -= len;
			}
			else if((IsEmptyMaterial() != SENSOR_ON) && (g_optWorkCtrl.runSteps2 == 10))
			{
				g_optWorkCtrl.runSteps2 = 0;
				g_optWorkCtrl.IsNoMaterialScanLen = g_pOpticalMcPara->materialFreeScanNum;//断线检测扫描次数复位
				g_optWorkCtrl.IsNoMaterialFlag = 2;
			}

			//情况3
			if((IsEmptyMaterial() != SENSOR_ON))
			{
				g_optWorkCtrl.runSteps3 = 10;
				g_optWorkCtrl.IsNoMaterialScanLen -= len;
			}
			else if((IsEmptyMaterial() == SENSOR_ON) && (g_optWorkCtrl.runSteps3 == 10))
			{
				g_optWorkCtrl.runSteps3 = 0;
				g_optWorkCtrl.IsNoMaterialScanLen = g_pOpticalMcPara->materialFreeScanNum;//断线检测扫描次数复位
				g_optWorkCtrl.IsNoMaterialFlag = 2;
			}

			if(g_optWorkCtrl.IsNoMaterialScanLen <= 0)
			{
				g_optWorkCtrl.IsNoMaterialFlag = 0;
				//g_optWorkCtrl.IsNoMaterialEn = 0;
			}

			if(g_optWorkCtrl.IsNoMaterialFlag <= 0)
			{
				ReportError(ERR_NO_MATERIAL_ERROR);
				g_optWorkCtrl.opticalSta = 1;//断线检测报警
				//g_optWorkCtrl.IsNoMaterialEn = 0;
				g_optWorkCtrl.IsNoMaterialFlag = 2;
				return STA_NORMAL_STOP;
			}
		}
	}
	return 0;
}
#endif

int StartButtonTask(void)
{
	printf("start button task start\r\n");
	OpticalStartWork();
	return 0;
}

void RefreshParas(int flag)
{
	s32 zeropos[AXIS_NUM];

	if ((flag & PARA_MC_VALID) == PARA_MC_VALID)
	{
		printf("\r\n\t RefreshOpticalMcParas:\r\n");
		printf("zero pos\r\n\t");

		memset(zeropos, 0, sizeof(s32)*AXIS_NUM);

#if(MODULE_X != 0)
		zeropos[MTX_IDX] = g_pOpticalMcPara->xSensorPos;
#endif

#if(MODULE_Y != 0)
		zeropos[MTY_IDX] = g_pOpticalMcPara->ySensorPos;
#endif

#if(LIFT_U1 != 0)
		zeropos[MTU1_IDX] = g_pOpticalMcPara->u1SensorPos;
#endif
#if(TURN_R1 != 0)
		zeropos[MTR1_IDX] = ROT_001DEG_TO_PULSE(g_pOpticalMcPara->r1SensorAng);
#endif
#if(MODULE_PULL != 0)
		zeropos[MTS1_IDX] = g_pOpticalMcPara->xSensorPos;
#endif
#if(MODULE_SEND != 0)
		zeropos[MTP1_IDX] = g_pOpticalMcPara->xSensorPos;
#endif

		SetOpticalZeroPosValue(zeropos);

		// 重置产量
		if (g_pOpticalMcPara->expectedOutput != g_workCtrl.expectedOutput)
		{
			g_workCtrl.expectedOutput = g_pOpticalMcPara->expectedOutput;
		}
	}

	if ((flag & PARA_WK_VALID) == PARA_WK_VALID)
	{
		printf("\r\n\t RefreshOpticalWkParas:\r\n");

	}
}

void SetOpticalZeroPosValue(s32 * list)
{
	int i;
	s32 temp;

	if (list == NULL)
	{
		printf("set zero to valid\r\n");
		memset(g_workCtrl.zeroPosList, 0, sizeof(s32)*AXIS_NUM);
		g_workCtrl.posEnFlag = 0;
		g_workCtrl.zeroEnFlag = 0;
	}

	ReadAndFreshMotosPos();

	printf("\r\n\tset Optical zero para\r\n");

	printf("before set, motos pos is:\r\n");
	for (i = 0; i < AXIS_NUM; i++)
	{
		printf("\t pos[%d]=%d\r\n", i+1, g_motosPara.motosPos[i]);
	}

#if(1)
#if(MODULE_X != 0)
	SET_MOTO_ZERO(MTX);
#endif

#if(MODULE_Y != 0)
	SET_MOTO_ZERO(MTY);
#endif

#if(LIFT_U1 != 0)
	SET_MOTO_ZERO(MTU1);
#endif

#if(TURN_R1 != 0)
	SET_MOTO_ZERO(MTR1);
#endif

#if(MODULE_PULL != 0)
	SET_MOTO_ZERO(MTP1);
#endif

#if(MODULE_SEND != 0)
	SET_MOTO_ZERO(MTS1);
#endif

#else
	temp = 0x00;
	if (temp == 0x00)
	{

	}
#endif

	if (list != NULL)
	{
		memcpy(g_workCtrl.zeroPosList, list, sizeof(s32)*AXIS_NUM);
		g_workCtrl.zeroEnFlag = DATA_VALID;
	}

	printf("after set, motos pos is:\r\n");
	for (i = 0; i < AXIS_NUM; i++)
	{
		printf("\t pos[%d]=%d\r\n", i+1, g_motosPara.motosPos[i]);
	}
}

void RefreshStatus(void)
{
	memcpy((void*)(&(g_workCtrl.cursta.tempdat[0])), (void*)(&(g_motosPara.motosPos[0])), sizeof(s32)*AXIS_NUM);
}

void DelayWhenRun (s32 tim)
{
	if ((g_workCtrl.cursta.workStatus & WORK_STA_WORKING) == WORK_STA_WORKING)
	{
		int dl = 0;
		if (tim != 0)
		{
			dl = 1;
		}
		else
		{
			tim = 1;
		}

		do
		{
			DelayRef(dl);
			OperatorTaskWhenRun(0);
			if (tim <= 0)
			{
				break;
			}
			tim--;
		}while(1);
	}
	else
	{
		DelayRef(tim);
	}
}

#if (XY_GAP_CONVERT == 1)
// 间隙补偿转换
s32 XGapConvert(s32 offst)
{
	static int xgdir = 0;	// 历史运行方向
	s32 sft = offst;
	if (xgdir == 0)
	{
		if (offst > 0)
		{
			xgdir = 1;
			sft = offst + g_pOpticalMcPara->xGapCorrect/2;
		}
		else if (offst < 0)
		{
			xgdir = -1;
			sft = offst - g_pOpticalMcPara->xGapCorrect/2;
		}
		else if (offst == 0)
		{
			xgdir = 0;
			sft = offst;
		}
	}
	else if (xgdir > 0)
	{
		if (offst > 0)
		{
			xgdir = 1;
			sft = offst;
		}
		else if (offst < 0)
		{
			xgdir = -1;
			sft = offst - g_pOpticalMcPara->xGapCorrect;
		}
		else if (offst == 0)
		{
			xgdir = 0;
			sft = offst - g_pOpticalMcPara->xGapCorrect/2;
		}
	}
	else if (xgdir < 0)
	{
		if (offst > 0)
		{
			xgdir = 1;
			sft = offst + g_pOpticalMcPara->xGapCorrect;
		}
		else if (offst < 0)
		{
			xgdir = -1;
			sft = offst;
		}
		else if (offst == 0)
		{
			xgdir = 0;
			sft = offst + g_pOpticalMcPara->xGapCorrect/2;
		}
	}
	//	printf ("xsft = %d",sft);
	if(g_optWorkCtrl.disosft == 1)
	{
		sft = offst;
	}
	return sft;
}
// 间隙补偿转换
s32 YGapConvert(s32 offst)
{
	static int ygdir = 0;	// 历史运行方向
	s32 sft = offst;
	//printf ("dir = %d,	offst = %d ",dir,offst);

	if (ygdir == 0)
	{
		if (offst > 0)
		{
			ygdir = 1;
			sft = offst + g_pOpticalMcPara->yGapCorrect/2;
		}
		else if (offst < 0)
		{
			ygdir = -1;
			sft = offst - g_pOpticalMcPara->yGapCorrect/2;
		}
		else if (offst == 0)
		{
			ygdir = 0;
			sft = offst;
		}
	}
	else if (ygdir > 0)
	{
		if (offst > 0)
		{
			ygdir = 1;
			sft = offst;
		}
		else if (offst < 0)
		{
			ygdir = -1;
			sft = offst - g_pOpticalMcPara->yGapCorrect;
		}
		else if (offst == 0)
		{
			ygdir = 0;
			sft = offst - g_pOpticalMcPara->yGapCorrect/2;
		}
	}
	else if (ygdir < 0)
	{
		if (offst > 0)
		{
			ygdir = 1;
			sft = offst + g_pOpticalMcPara->yGapCorrect;
		}
		else if (offst < 0)
		{
			ygdir = -1;
			sft = offst;
		}
		else if (offst == 0)
		{
			ygdir = 0;
			sft = offst + g_pOpticalMcPara->yGapCorrect/2;
		}
	}
//	printf (",ysft = %d \r\n",sft);

	return sft;
}
// XY移框后间隙修正
int MoveXYGap(s32 ofstx, s32 ofsty, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 xIdx;
	u16 yIdx;
	MotoSpd spd;
	s16 dirx, diry;

	ofstx = XGapConvert(ofstx);	// 间隙补偿转换
	ofsty = YGapConvert(ofsty);	// 间隙补偿转换

	xIdx = MTX_IDX;
	ofstx = 0;

	yIdx = MTY_IDX;
	ofsty = 0;

	if (ofstx == 0 && ofsty == 0)
	{
		return ERR_NONE;
	}

	printf("MoveXYGap, x=%d,y=%d\r\n", ofstx, ofsty);

	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);
		pSpd = &spd;
	}

	if (ofstx < 0)
	{
		dirx = -1;
	}
	else if (ofstx > 0)
	{
		dirx = 1;
	}
	else
	{
		dirx = 0;
	}

	if (ofsty < 0)
	{
		diry = -1;
	}
	else if (ofsty > 0)
	{
		diry = 1;
	}
	else
	{
		diry = 0;
	}

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 1;												// 需要更新电机配置

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

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


	lmctrl.mvmtPara.extraRepeat = 0;												// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofstx) > abs(ofsty) ? abs(ofstx) : abs(ofsty);	// 插补长轴位移
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;							// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;		// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd/2;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 = lmctrl.vAxisId;			// 工作参数
	lmctrl.GetEmergencyStop = MoveXYEmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲(无需降速)
	lmctrl.GetQuickStop = MoveXYQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止(按刹车加速度)
	lmctrl.GetNormalStop = MoveXYNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止(按降速加速度)
	lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = (((u32)diry & 0xffff) << 16 | ((u32)dirx & 0xffff));
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}
#endif
//---------------------------------------------------X--------------------------------------------
void getOptBorderWhenWork(void)
{
	g_optBorder.xNegaBorder = g_pOpticalMcPara->xMovNegaBorder;
	g_optBorder.yNegaBorder = g_pOpticalMcPara->yMovNegaBorder;
	g_optBorder.xPosiBorder = g_pOpticalMcPara->xMovPosiBorder;
	g_optBorder.yPosiBorder = g_pOpticalMcPara->yMovPosiBorder;

//	if(g_pOpticalMcPara->offsetX > 0)
//	{
//		g_optBorder.xNegaBorder -= g_pOpticalMcPara->offsetX;
//	}
//	else
//	{
//		g_optBorder.xPosiBorder -= g_pOpticalMcPara->offsetX;
//	}
//
//	if(g_pOpticalMcPara->offsetY > 0)
//	{
//		g_optBorder.yNegaBorder -= g_pOpticalMcPara->offsetY;
//	}
//	else
//	{
//		g_optBorder.yPosiBorder -= g_pOpticalMcPara->offsetY;
//	}

	return;
}

// 正向限位
u8 MoveXLimitPosi(void)
{
	if (//(IsXLimitPosi() == SENSOR_ON) || 	// 硬件限位
		((g_motosPara.enFlag == DATA_VALID) &&
		 (g_workCtrl.posEnFlag & (MTX_MOD)) == (MTX_MOD) &&							// X 坐标有效并且归零成功
		 (g_motosPara.motosPos[MTX_IDX] > /*g_pOpticalMcPara->xMovPosiBorder*/g_optBorder.xPosiBorder) &&		// 且坐标大于正边界
		 1	) ||		// 读入坐标，软件限位
		0	)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

// 反向限位
u8 MoveXLimitNega(void)
{
	if (//(IsXLimitNega() == SENSOR_ON) || 	// 硬件限位
		((g_motosPara.enFlag == DATA_VALID) &&
		 (g_workCtrl.posEnFlag & (MTX_MOD)) == (MTX_MOD) &&							// X 坐标有效并且归零成功
		(g_motosPara.motosPos[MTX_IDX] < /*g_pOpticalMcPara->xMovNegaBorder*/g_optBorder.xNegaBorder) &&		// 且坐标小于负边界
		1	) ||		// 读入坐标，软件限位
		0	)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

int MoveXBHStop(u32 para)		// 停止条件
{
	int rslt;

	rslt = ERR_NONE;

	if (IsNewOperCmd() == TRUE)				// 读取操作箱命令，如果有新命令
	{
		rslt = STA_NORMAL_STOP;
	}
	else
	{
	}

	return rslt;
}
// 慢速条件
int MoveXBHSlow(u32 para)
{
	int rslt;
	int dir;

	dir = (int)(para);

	rslt = ERR_NONE;
	if (dir != 0)
	{
		u32 curpps;
		s32 decCount;

		if ((g_motosPara.enFlag == DATA_VALID) &&
				(g_workCtrl.posEnFlag & (MTX_MOD)) == (MTX_MOD) &&					// X零位坐标有效
				1	)
		{
			curpps = GetVAxisPPS(VAXIS_ID1);					// 当前速度
			decCount = CalcDisplacement(curpps, XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd), XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd)*TOSLOWMUTI);	// 计算以当前运行速度减速到停止速度需要的位移

			decCount += 200*PULSE_PER_MM/100;	// 补偿值

			if (dir > 0)		// 正向
			{
				if (g_motosPara.motosPos[MTX_IDX] + decCount > g_pOpticalMcPara->xMovPosiBorder)
				{
					rslt = STA_NORMAL_STOP;		// 降速状态
				}
			}
			else if (dir < 0)	// 反向
			{
				if (g_motosPara.motosPos[MTX_IDX] - decCount < g_pOpticalMcPara->xMovNegaBorder)
				{
					rslt = STA_NORMAL_STOP;		// 降速状态
				}
			}
		}
	}

	return rslt;
}
int MoveXByHand(int dir, int spdLev)
{
	int err;
	int stspd, rspd;
	MVByHandCtrl mbhc;

	assert_param(dir == 1|| dir == -1);

	err = ERR_NONE;

	err = MoveZU1ToUpper();

	if (err != ERR_NONE)
	{
		ReportError(err);
		return err;
	}

	memset(&mbhc, 0, sizeof(MVByHandCtrl));

	mbhc.vAxisId = VAXIS_ID1;		// 虚轴
	mbhc.axisIdx = MTX_IDX;			// 运动轴
	mbhc.axisCfg = MTX_CFG;			// 配置
	mbhc.poutType = MTX_OTP;
	mbhc.newCfg = 1;

	mbhc.movement = (s32)XYAxisMMToPulse(MVXBH_MVMT)*dir;	// 最大移动位移
#if (XY_GAP_CONVERT == 1)
	mbhc.movement = XGapConvert(mbhc.movement);	// 间隙补偿转换
#endif
	mbhc.ppSegment = PULSE_PER_SEGMENT;

	stspd = g_pOpticalMcPara->xymvstspSpd;

	if (spdLev <= 1)
	{
		rspd = g_pOpticalMcPara->xymvhSpd1;	// 手动移动速度等级1
	}
	else if (spdLev == 2)
	{
		rspd = g_pOpticalMcPara->xymvhSpd2;	// 手动移动速度等级2
	}
	else if (spdLev >= 3)
	{
		rspd = g_pOpticalMcPara->xymvhSpd3;	// 手动移动速度等级3
	}

	if (stspd > rspd)
	{
		stspd = rspd;
	}

	mbhc.hspd.stspSpd = XYAxisMMPSToPPS(stspd);							// 起停速度
	mbhc.hspd.runSpd = XYAxisMMPSToPPS(rspd);
	mbhc.hspd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);	// 行走加速度
	mbhc.hspd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 刹车加速度

	mbhc.limitpsensor = MoveXBHLimitPosi;		// 正向限位
	mbhc.limitnsensor = MoveXBHLimitNega;		// 反向限位

	mbhc.precondition = NULL;	// 先决条件
	mbhc.stopscan = MoveXBHStop;		// 停止条件
	//mbhc.slowscan = MoveXBHSlow;		// 慢速条件

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = MoveByHandCtrl(&mbhc);
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}
int MoveXBHPrec(u32 para)		// 先决条件
{
	int err;

	if ((g_motosPara.enFlag == DATA_VALID) &&
		(g_workCtrl.posEnFlag & (MTX_MOD)) == (MTX_MOD) &&	// X 坐标有效并且归零成功
		1	)
	{
		err = ERR_NONE;
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	return err;
}
// 急停检测
int MoveXEmcScan(u32 para1, u32 para2)
{
	int rslt;
	s16 dirx;

	dirx = (s16)para1;

	rslt = GetCommonEms();

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

	// X限位
	if (dirx > 0)
	{
		if (MoveXLimitPosi() == SENSOR_ON)		// X正向限位传感器
		{
			rslt = ERR_X_LIT_POSI;	//
		}
	}
	else if (dirx < 0)
	{
		if (MoveXLimitNega() == SENSOR_ON)		// X反向限位传感器
		{
			rslt = ERR_X_LIT_NEGA;	//
		}
	}
	else // if (dirx == 0)
	{
		rslt = ERR_NONE;
	}

	return rslt;
}

// 快速停止检测
int MoveXQStopScan(u32 para1, u32 para2)
{
	int rslt;
	rslt = GetCommonQStop();

	if (rslt == ERR_NONE)
	{

	}

	return rslt;
}


// 普通停止检测
int MoveXNStopScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonNStop();

	ReadMotosPos();

	if (IsConsoleCancel() != 0)     //请求退出
	{
		rslt = STA_NORMAL_STOP;
	}

	return rslt;
}

u8 MoveXBHLimitPosi(u32 para)
{
	return MoveXLimitPosi();
}

u8 MoveXBHLimitNega(u32 para)
{
	return MoveXLimitNega();
}

// x偏移值
int MoveXAsOffset(s32 ofstx, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 xIdx;
	MotoSpd spd;
	s16 dirx;

#if (XY_GAP_CONVERT == 1)
	ofstx = XGapConvert(ofstx);	// 间隙补偿转换
#endif

	xIdx = MTX_IDX;

	if (ofstx == 0)
	{
		return ERR_NONE;
	}

	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);
		pSpd = &spd;
	}

	//方向
	if (ofstx < 0)
	{
		dirx = -1;
	}
	else if (ofstx > 0)
	{
		dirx = 1;
	}
	else
	{
		dirx = 0;
	}

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[xIdx].axisConfig = MTX_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[xIdx].poutType = MTX_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[xIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[xIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[xIdx] = ofstx;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofstx);							// 插补长轴位移	// GetSyntheticLength(ofstx, ofsty);	// 插补长轴位移，两个轴的合成长度
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;				// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;	// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;	// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 = lmctrl.vAxisId;			// 工作参数
	lmctrl.GetEmergencyStop = MoveXEmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveXQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveXNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
//	lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = ((u32)dirx & 0xffff);
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

	err = GetMoveResultError(rslt, lmctrl.errInfo);
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	return err;
}

// 移动x到绝对位置
int MoveXToPos(s32 posx, MotoSpd * pSpd)
{
	int err;
	s32 ofstx;

	g_workCtrl.sendStaWhenRun = 0;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	err = ERR_NONE;
	ReportError(err);

	ReadAndFreshMotosPos();

	if (g_motosPara.enFlag == DATA_VALID &&
		(g_workCtrl.posEnFlag & (MTX_MOD)) == (MTX_MOD) &&
		1	)
	{
		// 目标位置在区域内
		if (posx < g_pOpticalMcPara->xMovNegaBorder || 	// 工作区X负边界，单位0.01mm
			posx > g_pOpticalMcPara->xMovPosiBorder || 	// 工作区X正边界，单位0.01mm
			0	)
		{
			printf("\r\ntarget position out of range, x=%d\r\n", posx);
			err = ERR_OUT_RANGE;		// 目标位置越界
		}
		else
		{
			ReadAndFreshMotosPos();

			ofstx = posx - g_motosPara.motosPos[MTX_IDX];

//			printf("\r\n before move\r\n");
//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);

//			printf("\r\n move ofstx = %d, ofsty = %d\r\n", ofstx, ofsty);

			err = MoveXAsOffset(ofstx, pSpd);

//			printf("\r\n after move\r\n");
//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);
		}
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.sendStaWhenRun = 1;
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

// X归零

u8 IsXAtZero(void)
{
	u8 slow;

	slow = SENSOR_ON;

	if (IsXZeroPos() == SENSOR_ON &&
		slow == SENSOR_ON &&
		1	)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

// 归零条件
int XMotoMoveCheck(void)
{
	int err = ERR_NONE;

	return err;
}

// 归零
int FindXZero(void)
{
	int err;
	MTSCtrl mtsc;
	int fzdir = X_FINDZERO_DIR;

	err = ERR_NONE;
	// 标志失效
	g_workCtrl.posEnFlag &= (~MTX_MOD);

	ReadAndFreshMotosPos();

	memset(&mtsc, 0, sizeof(MTSCtrl));

	mtsc.vAxisId = VAXIS_ID1;			// 虚轴
	mtsc.axisIdx = MTX_IDX;				// 运动轴
	mtsc.axisCfg = MTX_CFG;				// 配置
	mtsc.poutType = MTX_OTP;

	mtsc.newCfg = 1;

	mtsc.movement = (s32)XYAxisMMToPulse(FXZ_MVMT)*fzdir;				// 最大移动位移

	mtsc.mvposi = (s32)XYAxisMMToPulse(FXZ_FXMV)*fzdir*(-1);				// 反向移动位移
	mtsc.lspd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);	// 起停速度
	mtsc.lspd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvzSpd);		// 归零速度
	mtsc.lspd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);		// 行走加速度
	mtsc.lspd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 刹车加速度

	mtsc.zerosensor = IsXAtZero;		// 零位检测传感器
	mtsc.limitpsensor = MoveXLimitPosi;	// 正向限位传感器
	mtsc.limitnsensor = MoveXLimitNega;	// 反向限位传感器


//#if (PLOTTER_MC_TYPE == PLOTTER_MC_SINGLE)
	mtsc.emcstopscan = NULL;	// 停止条件
//#endif
	mtsc.moveasoffset = MoveXAsOffset;	// 移动函数

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = LineRunToSensorCtrl(&mtsc);


	DelayRef(100);
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	if (err == STA_MTZ_SUCCESS && IsXAtZero() == SENSOR_ON)
	{
		MotoSpd MoSpd;
		MoSpd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);	// 起停速度
		MoSpd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvzSpd);		// 归零速度
		MoSpd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);		// 行走加速度
		MoSpd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 刹车加速度
		MoveXAsOffset(((s32)XYAxisMMToPulse(FXZ_FXMV)*fzdir*(-1)), &MoSpd);
		mtsc.vAxisId = VAXIS_ID1;			// 虚轴
		mtsc.axisIdx = MTX_IDX;				// 运动轴
		mtsc.axisCfg = MTX_CFG;				// 配置
		mtsc.poutType = MTX_OTP;

		mtsc.newCfg = 1;

		mtsc.movement = (s32)XYAxisMMToPulse(FXZ_FXMV*2)*fzdir;				// 最大移动位移

		mtsc.mvposi = (s32)XYAxisMMToPulse(FXZ_FXMV)*fzdir*(-1);										// 反向移动位移
		mtsc.lspd.stspSpd = XYAxisMMPSToPPS(1);	// 起停速度
		mtsc.lspd.runSpd = XYAxisMMPSToPPS(10);		// 归零速度
		mtsc.lspd.spdAdd = XYAxisMMPSSToPPSG(10);		// 行走加速度
		mtsc.lspd.brkAdd = XYAxisMMPSSToPPSG(100);	// 刹车加速度

		mtsc.zerosensor = IsXAtZero;		// 零位检测传感器
		mtsc.limitpsensor = MoveXLimitPosi;	// 正向限位传感器
		mtsc.limitnsensor = MoveXLimitNega;	// 反向限位传感器


	//#if (PLOTTER_MC_TYPE == PLOTTER_MC_SINGLE)
		mtsc.emcstopscan = NULL;	// 停止条件
	//#endif
		mtsc.moveasoffset = MoveXAsOffset;	// 移动函数
		err = LineRunToSensorCtrl(&mtsc);
		DelayRef(100);
		if (err == STA_MTZ_SUCCESS && IsXAtZero() == SENSOR_ON)
		{
			g_workCtrl.posEnFlag |= MTX_MOD;
			printf("FindXZero success\r\n");
			err = STA_MTZ_SUCCESS;
		}
		else
		{
			g_workCtrl.posEnFlag &= (~MTX_MOD);
			printf("FindXZero error, code=%d\r\n", err);
	//		err = ERR_MTZ_ERROR;	// 归零错误
		}
	}
	else
	{
		g_workCtrl.posEnFlag &= (~MTX_MOD);
		printf("FindXZero error, code=%d\r\n", err);
//		err = ERR_MTZ_ERROR;	// 归零错误
	}

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	printf("before set, MTX pos=%d, set to %d\r\n", g_motosPara.motosPos[MTX_IDX], g_workCtrl.zeroPosList[MTX_IDX]);

	g_workCtrl.zeroPosList[MTX_IDX] = g_pOpticalMcPara->xSensorPos;
	SetMotoCounter(MTX_IDX, g_workCtrl.zeroPosList[MTX_IDX]);		// 计数器初值

	ReadAndFreshMotosPos();

	printf("after set, MTX pos=%d\r\n", g_motosPara.motosPos[MTX_IDX]);

	ReportError(err);
	return err;
}

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




//---------------------------------------------------Y--------------------------------------------

// 手动移动Y
#define	MVYBH_MVMT		(3000)	// 3000 mm

// 正向限位
u8 MoveYLimitPosi(void)
{
	if (//(IsYLimitPosi() == SENSOR_ON) || 	// 硬件限位
		((g_motosPara.enFlag == DATA_VALID) &&
		 (g_workCtrl.posEnFlag & (MTY_MOD)) == (MTY_MOD) &&							// X 坐标有效并且归零成功
		 (g_motosPara.motosPos[MTY_IDX] > /*g_pOpticalMcPara->yMovPosiBorder*/g_optBorder.yPosiBorder) &&		// 且坐标大于正边界
		 1	) ||		// 读入坐标，软件限位
		0	)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

// 反向限位
u8 MoveYLimitNega(void)
{
	if (//(IsYLimitNega() == SENSOR_ON) || 	// 硬件限位
		((g_motosPara.enFlag == DATA_VALID) &&
		 (g_workCtrl.posEnFlag & (MTY_MOD)) == (MTY_MOD) &&							// Y 坐标有效并且归零成功
		(g_motosPara.motosPos[MTY_IDX] < /*g_pOpticalMcPara->yMovNegaBorder*/g_optBorder.yNegaBorder) &&		// 且坐标小于负边界
		1	) ||		// 读入坐标，软件限位
		0	)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}


u8 MoveYBHLimitPosi(u32 para)
{
	return MoveYLimitPosi();
}

u8 MoveYBHLimitNega(u32 para)
{
	return MoveYLimitNega();
}

int MoveYBHPrec(u32 para)		// 先决条件
{
	int err;

	if ((g_motosPara.enFlag == DATA_VALID) &&
		(g_workCtrl.posEnFlag & (MTY_MOD)) == (MTY_MOD) &&	// X 坐标有效并且归零成功
		1	)
	{
		err = ERR_NONE;
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	return err;
}

int MoveYBHStop(u32 para)		// 停止条件
{
	int rslt;

	rslt = ERR_NONE;

	if (IsNewOperCmd() == TRUE)				// 读取操作箱命令，如果有新命令
	{
		rslt = STA_NORMAL_STOP;
	}
	else
	{
	}

	if (rslt == ERR_NONE)
	{

	}
	return rslt;
}
//
//int MoveYBHSlow(u32 para)		// 慢速条件
//{
//	int rslt;
//	int dir;
//
//	dir = (int)(para);
//
//	rslt = ERR_NONE;
//	if (dir != 0)
//	{
//		u32 curpps;
//		s32 decCount;
//
//		if ((g_motosPara.enFlag == DATA_VALID) &&
//			(g_workCtrl.posEnFlag & (MTY_MOD)) == (MTY_MOD) &&					// X 坐标有效并且归零成功
//			1	)
//		{
//			curpps = GetVAxisPPS(VAXIS_ID1);					// 当前速度
//			decCount = CalcDisplacement(curpps, XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd), XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd)*TOSLOWMUTI);	// 计算以当前运行速度减速到停止速度需要的位移
//
//			decCount += 200;	// 补偿值
//
//			if (dir > 0)		// 正向
//			{
//				if (g_motosPara.motosPos[MTY_IDX] + decCount > g_pOpticalMcPara->yMovPosiBorder)
//				{
//					rslt = STA_NORMAL_STOP;		// 降速状态
//				}
//			}
//			else if (dir < 0)	// 反向
//			{
//				if (g_motosPara.motosPos[MTY_IDX] - decCount < g_pOpticalMcPara->yMovNegaBorder)
//				{
//					rslt = STA_NORMAL_STOP;		// 降速状态
//				}
//			}
//		}
//	}
//
//	return rslt;
//}

int MoveYByHand(int dir, int spdLev)
{
	int err;
	MVByHandCtrl mbhc;
	int stspd, rspd;

	assert_param(dir == 1|| dir == -1);

	err = ERR_NONE;

	err = MoveZU1ToUpper();

	if (err != ERR_NONE)
	{
		ReportError(err);
		return err;
	}

	memset(&mbhc, 0, sizeof(MVByHandCtrl));

	mbhc.vAxisId = VAXIS_ID1;		// 虚轴
	mbhc.axisIdx = MTY_IDX;			// 运动轴
	mbhc.axisCfg = MTY_CFG;			// 配置
	mbhc.poutType = MTY_OTP;
	mbhc.newCfg = 1;

	mbhc.movement = (s32)XYAxisMMToPulse(MVXBH_MVMT)*dir;	// 最大移动位移
#if (XY_GAP_CONVERT == 1)
	mbhc.movement = YGapConvert(mbhc.movement);	// 间隙补偿转换
#endif
	mbhc.ppSegment = PULSE_PER_SEGMENT;

	stspd = g_pOpticalMcPara->xymvstspSpd;

	if (spdLev <= 1)
	{
		rspd = g_pOpticalMcPara->xymvhSpd1;	// 手动移动速度等级1
	}
	else if (spdLev == 2)
	{
		rspd = g_pOpticalMcPara->xymvhSpd2;	// 手动移动速度等级2
	}
	else if (spdLev >= 3)
	{
		rspd = g_pOpticalMcPara->xymvhSpd3;	// 手动移动速度等级3
	}

	if (stspd > rspd)
	{
		stspd = rspd;
	}

	mbhc.hspd.stspSpd = XYAxisMMPSToPPS(stspd);							// 起停速度
	mbhc.hspd.runSpd = XYAxisMMPSToPPS(rspd);
	mbhc.hspd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);	// 行走加速度
	mbhc.hspd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 刹车加速度

	mbhc.limitpsensor = MoveYBHLimitPosi;	// 正向限位
	mbhc.limitnsensor = MoveYBHLimitNega;	// 反向限位

	mbhc.precondition = MoveYBHPrec;	// 先决条件
	mbhc.stopscan = MoveYBHStop;		// 停止条件
	//mbhc.slowscan =  MoveYBHSlow;				// 慢速条件

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = MoveByHandCtrl(&mbhc);
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	ReportError(err);
	return err;
}

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

// 急停检测
int MoveYEmcScan(u32 para1, u32 para2)
{
	int rslt;
	s16 diry;

	diry = (s16)para1;

	rslt = GetCommonEms();

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

	// Y限位
	if (diry > 0)
	{
		if (MoveYLimitPosi() == SENSOR_ON)		// Y正向限位传感器
		{
			rslt = ERR_Y_LIT_POSI;		// Y正向限位
		}
	}
	else if (diry < 0)
	{
		if (MoveYLimitNega() == SENSOR_ON)		// Y反向限位传感器
		{
			rslt = ERR_Y_LIT_NEGA;		// Y反向限位
		}
	}
	else // if (diry == 0)
	{
		rslt = ERR_NONE;
	}

	if (rslt == ERR_NONE)
	{

	}

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

// 快速停止检测
int MoveYQStopScan(u32 para1, u32 para2)
{
	int rslt;
	rslt = GetCommonQStop();

	if (rslt == ERR_NONE)
	{

	}

	return rslt;
}

// 普通停止检测
int MoveYNStopScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonNStop();

#if (MODULE_LOOSE == 1)			// 松料装置
	rslt = SendOpticalTask();			// 自动送光纤任务
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#endif

	return rslt;
}

// Y轴偏移移动
int MoveYAsOffset(s32 ofsty, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 yIdx;
	MotoSpd spd;
	s16 diry;
#if (XY_GAP_CONVERT == 1)
	ofsty = YGapConvert(ofsty);	// 间隙补偿转换
#endif
	yIdx = MTY_IDX;

	if (ofsty == 0)
	{
		return ERR_NONE;
	}

	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);
		pSpd = &spd;
	}

	//方向
	if (ofsty < 0)
	{
		diry = -1;
	}
	else if (ofsty > 0)
	{
		diry = 1;
	}
	else
	{
		diry = 0;
	}

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[yIdx].axisConfig = MTY_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[yIdx].poutType = MTY_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[yIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[yIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[yIdx] = ofsty;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofsty);							// 插补长轴位移
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;			// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;		// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;		// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 =  lmctrl.vAxisId;						// 工作参数
	lmctrl.GetEmergencyStop = MoveYEmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveYQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveYNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
//	lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = ((u32)diry & 0xffff);
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

//	printf("after move, motospos=%d\r\n", g_motosPara.motosPos[MTY_IDX]);

	err = GetMoveResultError(rslt, lmctrl.errInfo);
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	return err;
}


// 移动Y到绝对位置
int MoveYToPos(s32 posy, MotoSpd * pSpd)
{
	int err;
	s32 ofsty;

	g_workCtrl.sendStaWhenRun = 0;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	err = ERR_NONE;
	ReportError(err);

	ReadAndFreshMotosPos();

	if (g_motosPara.enFlag == DATA_VALID &&
		(g_workCtrl.posEnFlag & (MTY_MOD)) == (MTY_MOD) &&
		1	)
	{
		// 目标位置在区域内
		if (posy < g_pOpticalMcPara->yMovNegaBorder || 	// 工作区X负边界，单位0.01mm
			posy > g_pOpticalMcPara->yMovPosiBorder || 	// 工作区X正边界，单位0.01mm
			0	)
		{
			printf("\r\ntarget position out of range, y=%d\r\n", posy);
			err = ERR_OUT_RANGE;		// 目标位置越界
		}
		else
		{
			ReadAndFreshMotosPos();

			ofsty = posy - g_motosPara.motosPos[MTY_IDX];

//			printf("\r\n before move\r\n");
//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);

//			printf("\r\n move ofstx = %d, ofsty = %d\r\n", ofstx, ofsty);

			err = MoveYAsOffset(ofsty, pSpd);

//			printf("\r\n after move\r\n");
//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);
		}
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.sendStaWhenRun = 1;
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}
//-------------------------------------------------------------------------------
// Y归零
u8 IsYAtZero(void)
{
	u8 slow;

	slow = SENSOR_ON;

	if (IsYZeroPos() == SENSOR_ON &&
		slow == SENSOR_ON &&
		1	)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

// y归零条件
int YMotoMoveCheck(void)
{
	int err = ERR_NONE;

	return err;
}

// y归零
int YMotoToZero()
{
	int err;
	int motorun;
	int fzdir = -1;
	MTSCtrl mtsc;

	err = ERR_NONE;
	motorun = 0;

	if (IsYZeroPos() != SENSOR_ON)
	{
		motorun = 1;
	}

	err = YMotoMoveCheck();	//运动前判断

	if (err == ERR_NONE)
	{
		if (IsYZeroPos() == SENSOR_ON)
		{
			err = MoveYAsOffset(1000,NULL);
			motorun = 1;
		}
		else
		{
			motorun = 1;
		}
	}

	if (err == ERR_NONE && motorun != 0)
	{
		memset(&mtsc, 0, sizeof(MTSCtrl));

		mtsc.vAxisId = VAXIS_ID1;	// 虚轴
		mtsc.axisIdx = MTY_IDX;		// 运动轴
		mtsc.axisCfg = MTY_CFG;		// 配置
		mtsc.poutType = MTY_OTP;	// 配置
		mtsc.newCfg = 1;			// 重新配置标志

		mtsc.movement = (s32)XYAxisMMToPulse(FXZ_MVMT)*fzdir;				// 最大移动位移
		mtsc.lspd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);		// 启停速度
		mtsc.lspd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvzSpd);			// 运转转速
		if (mtsc.lspd.stspSpd > mtsc.lspd.runSpd)
		{
			mtsc.lspd.stspSpd = mtsc.lspd.runSpd;
		}

		if (g_pOpticalMcPara->xymvSAdd == 0)
		{
			g_pOpticalMcPara->xymvSAdd = 1;
		}
		mtsc.lspd.spdAdd =  XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);		// 加速度
		if (mtsc.lspd.spdAdd == 0)
		{
			mtsc.lspd.spdAdd = 1;
		}

		if (g_pOpticalMcPara->xymvbrkAdd == 0)
		{
			g_pOpticalMcPara->xymvbrkAdd = 1;
		}
		mtsc.lspd.brkAdd =  XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 刹车加速度
		if (mtsc.lspd.brkAdd == 0)
		{
			mtsc.lspd.brkAdd = 1;
		}

		mtsc.zerosensor = IsYZeroPos;			// 零位检测传感器

		mtsc.moveasoffset = MoveYAsOffset;	// 移动函数

		g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
		err = LineRunToSensorCtrl(&mtsc);
		g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

		DelayRef(100);
	}

	if (err == STA_MTZ_SUCCESS && IsYZeroPos() == SENSOR_ON)
	{
		g_workCtrl.posEnFlag |= MTY_MOD;
		printf("MTY To Zero success\r\n");
		err = STA_MTZ_SUCCESS;
	}
	else
	{
		g_workCtrl.posEnFlag &= (~MTY_MOD);
		printf("MTY To Zero error, code=0x%x\r\n", err);
		err = ERR_MTZ_ERROR;	// 归零错误
	}

	printf("before set, MTY pos=%d, set to %d\r\n", g_motosPara.motosPos[MTY_IDX],0);

	g_workCtrl.zeroPosList[MTY_IDX] = 0;
	SetMotoCounter(MTY_IDX, g_workCtrl.zeroPosList[MTY_IDX]);		// 计数器初值

	ReadAndFreshMotosPos();

	printf("after set, MTY pos=%d\r\n", g_motosPara.motosPos[MTY_IDX]);

	//ReportError(err);
	return err;
}

int FindYZero(int fzdir)
{
	int err;
	MTSCtrl mtsc;

	err = ERR_NONE;

	// 标志失效
	g_workCtrl.posEnFlag &= (~MTY_MOD);

	ReadAndFreshMotosPos();

	memset(&mtsc, 0, sizeof(MTSCtrl));

	mtsc.vAxisId = VAXIS_ID1;			// 虚轴
	mtsc.axisIdx = MTY_IDX;				// 运动轴
	mtsc.axisCfg = MTY_CFG;				// 配置
	mtsc.poutType = MTY_OTP;

	mtsc.newCfg = 1;

	mtsc.movement = (s32)XYAxisMMToPulse(FXZ_MVMT)*fzdir;				// 最大移动位移

	mtsc.mvposi = (s32)XYAxisMMToPulse(FXZ_FXMV)*fzdir*(-1);				// 反向移动位移
	mtsc.lspd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);	// 起停速度
	mtsc.lspd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvzSpd);		// 归零速度
	mtsc.lspd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);		// 行走加速度
	mtsc.lspd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 刹车加速度


	mtsc.zerosensor = IsYAtZero;		// 零位检测传感器
	mtsc.limitpsensor = MoveYLimitPosi;	// 正向限位传感器
	mtsc.limitnsensor = MoveYLimitNega;	// 反向限位传感器

	mtsc.emcstopscan =  NULL;	// 停止条件
	mtsc.moveasoffset = MoveYAsOffset;	// 移动函数

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = LineRunToSensorCtrl(&mtsc);

	DelayRef(100);
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif

	if (err == STA_MTZ_SUCCESS && IsYAtZero() == SENSOR_ON)
	{
		MotoSpd MoSpd;
		MoSpd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);	// 起停速度
		MoSpd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvzSpd);		// 归零速度
		MoSpd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);		// 行走加速度
		MoSpd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 刹车加速度
		MoveYAsOffset(((s32)XYAxisMMToPulse(FXZ_FXMV)*fzdir*(-1)), &MoSpd);
		mtsc.vAxisId = VAXIS_ID1;			// 虚轴
		mtsc.axisIdx = MTY_IDX;				// 运动轴
		mtsc.axisCfg = MTY_CFG;				// 配置
		mtsc.poutType = MTY_OTP;

		mtsc.newCfg = 1;

		mtsc.movement = (s32)XYAxisMMToPulse(FXZ_FXMV*2)*fzdir;				// 最大移动位移

		mtsc.mvposi = (s32)XYAxisMMToPulse(FXZ_FXMV)*fzdir*(-1);				// 反向移动位移
		mtsc.lspd.stspSpd = XYAxisMMPSToPPS(1);	// 起停速度
		mtsc.lspd.runSpd = XYAxisMMPSToPPS(10);		// 归零速度
		mtsc.lspd.spdAdd = XYAxisMMPSSToPPSG(10);		// 行走加速度
		mtsc.lspd.brkAdd = XYAxisMMPSSToPPSG(100);	// 刹车加速度

		mtsc.zerosensor = IsYAtZero;		// 零位检测传感器
		mtsc.limitpsensor = MoveYLimitPosi;	// 正向限位传感器
		mtsc.limitnsensor = MoveYLimitNega;	// 反向限位传感器

		mtsc.emcstopscan =  NULL;	// 停止条件
		mtsc.moveasoffset = MoveYAsOffset;	// 移动函数

		err = LineRunToSensorCtrl(&mtsc);
		DelayRef(100);
		if (err == STA_MTZ_SUCCESS && IsYAtZero() == SENSOR_ON)
		{
			g_workCtrl.posEnFlag |= MTY_MOD;
			printf("FindYZero success\r\n");
			err = STA_MTZ_SUCCESS;
		}
		else
		{
			g_workCtrl.posEnFlag &= (~MTY_MOD);
			printf("FindYZero error, code=%d\r\n", err);
	//		err = ERR_MTZ_ERROR;	// 归零错误
		}
	}
	else
	{
		g_workCtrl.posEnFlag &= (~MTY_MOD);
		printf("FindYZero error, code=%d\r\n", err);
//		err = ERR_MTZ_ERROR;	// 归零错误
	}
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	printf("before set, MTYpos=%d, set to %d\r\n", g_motosPara.motosPos[MTY_IDX], g_workCtrl.zeroPosList[MTY_IDX]);

	g_workCtrl.zeroPosList[MTY_IDX] = g_pOpticalMcPara->ySensorPos;
	SetMotoCounter(MTY_IDX, g_workCtrl.zeroPosList[MTY_IDX]);		// 计数器初值

	ReadAndFreshMotosPos();

	printf("after set, MTY pos=%d\r\n", g_motosPara.motosPos[MTY_IDX]);
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	ReportError(err);
	return err;
}


void ConfigToYMoto(void)
{
	MotosConfig config;

	memset(&config, 0, sizeof(MotosConfig));
	config.newConfig = 1;
	config.axisConfig[MTY_IDX].axisConfig = MTY_CFG;					// 实轴输出配置
	config.axisConfig[MTY_IDX].poutType = MTY_OTP;						// 脉冲输出模式 脉冲方式输出
	config.axisConfig[MTY_IDX].spdSource = MAKE_POUT_CMD(VAXIS_ID1);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	config.axisConfig[MTY_IDX].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)

	SetMotosConfig(&config);

	DelayRef(100);
}



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

// XY 同步
#if (MODULE_X != 0 && MODULE_Y != 0)

// 得到合成轴长度
u32 GetSyntheticLength(s32 x, s32 y)
{
	double t1, t2;

	t1 = x;
	t1 *= x;

	t2 = y;
	t2 *= y;

	return (u32)(sqrt(t1+t2)+0.5);
}

// 急停检测
int MoveXYEmcScan(u32 para1, u32 para2)
{
	int rslt;
	s16 dirx, diry;

	dirx = (s16)para1;
	diry = (s16)(para1>>16);

	rslt = GetCommonEms();

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

	// X限位
	if (dirx > 0)
	{
		if (MoveXLimitPosi() == SENSOR_ON)		// X正向限位传感器
		{
			rslt = ERR_X_LIT_POSI;
		}
	}
	else if (dirx < 0)
	{
		if (MoveXLimitNega() == SENSOR_ON)		// X反向限位传感器
		{
			rslt = ERR_X_LIT_NEGA;
		}
	}
	else // if (dirx == 0)
	{
		rslt = ERR_NONE;
	}

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

	// Y限位
		if (diry > 0)
		{
			if (MoveYLimitPosi() == SENSOR_ON)		// Y正向限位传感器
			{
				rslt = ERR_Y_LIT_POSI;			// Y正向限位
			}
		}
		else if (diry < 0)
		{
			if (MoveYLimitNega() == SENSOR_ON)		// Y反向限位传感器
			{
				rslt = ERR_Y_LIT_NEGA;			// Y反向限位
			}
		}
		else // if (diry == 0)
		{
			rslt = ERR_NONE;
		}

		return rslt;
	}

// 快速停止检测
int MoveXYQStopScan(u32 para1, u32 para2)
{
	int rslt;
	rslt = GetCommonQStop();

	if (rslt == ERR_NONE)
	{

	}

	return rslt;
}


// 普通停止检测
int MoveXYNStopScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonNStop();

#if (MODULE_LOOSE == 1)			// 松料装置
	rslt = SendOpticalTask();			// 自动送光纤任务
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#endif

//	rslt = IsStopConditionWhenCutting();
	return rslt;
}

//-------------------------------------------------------------------------------
// XY 移动偏移值
int MoveXYAsOffset(s32 ofstx, s32 ofsty, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 xIdx, yIdx;
	MotoSpd spd;
	s16 dirx, diry;
#if (XY_GAP_CONVERT == 1)
	ofstx = XGapConvert(ofstx);	// 间隙补偿转换
	ofsty = YGapConvert(ofsty);	// 间隙补偿转换
#endif

	xIdx = MTX_IDX;
	yIdx = MTY_IDX;

	if (ofstx == 0 && ofsty == 0)
	{
		return ERR_NONE;
	}

	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);
		pSpd = &spd;
	}

	if (ofstx < 0)
	{
		dirx = -1;
	}
	else if (ofstx > 0)
	{
		dirx = 1;
	}
	else
	{
		dirx = 0;
	}

	if (ofsty < 0)
	{
		diry = -1;
	}
	else if (ofsty > 0)
	{
		diry = 1;
	}
	else
	{
		diry = 0;
	}

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 不需要更新电机配置
	lmctrl.motosConfig.axisConfig[xIdx].axisConfig = MTX_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[xIdx].poutType = MTX_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[xIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[xIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[xIdx] = ofstx;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

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

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofstx) > abs(ofsty) ? abs(ofstx) : abs(ofsty);	// 插补长轴位移	// GetSyntheticLength(ofstx, ofsty);	// 插补长轴位移，两个轴的合成长度
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;				// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;	// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;	// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 = lmctrl.vAxisId;			// 工作参数
	lmctrl.GetEmergencyStop = MoveXYEmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveXYQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveXYNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	//lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = (((u32)diry & 0xffff) << 16 | ((u32)dirx & 0xffff));
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

	err = GetMoveResultError(rslt, lmctrl.errInfo);
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	return err;
}

// 移动xy到绝对位置
int MoveXYToPos(s32 posx, s32 posy, MotoSpd * pSpd)
{
	int err;
	s32 ofstx, ofsty;

	g_workCtrl.sendStaWhenRun = 0;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	err = ERR_NONE;
	ReportError(err);

	ReadAndFreshMotosPos();

	if (g_motosPara.enFlag == DATA_VALID &&
		(g_workCtrl.posEnFlag & (MTX_MOD|MTY_MOD)) == (MTX_MOD|MTY_MOD) &&
		1	)
	{
		// 目标位置在区域内
		if (posx < g_pOpticalMcPara->xMovNegaBorder || 	// 工作区X负边界，单位0.01mm
			posx > g_pOpticalMcPara->xMovPosiBorder || 	// 工作区X正边界，单位0.01mm
			posy < g_pOpticalMcPara->yMovNegaBorder ||	// 工作区Y负边界，单位0.01mm
			posy > g_pOpticalMcPara->yMovPosiBorder || 	// 工作区Y正边界，单位0.01mm
			0	)
		{
			printf("\r\ntarget position out of range, x=%d, y=%d\r\n", posx, posy);
			err = ERR_OUT_RANGE;		// 目标位置越界
		}
		else
		{
			ReadAndFreshMotosPos();

			ofstx = posx - g_motosPara.motosPos[MTX_IDX];
			ofsty = posy - g_motosPara.motosPos[MTY_IDX];

			printf("\r\n before move\r\n");
//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);

//			printf("\r\n move ofstx = %d, ofsty = %d\r\n", ofstx, ofsty);
#if (MODULE_X != 0)
			printf("\txpos=%d\r\n", g_motosPara.motosPos[MTX_IDX]);
#endif

#if (MODULE_Y != 0)
			printf("\typos=%d\r\n", g_motosPara.motosPos[MTY_IDX]);
#endif


#if(LIFT_U1 != 0)
			printf("\tu1pos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);
#endif

			err = MoveXYAsOffset(ofstx, ofsty, pSpd);

			printf("\r\n after move\r\n");
#if (MODULE_X != 0)
			printf("\txpos=%d\r\n", g_motosPara.motosPos[MTX_IDX]);
#endif

#if (MODULE_Y != 0)
			printf("\typos=%d\r\n", g_motosPara.motosPos[MTY_IDX]);
#endif


#if(LIFT_U1 != 0)
			printf("\tu1pos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);
#endif

#if (TURN_R1 != 0)
			printf("\trpos=%d\r\n", g_motosPara.motosPos[MTR1_IDX]);
#endif

//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);
		}
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.sendStaWhenRun = 1;
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

int FindXYZero(void)
{
	int err;
#if (1)
	// xy 分别归零
	err = FindXZero();
	if ((err == STA_MTZ_SUCCESS) &&
		(IsXAtZero() == SENSOR_ON) &&
		(g_motosPara.enFlag == DATA_VALID) &&
		((g_workCtrl.posEnFlag & (MTX_MOD)) == (MTX_MOD)) &&
		1	)
	{
		err = FindYZero(-1);
	}
#else

#endif
	return err;
}

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

//---------------------------------------------------U1--------------------------------------------
#define	UAxisMMToPulse(mm)				MM_TO_PULSE(mm)
#define	UAxisMMPSToPPS(mmps)			MMPS_TO_PPS(mmps)
#define	UAxisMMPSSToPPSG(mmpss)			MMPSS_TO_PPSG(mmpss)

// 手动移动 U1

#define	MVU1BH_MVMT		(300)	// 300 mm

// 正向限位
u8 MoveU1BHLimitPosi(u32 para)
{
	// 读入坐标，软件限位
	if (g_motosPara.motosPos[MTU1_IDX] > g_pOpticalMcPara->rkLiftUper)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

// 反向限位
u8 MoveU1BHLimitNega(u32 para)
{
	// 读入坐标，软件限位
	if (g_motosPara.motosPos[MTU1_IDX] < g_pOpticalMcPara->rkLiftLower)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

int MoveU1BHPrec(u32 para)		// 先决条件
{
	int err;

	if ((g_motosPara.enFlag == DATA_VALID) &&
		(g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD) &&	// U1 坐标有效并且归零成功
		1	)
	{
		err = ERR_NONE;
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	return err;
}

int MoveU1BHStop(u32 para)		// 停止条件
{
	int rslt;

	rslt = ERR_NONE;

	if (IsNewOperCmd() == TRUE)				// 读取操作箱命令，如果有新命令
	{
		rslt = STA_NORMAL_STOP;
	}

	return rslt;
}

int MoveU1ByHand(int dir, int mmps)
{
	int err;
	MVByHandCtrl mbhc;
	int stspd;

	assert_param(dir == 1|| dir == -1);

	memset(&mbhc, 0, sizeof(MVByHandCtrl));

	mbhc.vAxisId = VAXIS_ID1;		// 虚轴
	mbhc.axisIdx = MTU1_IDX;			// 运动轴
	mbhc.axisCfg = MTU1_CFG;			// 配置
	mbhc.poutType = MTU1_OTP;
	mbhc.newCfg = 0;

	mbhc.movement = (s32)UAxisMMToPulse(MVU1BH_MVMT)*dir;	// 最大移动位移
	mbhc.ppSegment = PULSE_PER_SEGMENT;

	stspd = g_pOpticalMcPara->ustspSpd;
	if (stspd > mmps)
	{
		stspd = mmps;
	}
	mbhc.hspd.stspSpd = UAxisMMPSToPPS(stspd);	// 起停速度
	mbhc.hspd.runSpd = UAxisMMPSToPPS(mmps);	// 手动移动速度
	mbhc.hspd.spdAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uliftAdd);	// 行走加速度
	mbhc.hspd.brkAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uBrkAdd);		// 刹车加速度

	mbhc.limitpsensor = MoveU1BHLimitPosi;	// 正向限位
	mbhc.limitnsensor = MoveU1BHLimitNega;	// 反向限位

	mbhc.precondition = MoveU1BHPrec;	// 先决条件
	mbhc.stopscan = MoveU1BHStop;		// 停止条件
//	mbhc.slowscan = NULL;				// 慢速条件

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = MoveByHandCtrl(&mbhc);
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

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

// 急停检测
int MoveU1EmcScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonEms();

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

	return rslt;
}


// 快速停止检测
int MoveU1QStopScan(u32 para1, u32 para2)
{
	int rslt;
	rslt = GetCommonQStop();

	if (rslt == ERR_NONE)
	{

	}

	return rslt;
}
// 普通停止检测
int MoveU1NStopScan(u32 para1, u32 para2)
{
	int rslt;

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

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#if (MODULE_LOOSE == 1)			// 松料装置
	rslt = SendOpticalTask();			// 自动送光纤任务
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#endif

	return rslt;
}
// 普通停止检测
int MoveS1P1NStopScan(u32 para1, u32 para2)
{
	int rslt;

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

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#if (MODULE_LOOSE == 1)			// 松料装置
	rslt = SendOpticalTask();			// 自动送光纤任务
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#endif
#if(MATERIAL_SCAN == 1)
	rslt  = isEmptyMaterialSta(g_pOpticalMcPara->xymvlSpd/10);//扫描周期按10ms
//	rslt = IsStopConditionWhenCutting();
#endif
	return rslt;
}

// U1轴偏移移动
int MoveU1AsOffset(s32 ofstu, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 pIdx;
	MotoSpd spd;

	pIdx = MTU1_IDX;

	if (ofstu == 0)
	{
		return ERR_NONE;
	}

	if (pSpd == NULL)
	{
		spd.stspSpd = UAxisMMPSToPPS(g_pOpticalMcPara->ustspSpd);
		spd.runSpd = UAxisMMPSToPPS(g_pOpticalMcPara->umoveSpd);
		spd.spdAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uliftAdd);
		spd.brkAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uBrkAdd);
		pSpd = &spd;
	}

//	printf("MoveU1AsOffset, ofstu=%d, spd=%d\r\n", ofstu, pSpd->runSpd);
//	printf("before move, motospos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[pIdx].axisConfig = MTU1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[pIdx].poutType = MTU1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[pIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[pIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[pIdx] = ofstu;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofstu);							// 插补长轴位移
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;			// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;		// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;		// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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.GetEmergencyStop = MoveU1EmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveU1QStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveU1NStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
//	lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = 0;
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

//	printf("after move, motospos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);

	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}

//-------------------------------------------------------------------------------
// U1归零

#define	FU1Z_MVMT		(100)	// 100 mm
#define	FU1Z_FXMV		(10)	// 10 mm

u8 IsU1AtZeroPos(void)
{// U1传感器软件滤波
	static s32 count = 0;

	if ((g_workCtrl.cursta.workStatus & WORK_STA_BUSY) == 0)
	{
		count = 0;
	}
	if (IsU1ZeroPos() == SENSOR_ON)
	{
		if (count < 50)
		{
			count++;
		}
	}
	else
	{
		if (count > -50)
		{
			count--;
		}
	}

	if (count > 0)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

int FindU1Zero(int fzdir)
{
	int err;
	MTSCtrl mtsc;

	// 标志失效
	g_workCtrl.posEnFlag &= (~MTU1_MOD);

	ReadAndFreshMotosPos();

	memset(&mtsc, 0, sizeof(MTSCtrl));

	mtsc.vAxisId = VAXIS_ID1;			// 虚轴
	mtsc.axisIdx = MTU1_IDX;			// 运动轴
	mtsc.axisCfg = MTU1_CFG;			// 配置
	mtsc.poutType = MTU1_OTP;

	mtsc.newCfg = 0;

	mtsc.movement = (s32)UAxisMMToPulse(FU1Z_MVMT)*fzdir;				// 最大移动位移
	mtsc.mvposi = (s32)UAxisMMToPulse(FU1Z_FXMV)*fzdir*(-1);			// 反向移动位移

	mtsc.lspd.stspSpd = UAxisMMPSToPPS(g_pOpticalMcPara->ustspSpd);		// 起停速度
	mtsc.lspd.runSpd = UAxisMMPSToPPS(g_pOpticalMcPara->umvtzSpd);		// 归零速度
	mtsc.lspd.spdAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uliftAdd);	// 行走加速度
	mtsc.lspd.brkAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uBrkAdd);		// 刹车加速度

	mtsc.slowsensor = NULL;				// 降速检测传感器
	mtsc.zerosensor = IsU1ZeroPos;		// 零位检测传感器

	mtsc.limitpsensor = NULL;		// 正向限位传感器
	mtsc.limitnsensor = NULL;		// 反向限位传感器

	mtsc.emcstopscan = NULL;	// 停止条件
	mtsc.moveasoffset = MoveU1AsOffset;	// 移动函数

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = LineRunToSensorCtrl(&mtsc);
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	DelayRef(100);

	if (err == STA_MTZ_SUCCESS && IsU1ZeroPos() == SENSOR_ON)
	{
		g_workCtrl.posEnFlag |= MTU1_MOD;
		printf("FindU1Zero success\r\n");
		err = STA_MTZ_SUCCESS;
	}
	else
	{
		g_workCtrl.posEnFlag &= (~MTU1_MOD);
		printf("FindU1Zero error, code=%d\r\n", err);
		err = ERR_MTZ_ERROR;	// 归零错误
	}

	printf("FindU1Zero, before set, MTU1 pos=%d, set to %d\r\n", g_motosPara.motosPos[MTU1_IDX], g_workCtrl.zeroPosList[MTU1_IDX]);

	g_workCtrl.zeroPosList[MTU1_IDX] = g_pOpticalMcPara->u1SensorPos;
	SetMotoCounter(MTU1_IDX, g_workCtrl.zeroPosList[MTU1_IDX]);		// 计数器初值
	ReadAndFreshMotosPos();

	printf("FindU1Zero, after set, MTU1 pos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);

	ReportError(err);

	return err;
}

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

// 移动U1到绝对位置
int MoveU1ToPos(s32 posu, MotoSpd * pSpd)
{
	int err;
	s32 ofstu;

	err = ERR_NONE;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	if (g_motosPara.enFlag == DATA_VALID &&
		(g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD) &&
		1	)
	{
		// 目标位置在区域内
		if (0 ||
			posu < g_pOpticalMcPara->rkLiftLower || 	// 机头升降下边界，单位0.01mm
			posu > g_pOpticalMcPara->rkLiftUper  || 	// 机头升降上边界，单位0.01mm
			0	)
		{
			printf("\r\ntarget position out of range, u1=%d, posu1=%d\r\n", g_motosPara.motosPos[MTU1_IDX], posu);
			err = ERR_OUT_RANGE;		// 目标位置越界
		}
		else
		{
				ReadAndFreshMotosPos();

				ofstu = posu - g_motosPara.motosPos[MTU1_IDX];

	//			printf("\r\n before move\r\n");
	//			printf("actpos: x=%d, p=%d\r\n", g_motosPara.motosPos[MTU1_IDX], g_motosPara.motosPos[MTU1_IDX]);

	//			printf("\r\n move ofstx = %d, ofsty = %d\r\n", 0, ofstu);

				if (ofstu != 0)
				{
					err = MoveU1AsOffset(ofstu, pSpd);
				}

	//			printf("\r\n after move\r\n");
	//			printf("actpos: x=%d, p=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTP_IDX]);

		}
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	ReportError(err);
	return err;
}
// 移动U1到绝对位置(抬刀用,抬刀时上料夹松开送料)
int MoveTU1ToPos(s32 posu, MotoSpd * pSpd)
{
	int err;
	s32 ofstu;

	err = ERR_NONE;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	UpClipMatClipCylinderOff();        // 上夹料关
	if (g_motosPara.enFlag == DATA_VALID &&
		(g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD) &&
		1	)
	{
		// 目标位置在区域内
		if (0 ||
			posu < g_pOpticalMcPara->rkLiftLower || 	// 机头升降下边界，单位0.01mm
			posu > g_pOpticalMcPara->rkLiftUper  || 	// 机头升降上边界，单位0.01mm
			0	)
		{
			printf("\r\ntarget position out of range, u1=%d, posu1=%d\r\n", g_motosPara.motosPos[MTU1_IDX], posu);
			err = ERR_OUT_RANGE;		// 目标位置越界
		}
		else
		{
				ReadAndFreshMotosPos();

				ofstu = posu - g_motosPara.motosPos[MTU1_IDX];

	//			printf("\r\n before move\r\n");
	//			printf("actpos: x=%d, p=%d\r\n", g_motosPara.motosPos[MTU1_IDX], g_motosPara.motosPos[MTU1_IDX]);

	//			printf("\r\n move ofstx = %d, ofsty = %d\r\n", 0, ofstu);

				if (ofstu != 0)
				{
					err = MoveU1AsOffset(ofstu, pSpd);
				}

	//			printf("\r\n after move\r\n");
	//			printf("actpos: x=%d, p=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTP_IDX]);

		}
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}
	UpClipMatClipCylinderOn();        // 上夹料夹紧
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	ReportError(err);
	return err;
}

// 放下U1
int MoveU1ToLower(void)
{
	int rslt;
	rslt = ERR_NONE;
	if (!(1 &&
 		 (g_workCtrl.paraEnFlag == PARA_VALID) &&
		 (g_motosPara.enFlag == DATA_VALID) &&
		 ((g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD)) &&
		 1	))
	{
		rslt = FindU1Zero(1);
	}

	if ((rslt == STA_MTZ_SUCCESS || rslt == ERR_NONE) &&
		(g_workCtrl.paraEnFlag == PARA_VALID) &&
		 (g_motosPara.enFlag == DATA_VALID) &&
		 ((g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD)) &&
		 1	)
	{
		rslt = MoveU1ToPos(g_pOpticalMcPara->rkLowPos, NULL);	// 右刀升降低位高度
	}
	else
	{
		rslt = ERR_HEAD_NOT_LOW;
	}

	return rslt;
}

// 抬起U1
int MoveU1ToUpper(s32 adj)
{
	int rslt;
	rslt = ERR_NONE;

	if (!(1 &&
 		 (g_workCtrl.paraEnFlag == PARA_VALID) &&
		 (g_motosPara.enFlag == DATA_VALID) &&
		 ((g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD)) &&
		 1	))
	{
		rslt = FindU1Zero(1);
	}

	if ((rslt == STA_MTZ_SUCCESS || rslt == ERR_NONE))
	{
		if (g_workCtrl.paraEnFlag == PARA_VALID)
		{
			if((g_motosPara.enFlag == DATA_VALID) &&
			 ((g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD)) &&
			1	)
			{
				rslt = MoveTU1ToPos(g_pOpticalMcPara->rkHighPos+adj, NULL);		// 右刀升降高位高度

			}
			else
			{
				rslt = ERR_HEAD_NOT_HIGH;	// 机头不在高位
			}
		}
		else
		{
			rslt = ERR_MC_PARA;		// 机器参数错误
		}
	}
	return rslt;
}

// 抬起U1(归零时上夹料不松开)
int MoveZU1ToUpper(void)
{
	int rslt;
	rslt = ERR_NONE;

	if (!(1 &&
 		 (g_workCtrl.paraEnFlag == PARA_VALID) &&
		 (g_motosPara.enFlag == DATA_VALID) &&
		 ((g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD)) &&
		 1	))
	{
		rslt = FindU1Zero(1);
	}

	if ((rslt == STA_MTZ_SUCCESS || rslt == ERR_NONE))
	{
		if (g_workCtrl.paraEnFlag == PARA_VALID)
		{
			if((g_motosPara.enFlag == DATA_VALID) &&
			 ((g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD)) &&
			1	)
			{
				rslt = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);		// 右刀升降高位高度

			}
			else
			{
				rslt = ERR_HEAD_NOT_HIGH;	// 机头不在高位
			}
		}
		else
		{
			rslt = ERR_MC_PARA;		// 机器参数错误
		}
	}
	return rslt;
}

// 电机升降
int MoveUToUpper(void)
{
	int err;

	err = ERR_NONE;
#if (LIFT_U1 != 0)
	if (err == ERR_NONE)
	{
		err = MoveU1ToUpper(0);			// 抬起右刀
	}
#endif
	return err;
}
//-------------------------------------------------------------------------------------------




//---------------------------------------------------R1--------------------------------------------
#define	RAxisMMToPulse(mm)				MM_TO_PULSE(mm)
#define	RAxisMMPSToPPS(mmps)			MMPS_TO_PPS(mmps)
#define	RAxisMMPSSToPPSG(mmpss)			MMPSS_TO_PPSG(mmpss)

// 速度
#define	MIN_R1_DPS		1
#define	MAX_R1_DPS		(10*ROT_PULSE_PER_CIRCLE)		// 最大速度, 10 圈/秒

#define	MVR1BH_MVMT		10000	// 最大位移 10000 圈

u8 IsR1AtZeroPos(void)
{// R1传感器软件滤波
	static s32 count = 0;

	if ((g_workCtrl.cursta.workStatus & WORK_STA_BUSY) == 0)
	{
		count = -1000;
	}

	if (IsR1ZeroPos() == SENSOR_ON)
	{
		if (count < 1000)
		{
			count++;
		}
	}
	else
	{
		if (count > -1000)
		{
			count--;
		}
	}

	if (count > 0)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

u8 GetR1ZeroPosRst(void)
{// R1传感器软件滤波
//	return IsR1AtZeroPos(1);

	static s32 count = 0;

	if (IsR1ZeroPos() == SENSOR_ON)
	{
		if (count < 5000)
		{
			count++;
		}
	}
	else
	{
		if (count > -5000)
		{
			count--;
		}
	}

	if (count > 0)
	{
		return SENSOR_ON;
	}
	else
	{
		return SENSOR_OFF;
	}
}

// 跟踪旋转归零
int FindR1Zero(void)
{
	int err;
	RSenserCheck rpck;

	// 标志失效
	g_workCtrl.posEnFlag &= (~MTR1_MOD);
	err = ERR_NONE;

#if(LIFT_U1 != 0)
	err = MoveZU1ToUpper();		// 抬起右刀
#endif
	if (err != ERR_NONE)
	{
		ReportError(err);
		return err;
	}

	ReadAndFreshMotosPos();

	memset(&rpck, 0, sizeof(RSenserCheck));

	rpck.vAxisId = VAXIS_ID1;	// 虚轴
	rpck.axisIdx = MTR1_IDX;	// 运动轴
	rpck.axisCfg = MTR1_CFG;	// 配置
	rpck.axisOtp = MTR1_OTP;	// 输出
	rpck.newCfg = 1;			// 重新配置标志

	rpck.circleNum = ROT_PULSE_PER_CIRCLE;				// 整圈脉冲数
	rpck.rspd.stspSpd = DPS_TO_PPS(g_pOpticalMcPara->rotstspDps);
	rpck.rspd.runSpd = DPS_TO_PPS(g_pOpticalMcPara->rottzDps);
	rpck.rspd.spdAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotAddDpss);
	rpck.rspd.brkAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotBrkDpss);

	rpck.zeroflag = 0;		// 已经在位置标志
	rpck.cansflag = 0;		// 跳过检测标志
	rpck.canscount = 0;		// 跳过位移

	rpck.sensorscan = IsR1ZeroPos;	// 跟踪旋转零位
	rpck.val = SENSOR_ON;

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = RoundRunToSensor(&rpck);
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	DelayRef(100);

	if (err == STA_NORMAL_STOP && rpck.sensorscan() == rpck.val)
	{
		g_workCtrl.posEnFlag |= MTR1_MOD;

		printf("FindR1Zero success\r\n");
		err = STA_MTZ_SUCCESS;
	}
	else
	{
		g_workCtrl.posEnFlag &= (~MTR1_MOD);

		printf("FindR1Zero error, code=%d\r\n", err);
//		err = ERR_MTZ_ERROR;	// 归零错误
	}

	if ((err == STA_MTZ_SUCCESS) &&
		((g_workCtrl.posEnFlag & (MTR1_MOD)) == (MTR1_MOD)) &&
		(IsR1ZeroPos() == SENSOR_ON) &&
		1	)
	{
		printf("before set, MTR1 pos=%d, set to %d\r\n", g_motosPara.motosPos[MTR1_IDX], g_workCtrl.zeroPosList[MTR1_IDX]);

		g_workCtrl.zeroPosList[MTR1_IDX] = ROT_001DEG_TO_PULSE(g_pOpticalMcPara->r1SensorAng);
		SetMotoCounter(MTR1_IDX, g_workCtrl.zeroPosList[MTR1_IDX]);		// 计数器初值
		ReadAndFreshMotosPos();

		printf("after set, MTR1 pos=%d\r\n", g_motosPara.motosPos[MTR1_IDX]);

		MoveR1ToPos(2500, NULL);//转到绝对坐标90°
	}
	else
	{
		g_workCtrl.posEnFlag &= (~MTR1_MOD);
	}

	ReportError(err);
	return err;
}

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

int MoveR1BHStop(u32 para)		// 停止条件
{
	int rslt;

	rslt = ERR_NONE;

	if (IsNewOperCmd() == TRUE)				// 读取操作箱命令，如果有新命令
	{
		rslt = STA_NORMAL_STOP;
	}

	return rslt;
}

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

// dir == 1 正向
// dir == -1, 反向

// dps 速度，单位 度/秒

int MoveR1ByHand(int dir, int dps)
{
	int err;
	int stdps;
	MVByHandCtrl mbhc;

	assert_param(dir == 1|| dir == -1);

	if (dps < MIN_R1_DPS)
	{
		dps = MIN_R1_DPS;
	}
	else if (dps > MAX_R1_DPS)
	{
		dps = MAX_R1_DPS;
	}

	err = ERR_NONE;

#if (1)

#if(LIFT_U1 != 0)
	err = MoveU1ToUpper(0);		// 抬起右刀
#endif

	if (err != ERR_NONE)
	{
		ReportError(err);
		return err;
	}
#endif

	memset(&mbhc, 0, sizeof(MVByHandCtrl));

	mbhc.vAxisId = VAXIS_ID1;			// 虚轴

	mbhc.axisIdx = MTR1_IDX;			// 运动轴
	mbhc.axisCfg = MTR1_CFG;			// 配置
	mbhc.poutType = MTR1_OTP;			// 输出
	mbhc.newCfg = 1;

	mbhc.movement = MVR1BH_MVMT * ROT_PULSE_PER_CIRCLE;	// 最大移动位移
	mbhc.movement *= dir;
	mbhc.ppSegment = ROT_PULSE_PER_CIRCLE;

	stdps = g_pOpticalMcPara->rotstspDps;
	if (stdps > dps)
	{
		stdps = dps;
	}

	mbhc.hspd.stspSpd = DPS_TO_PPS(stdps);							// 起停速度
	mbhc.hspd.runSpd = DPS_TO_PPS(dps);								// 运转速度
	mbhc.hspd.spdAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotAddDpss);	// 行走加速度
	mbhc.hspd.brkAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotBrkDpss);	// 刹车加速度

	mbhc.limitpsensor = NULL;		// 正向限位
	mbhc.limitnsensor = NULL;		// 反向限位

	mbhc.precondition = NULL;		// 先决条件
	mbhc.stopscan = MoveR1BHStop;	// 停止条件
	//mbhc.slowscan = NULL;			// 慢速条件

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = MoveByHandCtrl(&mbhc);
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

// 移动
int MoveR1AsOffset(s32 ofstr, MotoSpd * pSpd)
{
	int err, i;
	RMotoRun rmr;
	MotoSpd rspd;		// 速度

	if (ofstr == 0)
	{
		return ERR_NONE;
	}

	err = ERR_NONE;

	memset(&rmr, 0, sizeof(RMotoRun));

	rmr.vAxisId = VAXIS_ID1;	// 虚轴
	rmr.mrepeat = 0;			// 重复延伸
	rmr.interLong = abs(ofstr);	// 长轴位移
	rmr.stopmode = 0;			// 停止模式

	rmr.newCfg = 0;				// 不需要重新配置

	rmr.config[0].axisIdx = MTR1_IDX;	// 索引
	rmr.config[0].axisCfg = MTR1_CFG;	// 配置
	rmr.config[0].movement = ofstr;		// 运动量
	rmr.escStop = 0;
	rmr.segmentNum = ROT_PULSE_PER_CIRCLE;

	for (i = 1; i < AXIS_NUM; i++)
	{
		rmr.config[i].axisIdx = -1;
	}

	if (pSpd == NULL)
	{
		rspd.stspSpd = DPS_TO_PPS(g_pOpticalMcPara->rotstspDps);	// 启停速度
		rspd.runSpd = DPS_TO_PPS(g_pOpticalMcPara->rotrunDps);		// 运转转速
		rspd.spdAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotAddDpss);	// 加速度
		rspd.brkAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotBrkDpss);	// 刹车加速度

		memcpy(&rmr.rspd, &rspd, sizeof(MotoSpd));		// 速度
	}
	else
	{
		memcpy(&rmr.rspd, pSpd, sizeof(MotoSpd));		// 速度
	}

	err = RoundRunAsOffset(&rmr);
	return err;
}

int MoveR1ToPos(s32 posr, MotoSpd * pSpd)
{
	int err;
	s32 ofstr, abposr;

	g_workCtrl.sendStaWhenRun = 0;

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	err = ERR_NONE;
	ReportError(err);

	if (g_motosPara.enFlag == DATA_VALID &&
		(g_workCtrl.posEnFlag & (MTR1_MOD)) == (MTR1_MOD) &&
		1	)
	{
		// 得到一圈内的相对位置
		ReadAndFreshMotosPos();
		// 转化为 -180--180度之间的数


		abposr = g_motosPara.motosPos[MTR1_IDX];
		ofstr = posr - abposr;
		ChangeRotToNP180(&ofstr);
		err = MoveR1AsOffset(ofstr, NULL);

#if (0)		// 设置绝对位置
		abposr = g_motosPara.motosPos[MTR1_IDX];

		ChangeRotToNP360(&abposr);

		if (abposr != g_motosPara.motosPos[MTR1_IDX])	// 重置计数器
		{
			SetMotoCounter(MTR1_IDX, abposr);		// 计数器初值
			ReadAndFreshMotosPos();
		}
#endif
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.sendStaWhenRun = 1;

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	ReportError(err);
	return err;
}
//-------------------------------------------------------------------------------------------
//---------------------------------------------------S1--------------------------------------------

// 手动移动L

#define	MVSBH_MVMT		(3000)	// 3000 mm


int MoveSBHPrec(u32 para)		// 先决条件
{
	int err;

	if ((g_motosPara.enFlag == DATA_VALID) &&
		(g_workCtrl.posEnFlag & (MTS1_IDX)) == (MTS1_IDX) &&	//L 坐标有效并且归零成功
		1	)
	{
		err = ERR_NONE;
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	return err;
}

int MoveSBHStop(u32 para)		// 停止条件
{
	int rslt;

	rslt = ERR_NONE;

	if (IsNewOperCmd() == TRUE)				// 读取操作箱命令，如果有新命令
	{
		rslt = STA_NORMAL_STOP;
	}
	else
	{
	}

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

int MoveSBHSlow(u32 para)		// 慢速条件
{
	int rslt;
	int dir;

	dir = (int)(para);

	rslt = ERR_NONE;
	if (dir != 0)
	{
		u32 curpps;
		s32 decCount;

		if ((g_motosPara.enFlag == DATA_VALID) &&
			(g_workCtrl.posEnFlag & (MTS1_IDX)) == (MTS1_IDX) &&					//L 坐标有效并且归零成功
			1	)
		{
			curpps = GetVAxisPPS(VAXIS_ID1);					// 当前速度
			decCount = CalcDisplacement(curpps, XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd), XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd)*TOSLOWMUTI);	// 计算以当前运行速度减速到停止速度需要的位移

			decCount += 200;	// 补偿值

			if (dir > 0)		// 正向
			{
				if (g_motosPara.motosPos[MTS1_IDX] + decCount >g_pOpticalMcPara->xMovPosiBorder)
				{
					rslt = STA_NORMAL_STOP;		// 降速状态
				}
			}
			else if (dir < 0)	// 反向
			{
				if (g_motosPara.motosPos[MTS1_IDX] - decCount <g_pOpticalMcPara->xMovNegaBorder)
				{
					rslt = STA_NORMAL_STOP;		// 降速状态
				}
			}
		}
	}

	return rslt;
}

int MoveSByHand(int dir, int spdLev)
{
	int err;
    int stspd, rspd;
	MVByHandCtrl mbhc;

	assert_param(dir == 1|| dir == -1);

	err = ERR_NONE;

	memset(&mbhc, 0, sizeof(MVByHandCtrl));

	mbhc.vAxisId = VAXIS_ID1;		// 虚轴
	mbhc.axisIdx = MTS1_IDX;			// 运动轴
	mbhc.axisCfg = MTS1_CFG;			// 配置
	mbhc.poutType = MTS1_OTP;
	mbhc.newCfg = 1;

	mbhc.movement = (s32)XYAxisMMToPulse(MVSBH_MVMT)*dir;	// 最大移动位移
	mbhc.ppSegment = PULSE_PER_SEGMENT;

	stspd =g_pOpticalMcPara->lmvstspSpd;

	if (spdLev <= 1)
	{
		rspd =g_pOpticalMcPara->lmvhSpd1;	// 手动移动速度等级1
	}
	else if (spdLev == 2)
	{
		rspd =g_pOpticalMcPara->lmvhSpd2;	// 手动移动速度等级2
	}
	else if (spdLev >= 3)
	{
		rspd =g_pOpticalMcPara->lmvhSpd3;	// 手动移动速度等级3
	}

	if (stspd > rspd)
	{
		stspd = rspd;
	}

	mbhc.hspd.stspSpd = XYAxisMMPSToPPS(stspd);							// 起停速度
	mbhc.hspd.runSpd = XYAxisMMPSToPPS(rspd);
	mbhc.hspd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvSAdd);	// 行走加速度
	mbhc.hspd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvbrkAdd);	// 刹车加速度

	mbhc.precondition = NULL;// MoveSBHPrec;	// 先决条件
	mbhc.stopscan = MoveSBHStop;		// 停止条件
	//mbhc.slowscan = MoveSBHSlow;		// 慢速条件

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = MoveByHandCtrl(&mbhc);
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

////-------------------------------------------------------------------------------
// L 移动偏移值
int MoveSAsOffset(s32 ofstl, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 lIdx;
	MotoSpd spd;
	s16 dirl;

	lIdx = MTS1_IDX;

	if (ofstl == 0)
	{
		return ERR_NONE;
	}

	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->lmvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->lmvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvbrkAdd);
		pSpd = &spd;
	}

	if (ofstl < 0)
	{
		dirl = -1;
	}
	else if (ofstl > 0)
	{
		dirl = 1;
	}
	else
	{
		dirl = 0;
	}

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 不需要更新电机配置
	lmctrl.motosConfig.axisConfig[lIdx].axisConfig = MTS1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[lIdx].poutType = MTS1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[lIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[lIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[lIdx] = ofstl;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofstl);	// 插补长轴位移	// GetSyntheticLength(ofstx, ofsty);	// 插补长轴位移，两个轴的合成长度
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;				// 每段脉冲数

//	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;	// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;	// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 = lmctrl.vAxisId;			// 工作参数
	lmctrl.GetEmergencyStop = MoveXEmcScan;	    // 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveXQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveXNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	//lmctrl.GetSlowDown = NULL;				// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = ((u32)dirl & 0xffff);
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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


	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}

// 移动s到绝对位置
int MoveSToPos(s32 poss, MotoSpd * pSpd)
{
	int err;
	s32 ofsts;

	g_workCtrl.sendStaWhenRun = 0;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	//ReportError(err);

	ReadAndFreshMotosPos();

	if (1)
	{
			ReadAndFreshMotosPos();

			ofsts = poss - g_motosPara.motosPos[MTS1_IDX];

			printf("\r\n before move\r\n");

//			printf("\r\n move ofstx = %d, ofsty = %d\r\n", ofstx, ofsty);

			printf("\txpos=%d\r\n", g_motosPara.motosPos[MTS1_IDX]);


			err = MoveSAsOffset(ofsts, pSpd);

			printf("\r\n after move\r\n");
			printf("\txpos=%d\r\n", g_motosPara.motosPos[MTS1_IDX]);

//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.sendStaWhenRun = 1;
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

#endif

//-------------------------------------------------------------------------------
//送料\拉料电机异步

// 电机1控制，拉料电机
int MTS1MotoCanMoveTest_a(void)
{
	int err = ERR_NONE;

	return err;
}

// 电机1快速停止条件
int MoveMTS1QuickStop_a(u32 para1,u32 para2)
{
	return 0;
}


// 手动移动
int MoveMTS1BHStop_a(u32 para1,u32 para2)		// 停止条件
{
	int rslt;

	rslt = ERR_NONE;

	if (IsNewOperCmd() == TRUE)				// 读取操作箱命令，如果有新命令
	{
		rslt = STA_NORMAL_STOP;
	}
	else if (IsConsoleCancel() != 0)		// 终端输入停止
	{
		rslt = STA_NORMAL_STOP;
	}
	else
	{
	}

	return rslt;
}

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



//---------------------------------------------P----------------------------------------------
#define	MVPBH_MVMT		(3000)	// 3000 mm

int MTP_1PulseToP1Pulse(s32 Pulse)
{
	Pulse *= 8.488;//    8.488p/mm
	return Pulse;
}

int MovePBHPrec(u32 para)		// 先决条件
{
	int err;

	if ((g_motosPara.enFlag == DATA_VALID) &&
		(g_workCtrl.posEnFlag & (MTP1_IDX)) == (MTP1_IDX) &&	//L 坐标有效并且归零成功
		1	)
	{
		err = ERR_NONE;
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	return err;
}

int MovePBHStop(u32 para)		// 停止条件
{
	int rslt;

	rslt = ERR_NONE;

	if (IsNewOperCmd() == TRUE)				// 读取操作箱命令，如果有新命令
	{
		rslt = STA_NORMAL_STOP;
	}
	else
	{
	}

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

int MovePBHSlow(u32 para)		// 慢速条件
{
	int rslt;
	int dir;

	dir = (int)(para);

	rslt = ERR_NONE;
	if (dir != 0)
	{
		u32 curpps;
		s32 decCount;

		if ((g_motosPara.enFlag == DATA_VALID) &&
			(g_workCtrl.posEnFlag & (MTP1_IDX)) == (MTP1_IDX) &&					//L 坐标有效并且归零成功
			1	)
		{
			curpps = GetVAxisPPS(VAXIS_ID1);					// 当前速度
			decCount = CalcDisplacement(curpps, XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd), XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd)*TOSLOWMUTI);	// 计算以当前运行速度减速到停止速度需要的位移

			decCount += 200;	// 补偿值

			if (dir > 0)		// 正向
			{
				if (g_motosPara.motosPos[MTP1_IDX] + decCount >g_pOpticalMcPara->xMovPosiBorder)
				{
					rslt = STA_NORMAL_STOP;		// 降速状态
				}
			}
			else if (dir < 0)	// 反向
			{
				if (g_motosPara.motosPos[MTP1_IDX] - decCount <g_pOpticalMcPara->xMovNegaBorder)
				{
					rslt = STA_NORMAL_STOP;		// 降速状态
				}
			}
		}
	}

	return rslt;
}

int MovePByHand(int dir, int spdLev)
{
	int err;
    int stspd, rspd;
	MVByHandCtrl mbhc;

	assert_param(dir == 1|| dir == -1);

	err = ERR_NONE;

	memset(&mbhc, 0, sizeof(MVByHandCtrl));

	mbhc.vAxisId = VAXIS_ID1;		// 虚轴
	mbhc.axisIdx = MTP1_IDX;			// 运动轴
	mbhc.axisCfg = MTP1_CFG;			// 配置
	mbhc.poutType = MTP1_OTP;
	mbhc.newCfg = 1;

	mbhc.movement = (s32)XYAxisMMToPulse(MVPBH_MVMT)*dir;	// 最大移动位移
	mbhc.ppSegment = PULSE_PER_SEGMENT;

	stspd =g_pOpticalMcPara->lmvstspSpd;

	if (spdLev <= 1)
	{
		rspd =g_pOpticalMcPara->lmvhSpd1;	// 手动移动速度等级1
	}
	else if (spdLev == 2)
	{
		rspd =g_pOpticalMcPara->lmvhSpd2;	// 手动移动速度等级2
	}
	else if (spdLev >= 3)
	{		rspd =g_pOpticalMcPara->lmvhSpd3;	// 手动移动速度等级3
	}

	if (stspd > rspd)
	{
		stspd = rspd;
	}

	mbhc.hspd.stspSpd = XYAxisMMPSToPPS(stspd);							// 起停速度
	mbhc.hspd.runSpd = XYAxisMMPSToPPS(rspd);
	mbhc.hspd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvSAdd);	// 行走加速度
	mbhc.hspd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvbrkAdd);	// 刹车加速度

	mbhc.precondition = NULL;// MovePBHPrec;	// 先决条件
	mbhc.stopscan = MovePBHStop;		// 停止条件
	//mbhc.slowscan = MovePBHSlow;		// 慢速条件

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	err = MoveByHandCtrl(&mbhc);
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

////-------------------------------------------------------------------------------
// L 移动偏移值
int MovePAsOffset(s32 ofstl, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 lIdx;
	MotoSpd spd;
	s16 dirl;

	lIdx = MTP1_IDX;

	if (ofstl == 0)
	{
		return ERR_NONE;
	}

	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->lmvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->lmvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvbrkAdd);
		pSpd = &spd;
	}

	if (ofstl < 0)
	{
		dirl = -1;
	}
	else if (ofstl > 0)
	{
		dirl = 1;
	}
	else
	{
		dirl = 0;
	}

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 不需要更新电机配置
	lmctrl.motosConfig.axisConfig[lIdx].axisConfig = MTP1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[lIdx].poutType = MTP1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[lIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[lIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[lIdx] = ofstl;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofstl);	// 插补长轴位移	// GetSyntheticLength(ofstx, ofsty);	// 插补长轴位移，两个轴的合成长度
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;				// 每段脉冲数

//	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;	// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;	// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 = lmctrl.vAxisId;			// 工作参数
	lmctrl.GetEmergencyStop = MoveXEmcScan;	    // 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveXQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveXNStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	//lmctrl.GetSlowDown = NULL;				// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = ((u32)dirl & 0xffff);
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}
// 移动P到绝对位置
int MovePToPos(s32 poss, MotoSpd * pSpd)
{
	int err;
	s32 ofsts;

	g_workCtrl.sendStaWhenRun = 0;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	//ReportError(err);

	ReadAndFreshMotosPos();

	if (1)
	{
			ReadAndFreshMotosPos();

			ofsts = poss - g_motosPara.motosPos[MTP1_IDX];

			printf("\r\n before move\r\n");

//			printf("\r\n move ofstP1 = %d, ofsty = %d\r\n", ofstP1, ofsty);

			printf("\txpos=%d\r\n", g_motosPara.motosPos[MTP1_IDX]);


			err = MovePAsOffset(ofsts, pSpd);

			printf("\r\n after move\r\n");
			printf("\txpos=%d\r\n", g_motosPara.motosPos[MTP1_IDX]);

//			printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.sendStaWhenRun = 1;
	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}
//-------------------------------------------------------------------------------




//---------------------------------------------S----------------------------------------------



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

// xyrsp 同步移动
#if (1)

// 急停检测

#if ((MODULE_X != 0) &&  (MODULE_Y != 0)  && (TURN_R1 != 0))
int MoveXYREmcScan(u32 para1, u32 para2)
{
	int rslt;
	s8 dirx, diry, dirr1;

	dirx = (s8)para1;
	diry = (s8)(para1>>8);
	dirr1 = (s8)(para1>>16);

	rslt = GetCommonEms();

	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#if (MODULE_X != 0)
	// X限位
	if (dirx > 0)
	{
		if (MoveXLimitPosi() == SENSOR_ON)		// X正向限位传感器
		{
			rslt = ERR_X_LIT_POSI;	//
			printf("x=%d\r\n",g_motosPara.motosPos[MTX_IDX]);
		}
	}
	else if (dirx < 0)
	{
		if (MoveXLimitNega() == SENSOR_ON)		// X反向限位传感器
		{
			rslt = ERR_X_LIT_NEGA;	//
			printf("x=%d\r\n",g_motosPara.motosPos[MTX_IDX]);
		}
	}
	else // if (dirx == 0)
	{
		rslt = ERR_NONE;
	}

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

#if (MODULE_Y != 0)
	// Y限位
	if (diry > 0)
	{
		if (MoveYLimitPosi() == SENSOR_ON)		// Y正向限位传感器
		{
			rslt = ERR_Y_LIT_POSI;			// Y正向限位
			printf("y=%d\r\n",g_motosPara.motosPos[MTY_IDX]);
		}
	}
	else if (diry < 0)
	{
		if (MoveYLimitNega() == SENSOR_ON)		// Y反向限位传感器
		{
			rslt = ERR_Y_LIT_NEGA;			// Y反向限位
			printf("y=%d\r\n",g_motosPara.motosPos[MTY_IDX]);
		}
	}
	else // if (diry == 0)
	{
		rslt = ERR_NONE;
	}

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

	if (dirr1 > 0)
	{

	}
	else if (dirr1 < 0)
	{

	}
	else // if (dirr1 == 0)
	{
		rslt = ERR_NONE;
	}

	return rslt;
}


// 快速停止检测
int MoveXYRQStopScan(u32 para1, u32 para2)
{
	int rslt;
	rslt = GetCommonQStop();

	if (rslt == ERR_NONE)
	{

	}

	return rslt;
}

// 普通停止检测
int MoveXYRNStopScan(u32 para1, u32 para2)
{
	int rslt;

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

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#if (MODULE_LOOSE == 1)			// 松料装置
	rslt = SendOpticalTask();			// 自动送光纤任务
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#endif


	return rslt;
}


int MoveXYR1S1P1AsOffset( s32 ofstx, s32 ofsty, s32 ofstr1,s32 ofsts1,s32 ofstp1,  MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 xIdx, yIdx, r1Idx, s1Idx, p1Idx;
	MotoSpd spd;
	s8 dirx, diry, dirr1,dirs1,dirp1;
	u32 interlong;

#if (XY_GAP_CONVERT == 1)
	ofstx = XGapConvert(ofstx);	// 间隙补偿转换
	ofsty = YGapConvert(ofsty);	// 间隙补偿转换
#endif

	xIdx = MTX_IDX;
	yIdx = MTY_IDX;
	r1Idx = MTR1_IDX;
	s1Idx = MTS1_IDX;
	p1Idx = MTP1_IDX;

	if (ofstx == 0 && ofsty == 0 && ofstr1 == 0 && ofsts1 == 0 && ofstp1 == 0)
	{
		return ERR_NONE;
	}

	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);
		pSpd = &spd;
	}



	if (ofstx < 0)
	{
		dirx = -1;
	}
	else if (ofstx > 0)
	{
		dirx = 1;
	}
	else
	{
		dirx = 0;
	}

	if (ofsty < 0)
	{
		diry = -1;
	}
	else if (ofsty > 0)
	{
		diry = 1;
	}
	else
	{
		diry = 0;
	}

	if (ofstr1 < 0)
	{
		dirr1 = -1;
	}
	else if (ofstr1 > 0)
	{
		dirr1 = 1;
	}
	else
	{
		dirr1 = 0;
	}

	if (ofsts1 < 0)
	{
		dirs1 = -1;
	}
	else if (ofsts1 > 0)
	{
		dirs1 = 1;
	}
	else
	{
		dirs1 = 0;
	}

	if (ofstp1 < 0)
	{
		dirp1 = -1;
	}
	else if (ofstp1 > 0)
	{
		dirp1 = 1;
	}
	else
	{
		dirp1 = 0;
	}

	interlong = GetSyntheticLength(ofstx, ofsty);
	interlong = abs(ofstr1) > interlong ? abs(ofstr1) : interlong;
	interlong = abs(MTS1_IsWire001PulseToS1Pulse(ofsts1)) > interlong ? abs(MTS1_IsWire001PulseToS1Pulse(ofsts1)) : interlong;


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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1
	lmctrl.motosConfig.newConfig = 0;

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

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

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

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

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

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = interlong;
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;			// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;		// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;		// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 = lmctrl.vAxisId;			// 工作参数
	lmctrl.GetEmergencyStop = MoveXYREmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveXYRQStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveXYRNStopScan;	// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	//lmctrl.GetSlowDown = NULL;				// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = (((u32)dirp1 & 0xff) |((u32)dirs1 & 0xff)  | (((u32)dirr1 & 0xff) << 16) | (((u32)diry & 0xff) << 8) | ((u32)dirx & 0xff));
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

	err = GetMoveResultError(rslt, lmctrl.errInfo);

#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif

	return err;
}
// U1S1轴偏移移动
int MoveU1S1AsOffset(s32 ofstu, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 u1idx,s1idx;
	u1idx = MTU1_IDX;
	s1idx = MTS1_IDX;
	MotoSpd spd;

	if (pSpd == NULL)
	{

		spd.stspSpd = UAxisMMPSToPPS(g_pOpticalMcPara->ustspSpd);
		spd.runSpd = UAxisMMPSToPPS(g_pOpticalMcPara->umoveSpd);
		spd.spdAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uliftAdd);
		spd.brkAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uBrkAdd);
		pSpd = &spd;
	}
//	printf("MoveU1AsOffset, ofstu=%d, spd=%d\r\n", ofstu, pSpd->runSpd);
//	printf("before move, motospos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[u1idx].axisConfig = MTU1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[u1idx].poutType = MTU1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[u1idx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[u1idx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[u1idx] = ofstu;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

	lmctrl.motosConfig.newConfig = 0;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[s1idx].axisConfig = MTS1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[s1idx].poutType = MTS1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[s1idx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[s1idx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[s1idx] = ofstu*1.2;

	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	lmctrl.mvmtPara.interLong = abs(ofstu*1.2);							// 插补长轴位移
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;			// 每段脉冲数

	// 速度控制参数
//	if(pSpd == NULL)
//	{
		lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
		lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
		lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
		lmctrl.spdPara.slowPPS = pSpd->stspSpd;		// 降速速度
		lmctrl.spdPara.addPPSG = pSpd->spdAdd;		// 升速加速度
		lmctrl.spdPara.decPPSG = pSpd->spdAdd;		// 降速加速度
		lmctrl.spdPara.brkPPSG = pSpd->brkAdd;		// 刹车加速度
//	}
//	else
//	{
//		lmctrl.spdPara.startPPS = UAxisMMPSToPPS(g_pOpticalMcPara->ustspSpd);	// 启动速度
//		lmctrl.spdPara.stopPPS = UAxisMMPSToPPS(g_pOpticalMcPara->ustspSpd);		// 停止速度
//		lmctrl.spdPara.runPPS = UAxisMMPSToPPS(g_pOpticalMcPara->umoveSpd);		// 运行速度
//		lmctrl.spdPara.slowPPS = UAxisMMPSToPPS(g_pOpticalMcPara->ustspSpd);		// 降速速度
//		lmctrl.spdPara.addPPSG = UAxisMMPSSToPPSG(g_pOpticalMcPara->uliftAdd);		// 升速加速度
//		lmctrl.spdPara.decPPSG = UAxisMMPSSToPPSG(g_pOpticalMcPara->uliftAdd);		// 降速加速度
//		lmctrl.spdPara.brkPPSG = UAxisMMPSSToPPSG(g_pOpticalMcPara->uBrkAdd);		// 刹车加速度
//
//	}

	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.GetEmergencyStop = MoveU1EmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveU1QStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveU1NStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
//	lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = 0;
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

//	printf("after move, motospos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);

	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}
// p1S1轴偏移移动
int MoveP1S1AsOffset(s32 ofstu, MotoSpd * pSpd)
{

	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 p1idx,s1idx;
	p1idx = MTP1_IDX;
	s1idx = MTS1_IDX;
	MotoSpd spd;
	s32 ofsp1,ofss1;
	if (ofstu == 0)
	{
		return ERR_NONE;
	}
	if (pSpd == NULL)
	{
		spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->lmvstspSpd);
		spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->lmvlSpd);
		spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvSAdd);
		spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->lmvbrkAdd);
		pSpd = &spd;
	}
//	printf("MoveU1AsOffset, ofstu=%d, spd=%d\r\n", ofstu, pSpd->runSpd);
//	printf("before move, motospos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);


	ofsp1 = toActSpinLen(abs(MTP_1PulseToP1Pulse(ofstu)));
	ofss1 = toActSpinLen(abs(MTS1_IsSpin1PulseToS1Pulse(ofstu))*g_pOpticalMcPara->LevelOfAccAndDec/100);

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

	lmctrl.motosConfig.newConfig = 0;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[p1idx].axisConfig = MTP1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[p1idx].poutType = MTP1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[p1idx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[p1idx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[p1idx] = ofsp1;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p

	lmctrl.motosConfig.newConfig = 0;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[s1idx].axisConfig = MTS1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[s1idx].poutType = MTS1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[s1idx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[s1idx].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[s1idx] = ofss1;

	lmctrl.mvmtPara.interLong = ofss1 > ofsp1 ? ofss1 : ofsp1;
	lmctrl.mvmtPara.extraRepeat = 0;								// 延续重复次数
	//lmctrl.mvmtPara.interLong = abs(ofstu*11.781);				// 插补长轴位移
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;			// 每段脉冲数

	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;		// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd;		// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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.GetEmergencyStop = MoveU1EmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveU1QStopScan;		// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveS1P1NStopScan;		// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
//	lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = 0;
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

//	printf("after move, motospos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);

	err = GetMoveResultError(rslt, lmctrl.errInfo);

	return err;
}
int MoveXYR1S1P1ToPos(s32 posx, s32 posy, s32 posr1,s32 poss1,s32 posp1,MotoSpd * pSpd, int s1MotoRunFlag)
{
	int err;
	s32 ofstx, ofsty, ofstr1, ofsts1, ofstp1, xNegaBorder, yNegaBorder, xPosiBorder, yPosiBorder;
	s32 abposr1;

	g_workCtrl.sendStaWhenRun = 0;
	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

	err = ERR_NONE;
	ReportError(err);

	xNegaBorder = g_pOpticalMcPara->xMovNegaBorder;
	yNegaBorder = g_pOpticalMcPara->yMovNegaBorder;
	xPosiBorder = g_pOpticalMcPara->xMovPosiBorder;
	yPosiBorder = g_pOpticalMcPara->yMovPosiBorder;

//	if(g_pOpticalMcPara->offsetX > 0)
//	{
//		xNegaBorder -= g_pOpticalMcPara->offsetX;
//	}
//	else
//	{
//		xPosiBorder -= g_pOpticalMcPara->offsetX;
//	}
//
//	if(g_pOpticalMcPara->offsetY > 0)
//	{
//		yNegaBorder -= g_pOpticalMcPara->offsetY;
//	}
//	else
//	{
//		yPosiBorder -= g_pOpticalMcPara->offsetY;
//	}

	if (g_motosPara.enFlag == DATA_VALID &&
		(g_workCtrl.posEnFlag & (MTX_MOD|MTY_MOD)) == (MTX_MOD|MTY_MOD) &&
		(g_workCtrl.posEnFlag & MTR1_MOD) == MTR1_MOD &&
		1	)
	{
		// 目标位置在区域内
		if (posx < xNegaBorder ||//g_pOpticalMcPara->xMovNegaBorder || 	// 工作区X负边界，单位0.01mm
			posx > xPosiBorder ||//g_pOpticalMcPara->xMovPosiBorder || 	// 工作区X正边界，单位0.01mm
			posy < yNegaBorder ||//g_pOpticalMcPara->yMovNegaBorder ||	// 工作区Y负边界，单位0.01mm
			posy > yPosiBorder ||//g_pOpticalMcPara->yMovPosiBorder || 	// 工作区Y正边界，单位0.01mm
			0	)
		{
			printf("\r\ntarget position out of range, x=%d, y=%d\r\n", posx, posy);
			err = ERR_OUT_RANGE;		// 目标位置越界
		}
		else
		{
			ReadAndFreshMotosPos();

			ofstx = posx - g_motosPara.motosPos[MTX_IDX];
			ofsty = posy - g_motosPara.motosPos[MTY_IDX];

			if(s1MotoRunFlag == 1)
			{

				ofsts1 = sqrt((ofstx/10)*(ofstx/10) + (ofsty/10)*(ofsty/10))*g_pOpticalMcPara->LevelOfAccAndDec/100;
				ofsts1 *= 10;
			}
			else
			{
				ofsts1 = 0;
				ofstp1 = 0;//posp1 - g_motosPara.motosPos[MTP1_IDX];
			}


			// 得到一圈内的相对位置
			abposr1 = g_motosPara.motosPos[MTR1_IDX];
			ofstr1 = posr1 - abposr1;

			// 转化为 -180--180度之间的数
			ChangeRotToNP180(&ofstr1);


			if (ofstx != 0 || ofsty != 0 || ofstr1 != 0)
			{
/*
				printf("\r\n before xyr1r2 move\r\n");
				printf("actpos: x=%d, y=%d, r1=%d, r1=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX], g_motosPara.motosPos[MTR1_IDX], g_motosPara.motosPos[MTR2_IDX]);
				printf("\r\n move ofstx = %d, ofsty = %d, ofstr1 = %d, ofstr2 = %d\r\n", ofstx, ofsty, ofstr1, ofstr2);
*/
				err = MoveXYR1S1P1AsOffset(ofstx, ofsty, ofstr1, ofsts1, ofstp1, pSpd);
/*
				printf("\r\n after xyr1move\r\n");
				printf("actpos: x=%d, y=%d, r1=%d, r1=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX], g_motosPara.motosPos[MTR1_IDX], g_motosPara.motosPos[MTR2_IDX]);
*/
			}
		}
	}
	else
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	g_workCtrl.sendStaWhenRun = 1;

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	ReportError(err);
	return err;
}
#endif

s32 PitkToRpp(s32 dr)
{
	double tdr;
	tdr = dr;
	return PITK_TO_RPP(tdr);
}

int MoveXYRS1P1ToWorkPos(DataItem * pCurDat, MotoSpd *pSpd, int rToSpinPosFlag,int s1MotoRunFlag)
{
	int err;
	int posx, posy, posr1, poss1, posp1;
	err = ERR_NONE;
	printf("XYRS1P1MoveToPos\r\n");
	printf("idx = %d,go to x= %d, go to y = %d, \r\n",g_workCtrl.cursta.dataIdx,g_workCtrl.workPos[MTX_IDX],g_workCtrl.workPos[MTY_IDX]);

	if (err == ERR_NONE)
	{
		g_optWorkCtrl.IsNoMaterialEn = 0;

		ReadAndFreshMotosPos();

		posx = g_workCtrl.workPos[MTX_IDX];
		posy = g_workCtrl.workPos[MTY_IDX];
		if(rToSpinPosFlag == 1)
		{
			posr1 = 2500;//90°
		}
		else
		{
			posr1 = PitkToRpp(g_workCtrl.workPos[MTR1_IDX]);
		}
		poss1 = g_workCtrl.workPos[MTS1_IDX];
		posp1 = g_workCtrl.workPos[MTP1_IDX];

		if(g_optWorkCtrl.offsetToOpit == 1)
		{
			s1MotoRunFlag =1;
		}

//		if (uh != 0)
//		{
//			if (posx != g_motosPara.motosPos[MTX_IDX] ||
//				posy != g_motosPara.motosPos[MTY_IDX] ||
//				posr1 != g_motosPara.motosPos[MTR1_IDX] ||	// 有位移
//				poss1 != g_motosPara.motosPos[MTS1_IDX] ||	// 有位移
//				posp1 != g_motosPara.motosPos[MTP1_IDX] ||	// 有位移
//				0	)
//			{
//                // 机头整体上升(气缸)
//			}
//		}



		MotoSpd spd;
		if (pSpd == NULL)
		{
			spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
			spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvlSpd);
			spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
			spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);
		}
		else
		{
			memcpy(&spd, pSpd, sizeof(MotoSpd));		// 速度
		}

		err = MoveXYR1S1P1ToPos(posx, posy, posr1, poss1, posp1, &spd, s1MotoRunFlag);

		g_optWorkCtrl.IsNoMaterialEn = 1;
	}
	return err;
}

#endif

int MoveXYRS1P1SToWorkPos(DataItem * pCurDat, int uh)       //运行中到正确位置,上夹料气缸无夹紧(若加紧压料时上加夹料会出现反复夹紧导致断纤)
{
	int err;
	int posx, posy, posr1, poss1, posp1;
	err = ERR_NONE;

	if (err == ERR_NONE)
	{
		ReadAndFreshMotosPos();

		posx = g_workCtrl.workPos[MTX_IDX];
		posy = g_workCtrl.workPos[MTY_IDX];
		posr1 = PitkToRpp(g_workCtrl.workPos[MTR1_IDX]);
		poss1 = g_workCtrl.workPos[MTS1_IDX];
		posp1 = g_workCtrl.workPos[MTP1_IDX];

		g_optWorkCtrl.IsNoMaterialEn = 0;

		if (uh != 0)
		{
			if (posx != g_motosPara.motosPos[MTX_IDX] ||
				posy != g_motosPara.motosPos[MTY_IDX] ||
				posr1 != g_motosPara.motosPos[MTR1_IDX] ||	// 有位移
				poss1 != g_motosPara.motosPos[MTS1_IDX] ||	// 有位移
				posp1 != g_motosPara.motosPos[MTP1_IDX] ||	// 有位移
				0	)
			{
				//// 机头整体上升(气缸)

			}
		}

		if (err == ERR_NONE)
		{

		//	UpClipMatClipCylinderOn();   // 上夹料 开:上夹料气缸在剪线前感应,锁住光纤防止回退.
			err = MoveXYR1S1P1ToPos(posx, posy, posr1, poss1, posp1, NULL, 0);
		}

		g_optWorkCtrl.IsNoMaterialEn = 1;
	}
	return err;
}

//-------------------------------------------------------------
//-------------------------------------------------------------------------------
// 急停检测
int MoveXYU1R1EmcScan(u32 para1, u32 para2)
{
	int rslt;
	s8 dirx, diry,diru, dirr;

	dirr = (s8)(para1);
	diru = (s8)(para1>>8);
	diry = (s8)(para1>>16);
	dirx = (s8)(para1>>24);

	dirr = dirr;

	rslt = GetCommonEms();

	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#if (MODULE_X != 0)
	// X限位
	if (dirx > 0)
	{
		if (MoveXLimitPosi() == SENSOR_ON)		// X 正向限位传感器
		{
			rslt = ERR_X_LIT_POSI;	//
		}
	}
	else if (dirx < 0)
	{
		if (MoveXLimitNega() == SENSOR_ON)		// X 反向限位传感器
		{
			rslt = ERR_X_LIT_NEGA;	//
		}
	}
	else // if (dirx == 0)
	{
		rslt = ERR_NONE;
	}

	if (rslt == ERR_NONE)
	{
	}

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

#if (MODULE_Y != 0)
	// Y限位
	if (diry > 0)
	{
		if (MoveYLimitPosi() == SENSOR_ON)		// Y 正向限位传感器
		{
			rslt = ERR_Y_LIT_POSI;			// Y 正向限位
		}
	}
	else if (diry < 0)
	{
		if (MoveYLimitNega() == SENSOR_ON)		// Y 反向限位传感器
		{
			rslt = ERR_Y_LIT_NEGA;			// Y 反向限位
		}
	}
	else // if (diry == 0)
	{
		rslt = ERR_NONE;
	}

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


#if(LIFT_U1 != 0)
	// U1限位
	if (diru > 0)
	{
		if (MoveU1BHLimitPosi(0) == SENSOR_ON)	// U1 正向限位传感器
		{
			rslt = ERR_LMT_POSITIVE;			// 正向限位
		}
	}
	else if (diru < 0)
	{
		if (MoveU1BHLimitNega(0) == SENSOR_ON)	// U1 反向限位传感器
		{
			rslt = ERR_LMT_NEGATIVE;			// 反向限位
		}
	}
	else // if (diru == 0)
	{
		rslt = ERR_NONE;
	}

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


// 快速停止检测
int MoveXYU1R1QStopScan(u32 para1, u32 para2)
{
	int rslt;
	rslt = GetCommonQStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}

	if (rslt == ERR_NONE)
	{

	}

	return rslt;
}


// 普通停止检测
int MoveXYU1R1NStopScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonNStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#if (MODULE_LOOSE == 1)			// 松料装置
	rslt = SendOpticalTask();			// 自动送光纤任务
	if (rslt != ERR_NONE)
	{
		return rslt;
	}
#endif
	return rslt;
}

// X Y U1 R1 移动偏移值

int MoveXYU1R1AsOffset(s32 ofstx, s32 ofsty, s32 ofstu1, s32 ofstr1, MotoSpd * pSpd)
{
	int rslt, err;
	InterMoveCtrl lmctrl;
	u16 xIdx, yIdx, uIdx, rIdx;
	s16 dirx, diry, diru, dirr;
	MotoSpd spd, spdx, spdu, spdr;
	u32 interlong;

#if (XY_GAP_CONVERT == 1)
	ofstx = XGapConvert(ofstx);	// 间隙补偿转换
	ofsty = YGapConvert(ofsty);	// 间隙补偿转换
#endif

#if (MODULE_X != 0)
	if (ofstx == 0 && ofsty == 0 && ofstu1 == 0 && ofstr1 == 0)
	{
		return ERR_NONE;
	}

	interlong = abs(ofstx);
#endif

#if (MODULE_Y != 0)
	if (interlong < abs(ofsty))
	{
		interlong = abs(ofsty);
	}
#endif

#if(LIFT_U1 != 0)
	if (interlong < abs(ofstu1))
	{
		interlong = abs(ofstu1);
	}
#endif

#if(TURN_R1 != 0)
	if (interlong < abs(ofstr1))
	{
		interlong = abs(ofstr1);
	}
#endif

	// 速度
	if (pSpd == NULL)
	{
		spd.stspSpd = 0xffffffff;
		spd.runSpd = 0xffffffff;
		spd.spdAdd = 0xffffffff;
		spd.brkAdd = 0xffffffff;

		spdx.stspSpd = 0xffffffff;
		spdx.runSpd = 0xffffffff;
		spdx.spdAdd = 0xffffffff;
		spdx.brkAdd = 0xffffffff;

		spdu.stspSpd = 0xffffffff;
		spdu.runSpd = 0xffffffff;
		spdu.spdAdd = 0xffffffff;
		spdu.brkAdd = 0xffffffff;

		spdr.stspSpd = 0xffffffff;
		spdr.runSpd = 0xffffffff;
		spdr.spdAdd = 0xffffffff;
		spdr.brkAdd = 0xffffffff;

#if ((MODULE_X != 0) ||  (MODULE_Y != 0))
		if ((ofstx != 0) || (ofsty != 0))
		{
			spdx.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
			spdx.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvlSpd);
			spdx.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
			spdx.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);
		}
#endif

#if(LIFT_U1 != 0)
		if (ofstu1 != 0)
		{
			spdu.stspSpd = UAxisMMPSToPPS(g_pOpticalMcPara->ustspSpd);
			spdu.runSpd = UAxisMMPSToPPS(g_pOpticalMcPara->umoveSpd);
			spdu.spdAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uliftAdd);
			spdu.brkAdd = UAxisMMPSSToPPSG(g_pOpticalMcPara->uBrkAdd);
		}
#endif

#if(TURN_R1 != 0)
		if (ofstr1 != 0)
		{
			spdr.stspSpd = DPS_TO_PPS(g_pOpticalMcPara->rotstspDps);	// 启停速度
			spdr.runSpd = DPS_TO_PPS(g_pOpticalMcPara->rotrunDps);		// 运转转速
			spdr.spdAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotAddDpss);	// 加速度
			spdr.brkAdd = DPSS_TO_PPSG(g_pOpticalMcPara->rotBrkDpss);	// 刹车加速度
		}
#endif

		if (spd.stspSpd > spdx.stspSpd)
		{
			spd.stspSpd = spdx.stspSpd;
		}
		if (spd.stspSpd > spdu.stspSpd)
		{
			spd.stspSpd = spdu.stspSpd;
		}
		if (spd.stspSpd > spdr.stspSpd)
		{
			spd.stspSpd = spdr.stspSpd;
		}

		if (spd.runSpd > spdx.runSpd)
		{
			spd.runSpd = spdx.runSpd;
		}
		if (spd.runSpd > spdu.runSpd)
		{
			spd.runSpd = spdu.runSpd;
		}
		if (spd.runSpd > spdr.runSpd)
		{
			spd.runSpd = spdr.runSpd;
		}

		if (spd.spdAdd > spdx.spdAdd)
		{
			spd.spdAdd = spdx.spdAdd;
		}
		if (spd.spdAdd > spdu.spdAdd)
		{
			spd.spdAdd = spdu.spdAdd;
		}
		if (spd.spdAdd > spdr.spdAdd)
		{
			spd.spdAdd = spdr.spdAdd;
		}

		if (spd.brkAdd > spdx.brkAdd)
		{
			spd.brkAdd = spdx.brkAdd;
		}
		if (spd.brkAdd > spdu.brkAdd)
		{
			spd.brkAdd = spdu.brkAdd;
		}
		if (spd.brkAdd > spdr.brkAdd)
		{
			spd.brkAdd = spdr.brkAdd;
		}

		pSpd = &spd;
	}
	//printf("MoveXYU1R1AsOffset, ofstx=%d, ofsty=%d, ofstu1=%d, ofstr1=%d, runspd\r\n",ofstx, ofsty, ofstu1, ofstr1, pSpd->runSpd);

#if (MODULE_X != 0)
	// 方向
	if (ofstx < 0)
	{
		dirx = -1;
	}
	else if (ofstx > 0)
	{
		dirx = 1;
	}
	else
	{
		dirx = 0;
	}
#endif

#if (MODULE_Y != 0)
	if (ofsty < 0)
	{
		diry = -1;
	}
	else if (ofsty > 0)
	{
		diry = 1;
	}
	else
	{
		diry = 0;
	}
#endif

#if(LIFT_U1 != 0)
	if (ofstu1 < 0)
	{
		diru = -1;
	}
	else if (ofstu1 > 0)
	{
		diru = 1;
	}
	else
	{
		diru = 0;
	}
#endif

#if(TURN_R1 != 0)
	if (ofstr1 < 0)
	{
		dirr = -1;
	}
	else if (ofstr1 > 0)
	{
		dirr = 1;
	}
	else
	{
		dirr = 0;
	}
#endif

#if (MODULE_X != 0)
	xIdx = MTX_IDX;
#endif

#if (MODULE_Y != 0)
	yIdx = MTY_IDX;
#endif

#if(LIFT_U1 != 0)
	uIdx = MTU1_IDX;
#endif

#if(TURN_R1 != 0)
	rIdx = MTR1_IDX;
#endif

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

	lmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1

#if (MODULE_X != 0)
	lmctrl.motosConfig.newConfig = 1;												// 需要更新电机配置
	lmctrl.motosConfig.axisConfig[xIdx].axisConfig = MTX_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[xIdx].poutType = MTX_OTP;							// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[xIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[xIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[xIdx] = ofstx;											// 各个轴的运动量，0，无运动；其他，运动长度，单位p
#endif

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

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

#if(TURN_R1 != 0)
	lmctrl.motosConfig.axisConfig[rIdx].axisConfig = MTR1_CFG;						// 实轴输出配置
	lmctrl.motosConfig.axisConfig[rIdx].poutType = MTR1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	lmctrl.motosConfig.axisConfig[rIdx].spdSource = MAKE_POUT_CMD(lmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	lmctrl.motosConfig.axisConfig[rIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
	lmctrl.mvmtPara.movement[rIdx] = ofstr1;										// 各个轴的运动量，0，无运动；其他，运动长度，单位p
 #endif
	lmctrl.mvmtPara.extraRepeat = 0;												// 延续重复次数
	lmctrl.mvmtPara.interLong = interlong;											// 插补长轴位移
	lmctrl.mvmtPara.pulsePerSegment = PULSE_PER_SEGMENT;							// 每段脉冲数

	// 速度控制参数
	lmctrl.spdPara.startPPS = pSpd->stspSpd;	// 启动速度
	lmctrl.spdPara.stopPPS = pSpd->stspSpd;		// 停止速度
	lmctrl.spdPara.runPPS = pSpd->runSpd;		// 运行速度
	lmctrl.spdPara.slowPPS = pSpd->stspSpd;		// 降速速度
	lmctrl.spdPara.addPPSG = pSpd->spdAdd;		// 升速加速度
	lmctrl.spdPara.decPPSG = pSpd->spdAdd/2;	// 降速加速度
	lmctrl.spdPara.brkPPSG = pSpd->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 = lmctrl.vAxisId;			// 工作参数
	lmctrl.GetEmergencyStop = MoveXYU1R1EmcScan;	// 紧急停止条件函数，当为空时，不检测急停条件; 不为空时，条件满足，立刻停止发送脉冲（无需降速）
	lmctrl.GetQuickStop = MoveXYU1R1QStopScan;	// 快速停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按刹车加速度）
	lmctrl.GetNormalStop = MoveXYU1R1NStopScan;	// 一般停止条件函数，当为空时，不检测停止条件; 不为空时，条件满足，降速到停止（按降速加速度）
	//lmctrl.GetSlowDown = NULL;					// 慢速运动条件函数，当为空时，不检测降速条件; 不为空时，条件满足，速度降低，条件消失，速度恢复
	lmctrl.condPara1 = (((u32)dirx & 0xff) << 24 | ((u32)diry & 0xff) << 16 | ((u32)diru & 0xff) << 8 | ((u32)dirr & 0xff));
	lmctrl.condPara2 = lmctrl.vAxisId;			// 条件函数参数

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

	err = GetMoveResultError(rslt, lmctrl.errInfo);

#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif

	return err;
}

int MoveXYU1R1ToPos(s32 posx, s32 posy, s32 posu1, s32 posr1, MotoSpd * pSpd)
{
	int err;
	s32 ofstx, ofsty, ofstu, ofstr;

	err = ERR_NONE;

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志

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

	if (err == ERR_NONE)
	{
		if ((g_motosPara.enFlag == DATA_VALID) &&
			(g_workCtrl.posEnFlag & (MTX_MOD|MTY_MOD)) == (MTX_MOD|MTY_MOD) &&
			(g_workCtrl.posEnFlag & (MTU1_MOD)) == (MTU1_MOD) &&
			(g_workCtrl.posEnFlag & (MTR1_MOD)) == (MTR1_MOD) &&
			1	)
		{
			ReadAndFreshMotosPos();

#if (MODULE_X != 0)
			ofstx = posx - g_motosPara.motosPos[MTX_IDX];
#endif

#if (MODULE_Y != 0)
			ofsty = posy - g_motosPara.motosPos[MTY_IDX];
#endif

#if(LIFT_U1 != 0)
			ofstu = posu1 - g_motosPara.motosPos[MTU1_IDX];
#endif

#if(TURN_R1 != 0)
			ofstr = posr1 - g_motosPara.motosPos[MTR1_IDX];
#endif

			if (ofstx != 0 || ofsty != 0 || ofstu != 0 || ofstr != 0)
			{
				if (err == ERR_NONE)
				{
					err = MoveXYU1R1AsOffset(ofstx, ofsty, ofstu, ofstr, pSpd);
				}
			}
		}
		else
		{
			err = ERR_COOR_SYSTM;		// 坐标系统错误
		}
	}

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	ReportError(err);
	return err;
}


//风机
void FanMotosOn(void)
{
	Fan1MotoOn();			// 风机1启动

	if (g_optWorkCtrl.fanStatus == 0)
	{
		g_optWorkCtrl.fanCloseCounter = g_pOpticalMcPara->fanCloseDelay;	// 重置计数器
		printf("3. set FanMotosOn fanCloseCounter = %d\r\n",g_optWorkCtrl.fanCloseCounter);
	}
	g_optWorkCtrl.fanStatus = 1;	// 启动
}

void FanMotosOff(void)
{
//	printf("fans off\r\n");
	Fan1MotoOff();		// 风机1停止
}

// 判断
int IsAutoMoveEnable(void)
{
	int err;

	err = ERR_NONE;
	// 检测是否允许工作(时间, 状态)
	/*if (err == ERR_NONE &&
		(g_workCtrl.cursta.workStatus & WORK_STA_ENTIME) == 0)		// 允许时限
	{
		err = ERR_EXPIRATION;	// 使用时限已到
	}*/

	if (err == ERR_NONE &&
		(g_workCtrl.cursta.workStatus & WORK_STA_ENSTATE) == 0)	// 允许状态
	{
		err = ERR_NOT_ALLOWED;	// 不允许工作状态
	}

	// 条件
	if (err == ERR_NONE)
	{

	}

	// 参数是否有效
	if (err == ERR_NONE && g_workCtrl.paraEnFlag != PARA_VALID)	// 参数无效
	{
		err = ERR_MC_PARA;		// 参数错误
	}

	// 坐标系统是否有效
	if (err == ERR_NONE &&
			(	(g_motosPara.enFlag != DATA_VALID) ||				// 坐标系统无效
				((g_workCtrl.posEnFlag & MTX_MOD) != MTX_MOD) ||	// X坐标系统无效
				((g_workCtrl.posEnFlag & MTY_MOD) != MTY_MOD) ||	// Y坐标系统无效
			    ((g_workCtrl.posEnFlag & MTU1_MOD) != MTU1_MOD) ||	// U1坐标系统无效
			    ((g_workCtrl.posEnFlag & MTR1_MOD) != MTR1_MOD) ||	// R1坐标系统无效
					0	)	&&
			1	)
	{
		err = ERR_COOR_SYSTM;		// 坐标系统错误
	}

	return err;
}

//------------------------------------------------文件执行------------------------------------------------
#define	EXDIR_POSI	1		// 正向执行
#define	EXDIR_NEGA	-1		// 反向执行

// 通过针迹索引得到工作坐标
int GetWorkPosFromIndex(void)
{
	int rslt;
	u32 idx;
	DataItem sewStep;

	g_workCtrl.workAvaible = 0;		// 工作坐标有效标志

	g_workCtrl.workPos[MTX_IDX] = 0;
	g_workCtrl.workPos[MTY_IDX] = 0;
	g_workCtrl.workPos[MTR1_IDX] = 0;
	g_workCtrl.workPos[MTS1_IDX] = 0;
	g_workCtrl.workPos[MTP1_IDX] = 0;

	if (IsDatFileAvailable() == TRUE)
	{
		g_workCtrl.workPos[MTX_IDX] = g_datFile.pFileHead->fileHead.beginX - g_pOpticalMcPara->offsetX;		// 当前工作坐标 等于 起点坐标
		g_workCtrl.workPos[MTY_IDX] = g_datFile.pFileHead->fileHead.beginY - g_pOpticalMcPara->offsetY;		// 当前工作坐标 等于 起点坐标
		g_workCtrl.workPos[MTR1_IDX] = g_datFile.pFileHead->fileHead.beginR;		// 当前工作坐标 等于 起点坐标

	}
	else
	{
		return -1;
	}

	if (IsDatIndexAvailable(g_workCtrl.cursta.dataIdx) != TRUE)
	{
		g_workCtrl.cursta.dataIdx = 0;
		g_workCtrl.savedpara.dataIdx = g_workCtrl.cursta.dataIdx;
		SaveWorkNvPara();
	}

	for (idx = 0; idx < g_workCtrl.cursta.dataIdx; idx++)
	{
		rslt = GetADataItem(0, idx, &sewStep);
		if (rslt != 0)
		{
			break;		// 结束
		}
		ChangeWorkPos(&sewStep, 1);
	}
	g_workCtrl.workAvaible = DATA_VALID;		// 工作坐标有效标志

	return 0;
}


void ChangeWorkPos(DataItem * pDatItem, int dir)
{
	if (pDatItem == NULL || (dir != 1 && dir != -1))
	{
		return;
	}

	if (0	||
		pDatItem->ctrl == DATA_OFFSET	||
		pDatItem->ctrl == DATA_MILLING	||
		pDatItem->ctrl == DATA_DRAWING 	||
//		pDatItem->ctrl == DATA_PRESSURE ||
		pDatItem->ctrl == DATA_RKNIFE 	||
		0	)
	{
		g_workCtrl.workPos[MTX_IDX] += pDatItem->dx*dir;
		g_workCtrl.workPos[MTY_IDX] += pDatItem->dy*dir;

		g_workCtrl.workPos[MTR1_IDX] += pDatItem->dr*dir;

		g_workCtrl.workPos[MTS1_IDX] += pDatItem->dx*dir;
		g_workCtrl.workPos[MTP1_IDX] += pDatItem->dy*dir;

	}
	else
	{

	}
}

int CheckFileRange(void)
{
	int err = ERR_NONE;

	// 文件是否有效
	if (err == ERR_NONE &&
		IsDatFileAvailable() != TRUE)	// 文件有效
	{
		err = ERR_NO_DATA;	// 无数据文件
	}

	// 检查数据是否正确
	if (err == ERR_NONE)
	{
		if (
			g_datFile.pFileHead->fileHead.maxX   < g_datFile.pFileHead->fileHead.minX ||
			g_datFile.pFileHead->fileHead.maxY   < g_datFile.pFileHead->fileHead.minY ||	// 轮廓不正确
			g_datFile.pFileHead->fileHead.beginX < g_datFile.pFileHead->fileHead.minX ||
			g_datFile.pFileHead->fileHead.beginY < g_datFile.pFileHead->fileHead.minY ||
			g_datFile.pFileHead->fileHead.beginX > g_datFile.pFileHead->fileHead.maxX ||
			g_datFile.pFileHead->fileHead.beginY > g_datFile.pFileHead->fileHead.maxY ||	// 起点不在轮廓范围内
																	// 检查每个数据点是否在轮廓范围内
			0	)
		{
			err = ERR_DATA_ERROR;		// 数据错误
		}
	}

	// 检查图形是否支持越界
	if (err == ERR_NONE)
	{
		if (g_datFile.pFileHead->fileHead.minX < g_pOpticalMcPara->xOptNegaBorder ||	// 左边界
			g_datFile.pFileHead->fileHead.maxX > g_pOpticalMcPara->xOptPosiBorder ||
			g_datFile.pFileHead->fileHead.minY < g_pOpticalMcPara->yOptNegaBorder ||
			g_datFile.pFileHead->fileHead.maxY > g_pOpticalMcPara->yOptPosiBorder ||
			0	)
		{
			printf("graph range, left=%d, right=%d, top=%d, bottom=%d\r\n",
				g_datFile.pFileHead->fileHead.minX, g_datFile.pFileHead->fileHead.maxX, g_datFile.pFileHead->fileHead.maxY, g_datFile.pFileHead->fileHead.minY);
			printf("para set range, left=%d, right=%d, top=%d, bottom=%d\r\n",
				g_pOpticalMcPara->xOptNegaBorder,  g_pOpticalMcPara->xOptPosiBorder, g_pOpticalMcPara->yOptPosiBorder, g_pOpticalMcPara->yOptNegaBorder);
			err = ERR_GRAPH_OUT_RANGE;	// 图形超出范围
		}
	}

	return err;
}

void AddNewFileOffset(void)
{
	if (IsNewDatFile(0) == TRUE)		// 新文件
	{
		DataFileHead newpara;
		memcpy(newpara.headparts.fileparas, g_datFile.pFileHead->headparts.fileparas, sizeof(newpara.headparts.fileparas));

		printf("AddNewFileOffset\r\n");
		printf("before add, file info:\r\n");
		OutFileInfo();
//		newpara.fileHead.anchorX += g_pOpticalMcPara->datbegOffsetX;
//		newpara.fileHead.anchorY += g_pOpticalMcPara->datbegOffsetY;
//		newpara.fileHead.beginX  += g_pOpticalMcPara->datbegOffsetX;
//		newpara.fileHead.beginY  += g_pOpticalMcPara->datbegOffsetY;
//		newpara.fileHead.minX    += g_pOpticalMcPara->datbegOffsetX;
//		newpara.fileHead.minY    += g_pOpticalMcPara->datbegOffsetY;
//		newpara.fileHead.maxX    += g_pOpticalMcPara->datbegOffsetX;
//		newpara.fileHead.maxY    += g_pOpticalMcPara->datbegOffsetY;

		ReSetFilePara(&newpara);


		SetNewDatFlag(0, FALSE);
		SetDatIdxToStart();

		printf("after add, file info:\r\n");
		OutFileInfo();
	}
}
//--------------------------------------------------------------------------------------------------------------
typedef struct
{
	u32 dtype;				// 数据类型
	int exdir;				// 执行方向
	int simulation;			// 模拟工作标志

	//----------------
	s32 rworkPos;		// 旋转理论位置
	s32 liftofst;		// 提刀预先行走位移
	//----------------
	// 速度控制参数
	u32 minpps, maxpps, addppsg;
	u32 minadd, addadd;

	u32 minchgpps;           //最小速度变化限制
	//----------------
	u32 slowAngle;			// 降速角度
	u32 liftAngle;			// 提刀角度

	int runIdx;				// 运行计数
	int runnum;				// 需要运行的个数

	int drawstidx;			// 画笔数据的起始IDX
}OpticalFillCtrl;

#define	EXDIR_POSI	1		// 正向执行
#define	EXDIR_NEGA	-1		// 反向执行

//-------------------------------------------------------------------------------
// 急停检测
int OpticalEmcScan(u32 para1, u32 para2)
{
	int rslt;

	rslt = GetCommonEms();
	if (MoveXLimitPosi() == SENSOR_ON)
	{
		printf("x=%d\r\n",g_motosPara.motosPos[MTX_IDX]);
		rslt = ERR_X_LIT_POSI;	//

	}

	if (MoveXLimitNega() == SENSOR_ON)
	{
		printf("x=%d\r\n",g_motosPara.motosPos[MTX_IDX]);
		rslt = ERR_X_LIT_NEGA;	//
	}

	if (MoveYLimitPosi() == SENSOR_ON)
	{
		rslt = ERR_Y_LIT_POSI;	//
	}

	if (MoveYLimitNega() == SENSOR_ON)
	{
		rslt = ERR_Y_LIT_NEGA;	//
	}


	return rslt;
}


// 防撞检测
int OpticalLightCheck(void)
{
	int rslt = ERR_NONE;

	if (rslt == ERR_NONE &&
		(MoveXLimitPosi() == SENSOR_ON))	// 防撞信号1
	{
	//	printf("OpticalLightCheck防撞检测 1\r\n");
		rslt = ERR_X_LIT_POSI;	//
	}

	if (rslt == ERR_NONE &&
		(MoveXLimitNega() == SENSOR_ON))	// 防撞信号2
	{
	//	printf("Impact Valid 2\r\n");
		rslt = ERR_X_LIT_NEGA;	//
	}

	return rslt;
}


// 快速停止检测
int OpticalQStopScan(u32 para1, u32 para2)
{
	int rslt;
	rslt = GetCommonQStop();
	if (rslt != ERR_NONE)
	{
		return rslt;
	}

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

	}

	return rslt;
}

// 普通停止检测
int OpticalNStopScan(u32 para1, u32 para2)
{
	int rslt;

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

	rslt = SendOpticalTask();			// 自动送光纤任务
	if (rslt != ERR_NONE)
	{
		return rslt;
	}


	return rslt;
}



//-------------------------------------------------------------------------------
// 虚轴
#define	OPTICALVX_MUTI		5	// 切割虚轴脉冲倍数
#define	VAxisMMPSToPPS(mmps)		(MMPS_TO_PPS(mmps) * OPTICALVX_MUTI)		// 虚轴速度加快
#define	VAxisMMPSSToPPSG(mmpss)		(MMPSS_TO_PPSG(mmpss) / OPTICALVX_MUTI)		// 加速度增大

// 刷新数据
int OpticalRefreshDat(u32 para1, u32 para2)
{
	int rslt;
	OpticalFillCtrl * pFlCtrl;
	MoveSwapData * pMvdat;
	DataItem datItem, tempItem;
	int dirx, diry, dirr;
	int dirp, dirs;
	u32 stidx;
	int vlen, vsource;
	u16 mvmtx, mvmty, mvmtr;
    u16 mvmtp, mvmts;
	u32 tabstpps;
	s32 dgap;

	pFlCtrl = (OpticalFillCtrl *)(para1);
	pMvdat = (MoveSwapData *)(para2);	// 数据

	if (pMvdat == NULL || pFlCtrl == NULL)
	{
		return -1;
	}

	// 填充数据处理
	if (	pMvdat->mvmtStatus == 0 &&	// 数据为空，可以填充数据
			pMvdat->bufsta != 3 &&		// 有buff为空
			1	)
	{
		pMvdat->waitNoData = 0;			// 无数据不停车标志

		if (0 ||
			pFlCtrl->dtype == DATA_DRAWING	||	 	// 刀数据
			pFlCtrl->dtype == DATA_PRESSURE	||		// 压料数据
			pFlCtrl->dtype == DATA_OPTI ||	    // 吐丝数据
			0	)
		{

			// 计算正在执行表结束时的速度（也就是将要填充表开始时的速度）
			if (pMvdat->lastEndPPS == 0)		// 第一次填充表
			{
				tabstpps = 0;
				//	printf("first begin spd calc\r\n");
			}
			else
			{
				dgap = pMvdat->calcGap;
				if (pMvdat->lastEndPPS > pMvdat->runPPS)	// 上段结束的速度大于本段的目标速度，那么正在执行的段是在减速（判断是否为减速）
				{
					dgap *= -1;
				}
				// 根据上一表（刚刚执行完的）的结束速度（正在执行表的起始速度）和正在执行表的加速度，计算正在执行表的结束速度。（未考虑跳过速度变化脉冲数的因素）
				tabstpps = CalcEndSpd(pMvdat->mvmtdat.interLong, pMvdat->lastEndPPS, dgap);	// 本次填充的起始速度
				if (dgap > 0)
				{
					if (tabstpps > pMvdat->runPPS)
					{
						tabstpps = pMvdat->runPPS;
					}
				}
				else
				{
					if (tabstpps < pMvdat->runPPS)
					{
						tabstpps = pMvdat->runPPS;
					}
				}

				if (pMvdat->execCount == 0)		// 还未执行完一次表
				{
					pMvdat->lastEndPPS = tabstpps;
				//	printf("set endpps=%d\r\n", pMvdat->lastEndPPS);
				}
			}

			//-----------
			stidx = GetDatIndex();	// 记录起始填充数据索引

			memset(&(pMvdat->mvmtdat), 0, sizeof(MovementDat));

			vlen = 0;	// 虚轴脉冲数
			vsource = 0;

			if (GetDatIndex() < GetFileItemNums())		// 有数据
			{
//				printf("Fill data idx=%d\r\n", GetDatIndex());

				do
				{
					rslt = GetADataItem(0, GetDatIndex(), &datItem);

					if (rslt == 0)
					{
//						if(pFlCtrl->dtype == DATA_OPTI)			// 吐丝数据
//						{
//							printf("DATA_OPTI:dx=%d, dy=%d\r\n", datItem.dx, datItem.dy);
//						}
//						if(pFlCtrl->dtype == DATA_PRESSURE)			// 压料数据
//						{
//							printf("DATA_PRESSURE:dx=%d, dy=%d\r\n", datItem.dx, datItem.dy);
//						}

						if (datItem.ctrl == (u8)(pFlCtrl->dtype) && 		// 数据类型相同，继续填充数据到当前段
							1	)
						{
							// 速度控制简化处理
							if (pMvdat->fillCount != 0 && (abs(datItem.dr) > pFlCtrl->slowAngle))	// datItem.dx < 0 非第一次填充，且角度差大于降速角度，不做填充数据，使运动暂停一下，
							{
								rslt = 1;
								break;
							}

#if (1)						// 转角提刀角度
							if (  pFlCtrl->dtype == DATA_DRAWING	||		// 布线数据
								0	)
							{

								if (pMvdat->fillCount != 0 && (abs(datItem.dr) > pFlCtrl->liftAngle))		// 非第一次填充，且角度差大于转角提刀角度，不做填充数据，使运动暂停一下，
								{
								//	printf("1. dr=%d, is more than lift angle\r\n", datItem.dr);
									rslt = 1;
									break;
								}
								datItem.dr -= pFlCtrl->liftofst;		//
							}

#endif

							if (datItem.dx != 0 || datItem.dy != 0 || datItem.dr != 0)			// 非空数据
							{
								if(pFlCtrl->dtype == DATA_OPTI)			// 吐丝数据
								{
									dirx = 0;
									diry = 0;
									dirr = 0;
									dirp = 0;//MTP_1PulseToP1Pulse(datItem.len)*COMPENSATE_SPIN;
									dirs = 0;//MTS1_IsSpin1PulseToS1Pulse(datItem.len);
								}
								else if(pFlCtrl->dtype == DATA_PRESSURE)
								{
									dirx = 0;
									diry = 0;
									dirr = 0;
									dirp = 0;
									dirs = 0;
								}
								else
								{
									dirx = datItem.dx;
									diry = datItem.dy;
									dirr = datItem.dr;
									dirp = 0;
									dirs = MTS1_IsWire001PulseToS1Pulse(datItem.len)*g_pOpticalMcPara->LevelOfAccAndDec/100;


									rslt = sendOpticalTaskWhenRun();//自动送光纤任务
#if(AFTERTURN_COMPEN == 1)
									XYCompenCalcuAfterTurning(datItem);//转弯后补偿计算
#endif

#if(MATERIAL_SCAN == 1)
//									if (dirs != 0)
//									{
//										g_optWorkCtrl.IsNoMaterialEn = 1;
//									}
//									else
//									{
//										g_optWorkCtrl.IsNoMaterialEn = 0;
//									}

									rslt  = isEmptyMaterialSta(datItem.len);		// 断线检测

#endif

									if(g_optWorkCtrl.compenFlag1 == 1)//转弯后进行补偿
									{
										dirx += g_optWorkCtrl.xCompenAfterTurn;
										diry += g_optWorkCtrl.yCompenAfterTurn;
										g_optWorkCtrl.compenFlag1 = 0;

										g_optWorkCtrl.oldXCompenAfterTurn = g_optWorkCtrl.xCompenAfterTurn;
										g_optWorkCtrl.oldYCompenAfterTurn = g_optWorkCtrl.yCompenAfterTurn;
									}

//									printf("rPos = %d, xPos = %d, yPos = %d,\r\n",g_motosPara.motosPos[MTR1_IDX],g_motosPara.motosPos[MTX_IDX],g_motosPara.motosPos[MTY_IDX]);

									//printf("idx = %d, dx = %d, dy = %d\r\n",g_workCtrl.cursta.dataIdx,datItem.dx,datItem.dy);
									//printf("idx = %d, dr = %d,\r\n",g_workCtrl.cursta.dataIdx,datItem.dr);
								}

								if (pFlCtrl->exdir == EXDIR_NEGA)		// 倒车
								{
									dirx *= (-1);
									diry *= (-1);
									dirr *= (-1);
								}
#if (XY_GAP_CONVERT == 1)
								{
//									g_optWorkCtrl.disosft = 1;
									dirx = XGapConvert(dirx);	// 间隙补偿转换
									diry = YGapConvert(diry);
//									g_optWorkCtrl.disosft = 0;
								}
#endif
								// 换算单位，数据单位是万分之一弧度, 电机单位是10000p/c
								if (dirr != 0)
								{
									dirr = PitkToRpp(dirr);
								}

								mvmtx = abs(dirx) & 0x7fff;
								mvmty = abs(diry) & 0x7fff;
								mvmtr = abs(dirr) & 0x7fff;
								mvmtp = abs(dirp) & 0x7fff;
								mvmts = abs(dirs) & 0x7fff;

								if (dirx < 0)
								{
									mvmtx |= 0x8000;
								}

								if (diry < 0)
								{
									mvmty |= 0x8000;
								}

								if (dirr < 0)
								{
									mvmtr |= 0x8000;
								}

								if (dirp < 0)
								{
									mvmtp |= 0x8000;
								}

								if (dirs < 0)
								{
									mvmts |= 0x8000;
								}

								vlen = datItem.len;
								vlen *= OPTICALVX_MUTI;

								if (vlen < (abs(dirr) & 0x7fff))
								{
									vlen = abs(dirr) & 0x7fff;
									vsource = 1;
								}

								vsource = vsource;

								memset(&(pMvdat->mvmtdat), 0, sizeof(MovementDat));

								pMvdat->mvmtdat.movement[MTX_IDX][0] = mvmtx;
								pMvdat->mvmtdat.movement[MTY_IDX][0] = mvmty;
								pMvdat->mvmtdat.movement[MTR1_IDX][0] = mvmtr;

								if(	pFlCtrl->dtype == DATA_DRAWING)	 	// 布线数据
								{
									//速度匹配
									if(IsOptStoringHigh2() == SENSOR_ON )
									{
										pMvdat->mvmtdat.movement[MTS1_IDX][0] = mvmts/g_pOpticalMcPara->LevelOfAccAndDec;
									}
//									else if(IsOptStoringHigh1() == SENSOR_ON)
//									{
//										pMvdat->mvmtdat.movement[MTS1_IDX][0] = mvmts*g_pOpticalMcPara->LevelOfAccAndDec;
//									}
									else
									{
										pMvdat->mvmtdat.movement[MTS1_IDX][0] =mvmts;
									}
									//拉料电机不转
									pMvdat->mvmtdat.movement[MTP1_IDX][0] = 0;
								}
								else if(pFlCtrl->dtype == DATA_OPTI)	    // 吐丝数据
								{
//									//速度匹配
//									if(IsOptStoringHigh2() == SENSOR_ON )
//									{
//										pMvdat->mvmtdat.movement[MTS1_IDX][0] = mvmts/g_pOpticalMcPara->LevelOfAccAndDec;
//									}
//									else if(IsOptStoringHigh1() == SENSOR_ON)
//									{
//										pMvdat->mvmtdat.movement[MTS1_IDX][0] = mvmts*g_pOpticalMcPara->LevelOfAccAndDec;
//									}
//									else
//									{
//										pMvdat->mvmtdat.movement[MTS1_IDX][0] = mvmts;
//									}
//									//拉料电机转
//									pMvdat->mvmtdat.movement[MTP1_IDX][0] = mvmtp;
//									pFlCtrl->maxpps /= 7;
//
//									vlen = vlen > abs(dirs) ? vlen : abs(dirs);
//									vlen = vlen > abs(dirp) ? vlen : abs(dirp);
//									vlen *= OPTICALVX_MUTI;
								}

								pMvdat->mvmtdat.interLong = vlen;

								pMvdat->mvmtdat.segmnetNum[MTX_IDX] = 1;	// 段数
								pMvdat->mvmtdat.vppSegment[MTX_IDX] = vlen;

								pMvdat->mvmtdat.segmnetNum[MTY_IDX] = 1;	// 段数
								pMvdat->mvmtdat.vppSegment[MTY_IDX] = vlen;

								pMvdat->mvmtdat.segmnetNum[MTS1_IDX] = 1;	// 段数
								pMvdat->mvmtdat.vppSegment[MTS1_IDX] = vlen;

								pMvdat->mvmtdat.segmnetNum[MTP1_IDX] = 1;	// 段数
								pMvdat->mvmtdat.vppSegment[MTP1_IDX] = vlen;

								pMvdat->mvmtdat.segmnetNum[MTR1_IDX] = 1;	// 段数
								pMvdat->mvmtdat.vppSegment[MTR1_IDX] = vlen;


								// 布线速度控制
								if (pMvdat->runstat == CRST_NORMAL)
								{
									if (rslt == 0)
									{
										u32 decCount, mintpps, temp;
										u32 idx; 		// 最小目标速度和所处的位置
										DataItem oldItem;

										if (pMvdat->lastEndPPS == 0)		// 第一次填充表
										{
											mintpps = pFlCtrl->maxpps;
											pMvdat->lastEndPPS = pFlCtrl->minpps;
										//	printf("first begin spd calc\r\n");
										}
										else
										{
#define DRLVBO_NUM		10		// 滑动循环队列
											s32 dracc = 0;		// 累计角度变化
											int queueidx = 0;
											s16 drqueue[DRLVBO_NUM];
											memset(drqueue, 0, sizeof(s16)*DRLVBO_NUM);
											//	printf("other spd calc\r\n");

											decCount = CalcDisplacement(tabstpps, pFlCtrl->minpps, pFlCtrl->addppsg);	// 从本表起始速度降速到最低速所需要的位移
										//	printf("decCount = %d\r\n", decCount);

											// 扫描后续数据，找到减速点
											mintpps = pFlCtrl->maxpps;		// 最小速度初值为最大切割速度
											idx = stidx;
											memset(&tempItem, 0, sizeof(DataItem));

											while (decCount > 0)
											{
												idx++;
												memcpy(&oldItem, &tempItem, sizeof(DataItem));
												rslt = GetADataItem(0, idx, &tempItem);	// 读到一个数据
												if (rslt == 0)
												{
													// 得到当前数据需要的速度
													if (tempItem.ctrl == (u8)(pFlCtrl->dtype)) 			// 数据类型相同
													{
														dracc -= drqueue[queueidx];		// 减去最前面的
														drqueue[queueidx] = abs(tempItem.dr);
														dracc += drqueue[queueidx];		// 加上最新的
														queueidx++;
														if (queueidx >= DRLVBO_NUM)
														{
															queueidx = 0;
														}
														if (dracc > pFlCtrl->slowAngle ||
															dracc > pFlCtrl->liftAngle )
														{
														//	printf("dracc=%d, more than slow or lift angle idx=%d\r\n", dracc, idx);
															temp = pFlCtrl->minpps;	// 简化处理，直接降速到最低
														}
														else
														{
															if (((oldItem.dx * tempItem.dx) < 0) ||
																((oldItem.dy * tempItem.dy) < 0) ||	// 方向改变
																0 	)
															{
																if (abs(tempItem.dr) > pFlCtrl->slowAngle)		// 角度差大于降速角度，需要降速
																{
																	// 速度控制简化处理
																//	printf("2. dr=%d, is more than slow angle\r\n", tempItem.dr);
																	// 往下扫描不管多少针，到要降速的点，就认为那的速度是0，以当前速度为降速的初始速度，且当检测到降速点，那么认为从现在的位置到降速点的距离都是降速区，
																	temp = pFlCtrl->minpps;	// 简化处理，直接降速到最低
																}
																else if (abs(tempItem.dr) > pFlCtrl->liftAngle)
																{
																//	printf("2. dr=%d, is more than lift angle\r\n", tempItem.dr);
																	temp = pFlCtrl->minpps;	// 简化处理，直接降速到最低
																}
																else
																{
																	temp = mintpps;
																}
															}
															else
															{
																temp = mintpps;
															}
														}
													}
													else
													{
														// 不同类型，直接降速到最低
													//	printf("other type, use minpps\r\n");
														temp = pFlCtrl->minpps;	// 目标速度
													}

													if (mintpps > temp)		// 最小速度大于当前计算速度
													{
													//	printf("begin slow\r\n");
														mintpps = temp;		// 开始降速
														break;
													}

													if (decCount > tempItem.len*OPTICALVX_MUTI)
													{
														decCount -= tempItem.len*OPTICALVX_MUTI;
													}
													else
													{
														decCount = 0;
													}
												}
												else
												{
													printf("data end, use minpps\r\n");
													mintpps = pFlCtrl->minpps;	// 目标速度
													break;
												}
											}
										}

										pMvdat->lowPPS = pFlCtrl->minpps;
										pMvdat->runPPS = mintpps;
										pMvdat->calcGap = pFlCtrl->addppsg;

									//	printf("curpps=%d, idx=%d, lowPPS=%d, runPPS=%d, calcGap=%d\r\n", curpps, pMvdat->fillCount, pMvdat->lowPPS, pMvdat->runPPS, pMvdat->calcGap);
									//	printf("curpps=%d, idx=%d, runPPS=%d\r\n", curpps, pMvdat->fillCount, pMvdat->runPPS);
									}
								}

								pMvdat->waitNoData = 1;		// 无数据不停车标志
								pMvdat->mvmtStatus = 1;		// 设置数据有效标志
							}

							pFlCtrl->liftofst = 0;
							if (pFlCtrl->exdir == EXDIR_NEGA)		// 倒车
							{
								DatIndexDec();
								ChangeWorkPos(&datItem, -1);
							}
							else
							{
								DatIndexInc();
								rslt = GetADataItem(0, GetDatIndex(), &datItem);
								ChangeWorkPos(&datItem, 1);//改变坐标
							}

							if (datItem.dx == 0 && datItem.dy == 0 && datItem.dr == 0)			// 非空数据
							{
								//continue;
							}
						}
						else
						{
							rslt = 1;
//							printf("fill type not equ, enter stop phase\r\n");
							break;
						}
					}
					else
					{
						printf("GetADataItem rslt = %d\r\n", rslt);
					}

					break;
				}while(1);
			}
			else
			{
				// printf("Fill err idx=%d, stitches=%d\r\n", GetDatIndex(), GetFileItemNums());
			}
		}
		else
		{
			// 其他数据，暂时不支持
		}
	}

	if (pMvdat->runstat == CRST_NSTOP || 	// 普通停止准备进入停止或刹车状态
		pMvdat->runstat == CRST_QSTOP ||	// 快速停止准备进入停止或刹车状态
		0	)
	{
		pMvdat->lowPPS = pFlCtrl->minpps;
		pMvdat->runPPS = pFlCtrl->minpps;	// 运行速度
		pMvdat->calcGap = pFlCtrl->addppsg;	// 减速加速度

		printf("in stop status, xy move\r\n");
		pMvdat->runstat = CRST_BREAK;		// 进入刹车状态
	}

	if (pMvdat->runstat == CRST_BREAK &&
		pMvdat->fillLimit == 0 &&
		pMvdat->waitNoData != 0 &&
		1	)
	{
		pMvdat->waitNoData = 0;			// 无数据停止
	}
	return 0;
}

#if(AFTERTURN_COMPEN == 1)
void XYCompenCalcuAfterTurning(DataItem datItem)
{
	if(((datItem.dx != 0) || (datItem.dy != 0)) && (datItem.attr != DATA_PRESSURE))
	{
		if((datItem.dr == 0) && (g_optWorkCtrl.runSteps1 == 0))//先直线
		{
			g_optWorkCtrl.runSteps1 = 10;
		}
		else if((datItem.dr != 0) && (g_optWorkCtrl.runSteps1 == 10))//后拐弯
		{
			g_optWorkCtrl.begainRBeforeWiring = g_workCtrl.workPos[MTR1_IDX];//用来计算补偿
			g_optWorkCtrl.runSteps1 = 20;
		}
		else if((datItem.dr == 0) && (g_optWorkCtrl.runSteps1 == 20))//拐弯之后
		{
			g_optWorkCtrl.xCompenAfterTurn = g_pOpticalMcPara->rkMatThickness/100*cos(g_optWorkCtrl.begainRBeforeWiring/10000) - g_optWorkCtrl.oldXCompenAfterTurn;
			g_optWorkCtrl.yCompenAfterTurn = g_pOpticalMcPara->rkMatThickness/100*sin(g_optWorkCtrl.begainRBeforeWiring/10000) - g_optWorkCtrl.oldYCompenAfterTurn;
			g_optWorkCtrl.compenFlag1 = 1;
			g_optWorkCtrl.runSteps1 = 0;
		}
	}
}
#endif
//-------------------------------------------------------------------------------
int IsSignalTimeout(u8 (* func)(void), int timeoutMs, int err)
{
	if(SIGNAL_TIMEOUT_ENABLE == 0)
	{
		return 0;
	}
	do
	{
		DelayRef(1);
		timeoutMs--;
		if(timeoutMs < 0)
		{
			return err;
		}
		if(func() == SENSOR_ON)
		{
			return 0;
		}
	}while(1);

}
//-------------------------------------------------------------------------------
s16 Rot001ToPitk(s32 ang001)
{
	double temp = ang001;
	temp = ROT_001DEG_TO_PITK(temp);
	return (s16)temp;
}

int toActSpinLen(int theorySpin)
{
	theorySpin = theorySpin * g_pOpticalMcPara->SpinCompenCalBase / g_pOpticalMcPara->SpinCompenCalActLen;
	return theorySpin;
}
//-------------------------------------------------------------------------------
// 切割控制过程
// 支持 偏移, 记号（画笔），尖刀，震刀，圆刀，钻孔数据

//#define	CUTTER_CUTTING		0x00000020		// bit5, 正在切割标志		, = 1 在切割
//#define	CUTTER_CUT_OVER		0x00000040		// bit6, 切割完成标志		, = 1 切割完成
//#define	CUTTER_RUNNING		0x00000100		// bit8, 正在运行标志		, = 1 正在运行

int OpticalWork(int mode)
{

	int err, rslt, ret;
	ContinuousInterMoveCtrl cmctrl;
	OpticalFillCtrl fillctrl;
	DataItem curDat, oldDat, tmpDat;

	u8 oldDataForOffset = 0;
	int xIdx;
	int yIdx;
	int p1Idx, r1Idx, s1Idx;

	int spinLen = 0;

	int spinLenForOffset = 0;

	int idx, idxbuf, count, begIdx;

	idxbuf = 0;
	err = IsAutoMoveEnable();	// 切割前判断条件

	// 判断数据合法性
	AddNewFileOffset();

	getOptBorderWhenWork();

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

	if (err != ERR_NONE)
	{
		ReportError(err);
		return err;
	}

	//启动工作开始的处理
	{
		g_optWorkCtrl.isEndOfSpinFlag = 0;
		g_optWorkCtrl.runSteps1 = 0;
		g_optWorkCtrl.compenFlag1 = 0;
		g_optWorkCtrl.IsNoMaterialFlag = 2;
		g_optWorkCtrl.IsNoMaterialScanLen = g_pOpticalMcPara->materialFreeScanNum;
		g_optWorkCtrl.IsNoMaterialEn = 1;
		err = OpticalCloseAll();
		// 打开吸风
		FanMotosOn();		// 吸风开
		if (err == ERR_NONE)
		{
			err = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头
		}
	}


	if (err == ERR_NONE)
	{
		// 索引有效
		if (IsDatIndexAvailable(g_workCtrl.cursta.dataIdx) != TRUE)

		{
			SetDatIdxToStart();
		}

		if (g_workCtrl.cursta.dataIdx != 0)
		{// 向前找到吐丝数据
			DataItem chackDat;

	  		do
			{
	  			GetADataItem(0, g_workCtrl.cursta.dataIdx, &chackDat);

				if(chackDat.ctrl == DATA_OPTI)//判断是否在吐丝处偏移
				{
					break;
				}
				else
				{
					g_workCtrl.cursta.dataIdx--;
					if (g_workCtrl.cursta.dataIdx == 0)
					{
						break;
					}
				}
			}while(1);
			printf("find near opti data to file idx=%d\r\n", g_workCtrl.cursta.dataIdx);
		}

		{
			GetWorkPosFromIndex();
		}
	}

	if (err == ERR_NONE)
	{
		// XYR走到正确的工作位置
		rslt = GetADataItem(0, GetDatIndex(), &curDat);
		if (rslt != 0)
		{
			err = STA_WORK_OVER;	// 结束
		}
		else
		{
			FixMatCylinderOn();
			DelayRef(10);
			err = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头

			ReadMotosPos();
			if(g_workCtrl.workPos[MTX_IDX] <= g_motosPara.motosPos[MTX_IDX])
			{
				g_workCtrl.workPos[MTX_IDX] -= g_pOpticalMcPara->startPosoffx;
				g_optWorkCtrl.usecomx = 1;
			}
			if(g_workCtrl.workPos[MTY_IDX] <= g_motosPara.motosPos[MTY_IDX])
			{
				g_workCtrl.workPos[MTY_IDX] -= g_pOpticalMcPara->startPosoffy;
				g_optWorkCtrl.usecomy = 1;
			}
/*			if (err == ERR_NONE)
			{
				err = MoveXYToPos(-1100, -6000, NULL);
			}*/
			if (err == ERR_NONE)
			{
				err = MoveXYRS1P1ToWorkPos(&curDat, NULL, 1, 0);
			}

			if(g_optWorkCtrl.usecomx == 1 ||
				g_optWorkCtrl.usecomy == 1)
			{
				if(g_optWorkCtrl.usecomx == 1)
				{
					g_workCtrl.workPos[MTX_IDX] += g_pOpticalMcPara->startPosoffx;
					g_optWorkCtrl.usecomx = 0;
				}
				if(g_optWorkCtrl.usecomy == 1)
				{
					g_workCtrl.workPos[MTY_IDX] += g_pOpticalMcPara->startPosoffy;
					g_optWorkCtrl.usecomy = 0;
				}
				if (err == ERR_NONE)
				{
					err = MoveXYRS1P1ToWorkPos(&curDat, NULL, 1, 0);
				}
			}
		}
	}

	// 初始化数据控制
	memset(&fillctrl, 0, sizeof(OpticalFillCtrl));	// 清零
	memset(&oldDat, 0, sizeof(DataItem));
	if (g_workCtrl.cursta.dataIdx != 0)
	{
		GetADataItem(0, (g_workCtrl.cursta.dataIdx - 1), &oldDat);
	}
	if (err == ERR_NONE)
	{
		if (mode == WKMD_BACKWORD)
		{
			fillctrl.exdir = EXDIR_NEGA;	// 方向为负
			rslt = DatIndexDec();
			if (rslt < 0)
			{
				err = STA_WORK_OVER;	// 结束
			}
		}
		else
		{
			fillctrl.exdir = EXDIR_POSI;	// 方向为正
		}

		if (mode == WKMD_SIMULATION)
		{
			fillctrl.simulation = 1;		// 模拟执行
		}
	}

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

	g_workCtrl.cursta.workStatus |= (WORK_STA_BUSY|WORK_STA_WORKING);		// 清除正在忙标志

	// 初始化控制结构
	xIdx = MTX_IDX;
	yIdx = MTY_IDX;
	p1Idx = MTP1_IDX;
	r1Idx = MTR1_IDX;
	s1Idx = MTS1_IDX;


	memset(&cmctrl, 0, sizeof(ContinuousInterMoveCtrl));	// 清零
	cmctrl.vAxisId = VAXIS_ID1;			// 使用虚轴1
	cmctrl.motosConfig.newConfig = 0;	// 需要重新配置

#if (MODULE_X != 0)
	cmctrl.motosConfig.axisConfig[xIdx].axisConfig = MTX_CFG;						// 实轴输出配置
	cmctrl.motosConfig.axisConfig[xIdx].poutType = MTX_OTP;							// 脉冲输出模式(CW/CCW，PULSE/DIR)
	cmctrl.motosConfig.axisConfig[xIdx].spdSource = MAKE_POUT_CMD(cmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	cmctrl.motosConfig.axisConfig[xIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if (MODULE_Y != 0)
	cmctrl.motosConfig.axisConfig[yIdx].axisConfig = MTY_CFG;						// 实轴输出配置
	cmctrl.motosConfig.axisConfig[yIdx].poutType = MTY_OTP;							// 脉冲输出模式(CW/CCW，PULSE/DIR)
	cmctrl.motosConfig.axisConfig[yIdx].spdSource = MAKE_POUT_CMD(cmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	cmctrl.motosConfig.axisConfig[yIdx].datSource = DATI_INTERPOLATION;				// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)										// 各个轴的运动量，0，无运动；其他，运动长度，单位p
#endif

#if (TURN_R1 != 0)
	cmctrl.motosConfig.axisConfig[r1Idx].axisConfig = MTR1_CFG;						// 实轴输出配置
	cmctrl.motosConfig.axisConfig[r1Idx].poutType = MTR1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	cmctrl.motosConfig.axisConfig[r1Idx].spdSource = MAKE_POUT_CMD(cmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	cmctrl.motosConfig.axisConfig[r1Idx].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if (MODULE_SEND != 0)
	cmctrl.motosConfig.axisConfig[s1Idx].axisConfig = MTS1_CFG;						// 实轴输出配置
	cmctrl.motosConfig.axisConfig[s1Idx].poutType = MTS1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	cmctrl.motosConfig.axisConfig[s1Idx].spdSource = MAKE_POUT_CMD(cmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	cmctrl.motosConfig.axisConfig[s1Idx].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

#if (MODULE_PULL != 0)
	cmctrl.motosConfig.axisConfig[p1Idx].axisConfig = MTP1_CFG;						// 实轴输出配置
	cmctrl.motosConfig.axisConfig[p1Idx].poutType = MTP1_OTP;						// 脉冲输出模式(CW/CCW，PULSE/DIR)
	cmctrl.motosConfig.axisConfig[p1Idx].spdSource = MAKE_POUT_CMD(cmctrl.vAxisId);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
	cmctrl.motosConfig.axisConfig[p1Idx].datSource = DATI_INTERPOLATION;			// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
#endif

	cmctrl.Delay = DelayRef;						// 延时回调函数
	cmctrl.RefreshData = OpticalRefreshDat;			// 数据刷新函数
	cmctrl.refDatPara = (u32)(&fillctrl);			// 数据刷新参数
	cmctrl.RefreshPosition = NormalRefPosition;		// 坐标刷新回调函数      //
	cmctrl.refPosPara = 0;							// 坐标刷新参数

	cmctrl.WorkBeforeRun = NULL;
	cmctrl.WorkAfterRun = NULL;
	cmctrl.workPara1 = (u32)(&fillctrl);
	cmctrl.workPara2 = 0;


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

// 执行循环
	do
	{
		// 读取一组数据
		err = ERR_NONE;
		rslt = GetADataItem(0, GetDatIndex(), &curDat);

		if(curDat.ctrl == DATA_OPTI)
		{
			curDat.ctrl = DATA_DRAWING;
		}

		if (GetDatIndex() < GetFileItemNums())
		{
			rslt = GetADataItem(0, GetDatIndex(), &curDat);
			if (rslt != 0)
			 {
				err = STA_WORK_OVER;	// 结束
			}
		}
		else
		{
			err = STA_WORK_FINISH;		// 完成
		}

		//printf("Get a data item [idx=%d], ctrl=0x%x, dr=%d\r\n", GetDatIndex(), curDat.ctrl, curDat.dr);

		if (err != ERR_NONE)
		{
			if (mode == WKMD_BACKWORD)		// 回退
			{
				rslt = DatIndexInc();
			}
			printf("err at get step data\r\n");
			break;
		}

		if ((oldDat.ctrl != curDat.ctrl)										&&
			!(oldDat.ctrl == DATA_DRAWING && curDat.ctrl == DATA_PRESSURE)		&&
			!(curDat.ctrl == DATA_DRAWING && oldDat.ctrl == DATA_PRESSURE)
			)
		{
			oldDataForOffset = oldDat.ctrl;
			//布线结束后的动作
			if((oldDat.ctrl == DATA_DRAWING || oldDat.ctrl == DATA_PRESSURE) && g_optWorkCtrl.motopreFlag == 1)
			{
				FixMatCylinderOff();//顶料松开
				DelayRef(300);
				UpClipMatClipCylinderOff();//上夹料松开
				err = IsSignalTimeout(UpClipMatClipCylinderPlaceOff, CYLINDER_TIMEOUT, ERR_UPCLIP_ERROR);//等上夹料回位传感器亮起

				PreMatCylinderOn();//压料
				DelayRef(200);

				if(err == ERR_NONE)
				{
					err = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头
				}
				if(err == ERR_NONE)
				{
					//FixMatCylinderOn();//顶料顶料
					PreMatCylinderOff();//压料气缸松开
				}
				//err = IsSignalTimeout(IsOptPreLiftUp, CYLINDER_TIMEOUT, 0x0E00);//等压料回位传感器亮起
				g_optWorkCtrl.motopreFlag = 0;

				if (err != ERR_NONE)
				{
					ReportError(err);
					break;
				}

				g_optWorkCtrl.xCompenAfterTurn = 0;
				g_optWorkCtrl.yCompenAfterTurn = 0;
				g_optWorkCtrl.silkRetreat = 1;
			}

			if(oldDat.ctrl == DATA_OPTI && g_optWorkCtrl.motoRunFlag == 1)
			{//吐丝结束后的动作

				if(g_optWorkCtrl.cutingFlag == 1)
				{//剪线
					err = IsSignalTimeout(IsOptPreLiftUp, CYLINDER_TIMEOUT, 0x002F);//压料气缸回位传感器亮起
					if(err == ERR_NONE)
					{
						DelayRef(200);
						TrimmingCylinderOn();  //剪线气缸开
						DelayRef(200);
						TrimmingCylinderOff();     //剪线气缸关
						DelayRef(100);
					}
				}

				if(err == ERR_NONE)
				{
					if(g_optWorkCtrl.cutingFlag == 1)
					{
						err = MoveP1S1AsOffset(g_pOpticalMcPara->beforeSpinLen,NULL);//预吐丝
						g_optWorkCtrl.cutingFlag = 0;
						g_optWorkCtrl.lowingFlag = 0;
					}

					if(err == ERR_NONE)
					{
						FixMatCylinderOn();//顶料顶料
						DelayRef(200);
						DownClipMatClipCylinderOff();//下夹料松开
						UpClipMatClipCylinderOff();//上夹料松开
						DelayRef(200);
						IsBlowingCylinderOff();//送料收回
						DelayRef(500);
						if(g_optWorkCtrl.silkRetreat == 1)
						{
							g_optWorkCtrl.silkRetreat = 0;
							//回到吐丝位
							err = MoveXYToPos(g_optWorkCtrl.opitPosx, g_optWorkCtrl.opitPosy, NULL);
							if (err != ERR_NONE)
							{
								ReportError(err);
								break;
							}
							DelayRef(100);
						}
						if(g_optWorkCtrl.lowingFlag == 1)
						{
							err = MoveU1ToPos(g_pOpticalMcPara->rkLowPos, NULL);//机头下降
						}
					}
				}

				if (err != ERR_NONE)
				{
					ReportError(err);
					break;
				}
			}

			// 吐丝数据动作处理
			if (curDat.ctrl == DATA_OPTI)
			{
				g_optWorkCtrl.motoRunFlag = 1;
				err = MoveR1ToPos(2500, NULL);//旋转到吐丝位置:90°

				if(err == ERR_NONE)
				{
					PreMatCylinderOff();//压料气缸松开
					err = IsSignalTimeout(IsOptPreLiftUp, CYLINDER_TIMEOUT, 0x002F);//压料气缸回位传感器亮起
				}

				if(err == ERR_NONE)
				{
					err = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//机头抬起

					if(err == ERR_NONE)
					{
						TrimmingCylinderOn();  //剪线气缸开
						DelayRef(300);
						IsBlowingCylinderOn();// 送料伸出
						DelayRef(700);
						DownClipMatClipCylinderOn();//下夹料夹料
						DelayRef(700);
						err = IsSignalTimeout(DownClipMatClipCylinderPlaceOn, CYLINDER_TIMEOUT, ERR_DOWNCLIP_ERROR);//等下夹料出位传感器亮起
						TrimmingCylinderOff(); //剪线气缸关
					}
					DelayRef(300);
					UpClipMatClipCylinderOn();//上夹料夹料
				}

				if(err == ERR_NONE)
				{
					err = IsSignalTimeout(UpClipMatClipCylinderPlaceOn, CYLINDER_TIMEOUT, ERR_UPCLIP_ERROR);//等上夹料气缸-出位传感器亮起
					FixMatCylinderOff();
					DelayRef(300);
				}

				if(g_optWorkCtrl.isEndOfSpinFlag == 1)
				{
					if(curDat.len > (spinLenForOffset/100))
					{
						spinLen = curDat.len - (spinLenForOffset/100);
					}
					else
					{
						spinLen = 0;
					}
					g_optWorkCtrl.isEndOfSpinFlag = 0;
				}
				else
				{
					spinLen = curDat.len;
				}

				if(err == ERR_NONE)
				{
					printf("spin actpos=(%d,%d)\r\n", g_motosPara.motosPos[MTX_IDX],g_motosPara.motosPos[MTY_IDX]);
					g_optWorkCtrl.IsNoMaterialEn = 1;
					err = MoveP1S1AsOffset(spinLen,NULL);//吐丝
				}
				if (err != ERR_NONE)
				{
					ReportError(err);
					break;
				}
			}

			// 布线数据动作处理
			if (curDat.ctrl == DATA_DRAWING )//|| curDat.ctrl == DATA_PRESSURE)
			{
				g_optWorkCtrl.IsNoMaterialScanLen = g_pOpticalMcPara->materialFreeScanNum;

				g_optWorkCtrl.oldXCompenAfterTurn = 0;
				g_optWorkCtrl.oldYCompenAfterTurn = 0;

				if(err == ERR_NONE)
				{
					g_optWorkCtrl.motopreFlag = 1;
					err = MoveR1ToPos(PitkToRpp(g_workCtrl.workPos[MTR1_IDX]),NULL);
					if (err == ERR_NONE)
					{
						err = MoveXYRS1P1SToWorkPos(&curDat, 0);		// 走到正确位置
					}


					if(err == ERR_NONE)
					{
						err = MoveU1ToPos(g_pOpticalMcPara->rkLowPos, NULL);//机头下降
						PreMatCylinderOn();//压料
						DelayRef(200);
						PreMatCylinderOff();//压料松开
						DelayRef(300);
						UpClipMatClipCylinderOn();//上夹料夹料
					}
					if(err == ERR_NONE)
					{
						err = IsSignalTimeout(UpClipMatClipCylinderPlaceOn, CYLINDER_TIMEOUT, ERR_UPCLIP_ERROR);//等上夹料出位传感器亮起
					}

					FixMatCylinderOff();//顶料松开
				}

				if (err != ERR_NONE)
				{
					ReportError(err);
					break;
				}
			}
		}

		// 压料数据动作处理
		if(curDat.ctrl == DATA_PRESSURE)
		{
			PreMatCylinderOn();//顶料顶料
			DelayRef(200);
			PreMatCylinderOff();//顶料松开
			DelayRef(300);
		}

		memcpy(&oldDat, &curDat, sizeof(DataItem));

		// 扫描后续数据，确定连续同类型数据的个数
		begIdx = GetDatIndex();
		idx = begIdx;
		count = 0;		// 计数值
		do
		{

			count++;
			if (fillctrl.exdir == EXDIR_POSI)		// 正向执行
			{
				idx++;
			}
			else if (fillctrl.exdir == EXDIR_NEGA)	// 反向执行
			{
				if (idx != 0)
				{
					idx--;
				}
			}

			if (idx < GetFileItemNums())
			{
				rslt = GetADataItem(0, idx, &tmpDat);
				if (rslt != 0)
				{
					break;
				}
			}
			else
			{
				break;
			}

			if (curDat.ctrl != tmpDat.ctrl)
			{
				break;
			}

			if (0)
			{
				if (idxbuf < idx)
				{
					idxbuf = idx;
					//printf("[idx=%d], ctrl=0x%x, dr=%d\r\n", idx, curDat.ctrl, curDat.dr);
					if ((idx % 20) == 0)
					{
						DelayRef(10);
					}
				}
			}

		}while(1);

	//	printf("Get data, ctrl = 0x%x, begidx=%d, num=%d\r\n", curDat.ctrl, begIdx, count);

		cmctrl.motosConfig.newConfig = 0;	// 不需要重新配置

		fillctrl.dtype = curDat.ctrl;	// 运动类型 = 控制字节
		fillctrl.runIdx = 0;				// 本次运行计数
		fillctrl.runnum = count;			// 运行的线段数


		// 根据运行长度，确定最大运行速度
		memset(&(cmctrl.mvData), 0, sizeof(MoveSwapData));

		if (curDat.ctrl == DATA_DRAWING || curDat.ctrl == DATA_OPTI)
		{
			fillctrl.minpps = VAxisMMPSToPPS(g_pOpticalMcPara->minRkOptSpd);	// 最小布线速度
			fillctrl.maxpps = VAxisMMPSToPPS(g_pOpticalMcPara->maxRkOptSpd);	// 最大布线速度
			fillctrl.addppsg = VAxisMMPSSToPPSG(g_pOpticalMcPara->rkOptAdd);
			fillctrl.minadd = VAxisMMPSSToPPSG(g_pOpticalMcPara->rkOptAdd);

			if (fillctrl.minadd < MIN_GAP)
			{
				fillctrl.minadd = MIN_GAP;
			}
			else if (fillctrl.minadd > MAX_GAP)
			{
				fillctrl.minadd = MAX_GAP;
			}
			fillctrl.addadd = MAX_GAP;

			// 降速角度
			fillctrl.slowAngle = Rot001ToPitk(g_pOpticalMcPara->rkOptSlowAngle);
			fillctrl.liftAngle = Rot001ToPitk(g_pOpticalMcPara->rkLiftAng);

			cmctrl.stopPPS = VAxisMMPSToPPS(g_pOpticalMcPara->minRkOptSpd);		// 停车速度
			cmctrl.nstopPPSG = VAxisMMPSSToPPSG(g_pOpticalMcPara->rkOptAdd);	// 普通停止加速度
			cmctrl.qstopPPSG = VAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);	// 快速停止加速度
		}

		// 变速速度限制
		fillctrl.minchgpps = VAxisMMPSToPPS(g_pOpticalMcPara->optMinSpd*1.0);
//		printf("calc slowAngle = %d, liftAngle=%d, minchgpps=%d\r\n", fillctrl.slowAngle, fillctrl.liftAngle, fillctrl.minchgpps);

//		printf("\r\nbefore ContinuousInterpolation, dataidx=%d\r\n", g_workCtrl.cursta.dataIdx);
//		printf("workpos: x=%d, y=%d\r\n", g_workCtrl.workPos[MTX_IDX], g_workCtrl.workPos[MTY_IDX]);
//		printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);

		rslt = ContinuousInterpolation(&cmctrl);		// 连续插补运动

//		printf("after ContinuousInterpolation, dataidx=%d, rslt=%d\r\n", g_workCtrl.cursta.dataIdx, rslt);
//		printf("workpos: x=%d, y=%d\r\n", g_workCtrl.workPos[MTX_IDX], g_workCtrl.workPos[MTY_IDX]);
//		printf("actpos: x=%d, y=%d\r\n", g_motosPara.motosPos[MTX_IDX], g_motosPara.motosPos[MTY_IDX]);


#if(MATERIAL_SCAN == 1)
//		g_optWorkCtrl.IsNoMaterialEn = 0;
//		g_optWorkCtrl.IsNoMaterialScanLen = TBDSCAN_NUM;
#endif

		// 异常停止的情况下，可能有些填充数据没有执行，处理
		idx = GetDatIndex();	// 当前数据指针
		if (idx < begIdx)
		{
			count = begIdx-idx;
		}
		else
		{
			count = idx - begIdx;
		}
//			printf("datcount=%d, fillCount=%d, execCount=%d\r\n", count, cmctrl.mvData.fillCount, cmctrl.mvData.execCount);
#if (0)
		idx = cmctrl.mvData.fillCount;	// 已填充数据
#else
		idx = count;					// 已移动指针数据
#endif
		if(idx > cmctrl.mvData.execCount)	// 填充数据大于执行数据
		{
			//printf("fill data are not be execed, idx=%d\r\n", idx);
			if (fillctrl.exdir == EXDIR_POSI)
			{
				ret = GetADataItem(0, g_workCtrl.cursta.dataIdx, &tmpDat);
				ChangeWorkPos(&tmpDat, -1);
				ret = DatIndexDec();
				if (ret < 0)
				{
					err = STA_WORK_OVER;		// 结束
				}
			}
			else if (fillctrl.exdir == EXDIR_NEGA)
			{
				ret = GetADataItem(0, g_workCtrl.cursta.dataIdx, &tmpDat);
				ChangeWorkPos(&tmpDat, 1);
				ret = DatIndexInc();
				if (ret < 0)
				{
					err = STA_WORK_FINISH;		// 完成
				}
			}
			idx--;
		}

		if (err == ERR_NONE)
		{
			err = GetMoveResultError(rslt, cmctrl.mvData.errInfo);
		}

		if (curDat.ctrl == DATA_OFFSET)		// 偏移数据
		{
			g_optWorkCtrl.IsNoMaterialEn = 0;

			UpClipMatClipCylinderOff();//上夹料松开
			err = IsSignalTimeout(UpClipMatClipCylinderPlaceOff, CYLINDER_TIMEOUT, ERR_UPCLIP_ERROR);//等上夹料回位传感器亮起
			if(err != ERR_NONE)
			{
				ReportError(err);
				break;
			}
			DataItem * pDat = NULL;

			//吐丝减去每根光纤尾部因偏移产生的量
			int datx1,daty1,datx2,daty2;

			datx1 = g_motosPara.motosPos[MTX_IDX];
			daty1 = g_motosPara.motosPos[MTY_IDX];

			int tempx, tempy, temp;
			// 扫描后续数据，得到所有连续的偏移数据
			do
			{
				if(err != ERR_NONE)
				{
					break;
				}
				if (GetDatIndex() < GetFileItemNums())
				{
					rslt = GetADataItem(0, GetDatIndex(), &tmpDat);
					if (rslt != 0)
					{
						break;
					}
					pDat = &tmpDat;

					if (curDat.ctrl != tmpDat.ctrl)
					{
						break;
					}

					memcpy(&oldDat, &curDat, sizeof(DataItem));

					if (fillctrl.exdir == EXDIR_POSI)		// 正向执行
					{
						DatIndexInc();
					}
				   else if (fillctrl.exdir == EXDIR_NEGA)	// 反向执行
				   {
						DatIndexDec();
					}
				}
				else
				{

					break;
				}
			}while(1);

			GetWorkPosFromIndex();

			datx2 = g_workCtrl.workPos[MTX_IDX];
			daty2 = g_workCtrl.workPos[MTY_IDX];

			//布线结束后偏移到吐丝位置（判定：偏移数据前是布线数据，后是吐丝数据）
			if((oldDataForOffset == DATA_DRAWING || oldDataForOffset == DATA_PRESSURE) && tmpDat.ctrl == DATA_OPTI)
			{
				if(g_optWorkCtrl.useGapCorx == 1)
				{
					g_workCtrl.workPos[MTX_IDX] -= g_pOpticalMcPara->xGapCorrect;
					g_optWorkCtrl.useGapCorx = 0;
				}
				if(g_optWorkCtrl.useGapCory == 1)
				{
					g_workCtrl.workPos[MTY_IDX] -= g_pOpticalMcPara->yGapCorrect;
					g_optWorkCtrl.useGapCory = 0;
				}
				spinLenForOffset = GetSyntheticLength(datx2 - datx1,daty2 - daty1);
				g_optWorkCtrl.isEndOfSpinFlag = 1;
				g_optWorkCtrl.cutingFlag = 1;
				oldDataForOffset = 0;
				g_optWorkCtrl.offsetToOpit = 1;
				UpClipMatClipCylinderOn();//上夹料夹料
				err = IsSignalTimeout(UpClipMatClipCylinderPlaceOn, CYLINDER_TIMEOUT, ERR_UPCLIP_ERROR);//等上夹料气缸-出位传感器亮起
				if(err == ERR_NONE)
				{
					FixMatCylinderOff();//顶料松开
					DelayRef(200);

					ReadMotosPos();
					if(g_workCtrl.workPos[MTX_IDX] < g_motosPara.motosPos[MTX_IDX])
					{
						g_optWorkCtrl.opitPosDirx = -1;
					}
					else if(g_workCtrl.workPos[MTX_IDX] > g_motosPara.motosPos[MTX_IDX])
					{
						g_optWorkCtrl.opitPosDirx = 1;
					}
					else
					{
						g_optWorkCtrl.opitPosDirx = 0;
					}
					if(g_workCtrl.workPos[MTY_IDX] < g_motosPara.motosPos[MTY_IDX])
					{
						g_optWorkCtrl.opitPosDiry = -1;
					}
					else if(g_workCtrl.workPos[MTY_IDX] > g_motosPara.motosPos[MTY_IDX])
					{
						g_optWorkCtrl.opitPosDiry = 1;
					}
					else
					{
						g_optWorkCtrl.opitPosDiry = 0;
					}

					MotoSpd spd;
					spd.stspSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->xymvstspSpd);
					spd.runSpd = XYAxisMMPSToPPS(g_pOpticalMcPara->maxRkOptSpd);
					spd.spdAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvSAdd);
					spd.brkAdd = XYAxisMMPSSToPPSG(g_pOpticalMcPara->xymvbrkAdd);

					err = MoveXYRS1P1ToWorkPos(pDat, &spd, 0, 1);

					if(err != ERR_NONE)
					{
						ReportError(err);
						break;
					}

					DelayRef(50);
					ReadMotosPos();
					if(g_optWorkCtrl.opitPosDirx == 1)
					{
						g_optWorkCtrl.opitPosx = g_motosPara.motosPos[MTX_IDX] + g_pOpticalMcPara->opitEndPosOffset;
					}
					else
					{
						g_optWorkCtrl.opitPosx = g_motosPara.motosPos[MTX_IDX];
					}
					if(g_optWorkCtrl.opitPosDiry == 1)
					{
						g_optWorkCtrl.opitPosy = g_motosPara.motosPos[MTY_IDX] + g_pOpticalMcPara->opitEndPosOffset;
					}
					else if(g_optWorkCtrl.opitPosDiry == -1)
					{
						g_optWorkCtrl.opitPosy = g_motosPara.motosPos[MTY_IDX] - g_pOpticalMcPara->opitEndPosOffset;
					}
					else
					{
						g_optWorkCtrl.opitPosy = g_motosPara.motosPos[MTY_IDX];
					}

					g_optWorkCtrl.offsetToOpit = 0;

					//回退一下，松一下线
					if(curDat.dx == 0)
					{
						tempx = 0;
					}
					else
					{
						tempx = -1;
					}

					if(curDat.dy == 0)
					{
						tempy = 0;
					}
					else
					{
						tempy = -1;
					}
					temp = GetSyntheticLength(curDat.dx, curDat.dy);
					tempx = tempx * curDat.dx * ((float)(g_pOpticalMcPara->offsetToSpinBack) / (float)(temp));
					tempy = tempy * curDat.dy * ((float)(g_pOpticalMcPara->offsetToSpinBack) / (float)(temp));

					FixMatCylinderOn();//顶料夹住
					DelayRef(200);

					err = MoveXYAsOffset(tempx, tempy, NULL);

					if(err != ERR_NONE)
					{
						ReportError(err);
						break;
					}

					DelayRef(200);
					UpClipMatClipCylinderOff();//上夹料松开
				}

				if (err != ERR_NONE)
				{
					ReportError(err);
					break;
				}
			}
			//前一根布线结束移到下一根布线起始（判定：偏移数据前是吐丝数据，后是吐丝数据）
			else if(oldDataForOffset == DATA_OPTI  && tmpDat.ctrl == DATA_OPTI)
			{

//				MoveXYToPos(-1100, -6000, NULL);

/*				if(g_workCtrl.workPos[MTX_IDX] > 65000 ||
						g_workCtrl.workPos[MTY_IDX] > 44000)
				{
					MoveXYToPos(70700, 64000, NULL);
				}
				else
				{
					MoveXYToPos(-1100, -6000, NULL);
				}*/
				err = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头
				ReadMotosPos();
				if(g_workCtrl.workPos[MTX_IDX] <= g_motosPara.motosPos[MTX_IDX])
				{
					g_workCtrl.workPos[MTX_IDX] -= g_pOpticalMcPara->startPosoffx;
					g_optWorkCtrl.usecomx = 1;
				}
				if(g_workCtrl.workPos[MTY_IDX] <= g_motosPara.motosPos[MTY_IDX])
				{
					g_workCtrl.workPos[MTY_IDX] -= g_pOpticalMcPara->startPosoffy;
					g_optWorkCtrl.usecomy = 1;
				}
				if (err == ERR_NONE)
				{
					err = MoveXYRS1P1ToWorkPos(pDat, NULL, 1, 0);
				}

				if(g_optWorkCtrl.usecomx == 1 ||
					g_optWorkCtrl.usecomy == 1)
				{
					if(g_optWorkCtrl.usecomx == 1)
					{
						g_workCtrl.workPos[MTX_IDX] += g_pOpticalMcPara->startPosoffx;
						g_optWorkCtrl.usecomx = 0;
					}
					if(g_optWorkCtrl.usecomy == 1)
					{
						g_workCtrl.workPos[MTY_IDX] += g_pOpticalMcPara->startPosoffy;
						g_optWorkCtrl.usecomy = 0;
					}
					if (err == ERR_NONE)
					{
						err = MoveXYRS1P1ToWorkPos(pDat, NULL, 1, 0);
					}
				}
//				err = MoveXYRS1P1ToWorkPos(pDat, NULL, 1, 0);
				printf("spin star actpos=(%d,%d)\r\n", g_motosPara.motosPos[MTX_IDX],g_motosPara.motosPos[MTY_IDX]);
				if(err != ERR_NONE)
				{
					ReportError(err);
					break;
				}
			}
			//吐丝结束后偏移到布线位置
			else
			{
				FixMatCylinderOn();//顶料顶料
				DelayRef(200);
				ReadMotosPos();		//起始点到偏移的位置和移动到起始点反向增加间隙补偿

				if(g_workCtrl.workPos[MTX_IDX] < g_motosPara.motosPos[MTX_IDX])
				{
					g_workCtrl.workPos[MTX_IDX] += g_pOpticalMcPara->xGapCorrect;
					g_optWorkCtrl.useGapCorx = 1;
				}
				if(g_workCtrl.workPos[MTY_IDX] < g_motosPara.motosPos[MTY_IDX])
				{
					g_workCtrl.workPos[MTY_IDX] += g_pOpticalMcPara->yGapCorrect;
					g_optWorkCtrl.useGapCory = 1;
				}
				err = MoveR1ToPos(PitkToRpp(g_workCtrl.workPos[MTR1_IDX]),NULL);

				if (err == ERR_NONE)
				{
					err = MoveU1ToPos(g_pOpticalMcPara->rkLowPos, NULL);//机头下降
				}

				if (err == ERR_NONE)
				{
					err = MoveXYRS1P1ToWorkPos(pDat, NULL, 0, 0);
				}
				g_optWorkCtrl.lowingFlag = 1;

				if (err != ERR_NONE)
				{
					ReportError(err);
					break;
				}
			}

			g_optWorkCtrl.IsNoMaterialEn = 1;
		}

		else if (curDat.ctrl == DATA_NULL || curDat.ctrl == DATA_END)
		{
			rslt = OpticalCloseAll();

			if (rslt == ERR_NONE && mode == WKMD_NORMAL && g_optWorkCtrl.motopreFlag == 1)          //最后一下剪线
			{
				//布线结束后的动作
				DelayRef(300);
				UpClipMatClipCylinderOff();//上夹料松开
				err = IsSignalTimeout(UpClipMatClipCylinderPlaceOff, CYLINDER_TIMEOUT, ERR_UPCLIP_ERROR);//等上夹料回位传感器亮起

				if(err == ERR_NONE)
				{
					FixMatCylinderOn();//顶料顶料
					PreMatCylinderOff();//压料气缸松开
					DelayRef(100);
				}

				if(err == ERR_NONE)
				{
					err = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头
				}

				err = IsSignalTimeout(IsOptPreLiftUp, CYLINDER_TIMEOUT, ERR_UPCLIP_ERROR);//等上夹料回位传感器亮起
				g_optWorkCtrl.motopreFlag = 0;

				//结束回结束点
				rslt = OpticalGotoFinishPos();
				if (rslt == ERR_NONE)
				{
					err = STA_WORK_FINISH;	// 工作完成
					SetDatIdxToStart();
					SP = 0;
				}
				else
				{
					err = rslt;
				}
			}
			else
			{
				//结束回结束点
				rslt = OpticalGotoFinishPos();
				if (rslt == ERR_NONE)
				{
					err = STA_WORK_FINISH;	// 工作完成
					SetDatIdxToStart();
					SP = 0;
				}
				else
				{
					err = rslt;
				}

				err = rslt;
				if (rslt == ERR_NONE)
				{
					err = STA_WORK_FINISH;	// 工作完成
					SetDatIdxToStart();
					SP = 0;
				}
			}

			break;
		}
		else
		{
			// 跳过
			memcpy(&oldDat, &curDat, sizeof(DataItem));

//			if (mode == WKMD_BACKWORD)		// 回退
//			{
//				rslt = DatIndexDec();11
//			}
//			else
//			{
//				rslt = DatIndexInc();
//			}
		}

		// 完成，结束，错误，暂停
		if (err != ERR_NONE)
		{
			OpticalCloseAll();
			UpClipMatClipCylinderOn();			// 上夹料夹

			if(g_optWorkCtrl.opticalSta == 1)//断线检测报警
			{
				err = ERR_NO_MATERIAL_ERROR;
				g_optWorkCtrl.opticalSta = 0;
			}

			ReportError(err);
			printf("err ok or pause\r\n");

//			//暂停
//			if(err == STA_NORMAL_STOP)
//			{
//				printf("err ok or pause1\r\n");
//				int rslt;
//				rslt = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头
//				if (rslt == ERR_NONE)
//				{
//					printf("err ok or pause2\r\n");
//					rslt = MoveR1ToPos(2500, NULL);//转到绝对坐标90°
//				}
//
//				if(rslt != ERR_NONE)
//				{
//					ReportError(rslt);
//				}
//			}

			break;
		}
	}while(1);



	if (err != ERR_NONE)		// 完成，结束，错误，暂停
	{
		if(g_optWorkCtrl.opticalSta == 1)//断线检测报警
		{
			err = ERR_NO_MATERIAL_ERROR;
			g_optWorkCtrl.opticalSta = 0;
		}

		OpticalCloseAll();
		UpClipMatClipCylinderOn();			// 上夹料开

		ReportError(err);
//			printf("err ok or pause\r\n");
//		if(err == STA_NORMAL_STOP)
//		{
//			int rslt;
//			rslt = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头
//			if (rslt == ERR_NONE)
//			{
//				printf("err ok or pause3\r\n");
//				rslt = MoveR1ToPos(2500, NULL);//转到绝对坐标90°
//			}
//
//			if(rslt != ERR_NONE)
//			{
//				ReportError(rslt);
//			}
//		}
	}
	else
	{
		OpticalCloseAll();
	}


	if (err == ERR_NONE || 			// 完成
		err == STA_WORK_OVER || 	// 工作结束
		err == STA_WORK_FINISH ||	// 工作完成
		err == STA_WORK_DONE ||		// 完成产量
		0	)
	{
		int idx,item;
		OpticalCloseAll();
		idx = GetDatIndex();
		item = GetFileItemNums();

		printf("idx = %d,item = %d err=%x\r\n", idx, item, err);

		if (idx > (item - 3))
		{
			g_workCtrl.cursta.outCounter++;	// 产量增加
			g_workCtrl.savedpara.outCounter = g_workCtrl.cursta.outCounter;
			SaveWorkNvPara();

			printf("outCounter = %d \r\n",g_workCtrl.cursta.outCounter);
		}
	}
#if (XY_GAP_CONVERT == 1)
	MoveXYGap(0,0,NULL);		// 停车补偿
#endif
	if (err == STA_WORK_OVER || 	// 工作结束
		err == STA_WORK_FINISH ||	// 工作完成
		err == STA_WORK_DONE ||		// 完成产量
		0 )
	{
		OpticalCloseAll();
		SetDatIdxToStart();
		err = OpticalGotoFinishPos();
	}


	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY|WORK_STA_WORKING);		// 清除正在忙标志

	ReportError(err);
	return err;
}
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
void AllToZero(void)
{
	int rslt;

	rslt = ERR_NONE;

	g_optWorkCtrl.onceflag = 0;                  // 一次性执行标记
	g_optWorkCtrl.motoRunFlag = 0;               // 吐丝标志
	g_optWorkCtrl.cutingFlag = 0;	             // 已经剪线标志
	g_optWorkCtrl.motopreFlag = 0;	             // 已经压料标志
//	g_optWorkCtrl.lowingFlag = 1;				 // 需要下降机头标志

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	ReportError(0);
	DelayRef(10);
	FixMatCylinderOn();//顶料夹住
	PreMatCylinderOff();

#if (1)
	enum Motosta
	{
		U1MotoSta= 1,
		R1MotoSta,
		XMotoSta,
		YMotoSta,
		EndSta
	}motosta;

	motosta = 1;

	while(1)
	{
		switch(motosta)
		{
#if(LIFT_U1 != 0)
			case U1MotoSta:
			{
				rslt = FindU1Zero(1);
				break;
			}
#endif

#if(TURN_R1 != 0)
			case R1MotoSta:
			{
				rslt = FindR1Zero();
				break;
			}
#endif

#if (MODULE_X != 0)
			case XMotoSta:
			{
				rslt = FindXZero();
				break;
			}
#endif

#if ((MODULE_X != 0) && (MODULE_Y != 0))
			case YMotoSta:
			{
				rslt = FindYZero(-1);
				break;
			}
#endif

			default:
			{
				break;
			}
		}

		if (rslt == STA_MTZ_SUCCESS)
		{
			motosta++;
			DelayRef(100);
			if (motosta == EndSta)
			{
				break;
			}
		}
		else
		{
			break;
		}
	}
#endif
	rslt = IsSignalTimeout(IsOptPreLiftUp, CYLINDER_TIMEOUT, 0x002F);//压料气缸回位传感器亮起
	ReportError(rslt);
}


// 流程复位
void AllToReset(void)
{
	int err = ERR_NONE;

	g_workCtrl.cursta.runSteps = 0;
	g_workCtrl.cursta.dataIdx = 0;

	g_workCtrl.savedpara.runSteps = g_workCtrl.cursta.runSteps;
	g_workCtrl.savedpara.dataIdx = g_workCtrl.cursta.dataIdx;
	SaveWorkNvPara();

	g_optWorkCtrl.onceflag = 0;                  // 一次性执行标记
	g_optWorkCtrl.motoRunFlag = 0;               // 吐丝标志
	g_optWorkCtrl.cutingFlag = 0;	             // 已经剪线标志
	g_optWorkCtrl.motopreFlag = 0;	             // 已经压料标志
//	g_optWorkCtrl.lowingFlag = 1;				 // 需要下降机头标志

	err = MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头

	if (err == ERR_NONE)
	{
		err = MoveR1ToPos(2500, NULL);//转到绝对坐标90°
		if (err == ERR_NONE)
		{
			err = OpticalGotoFinishPos();	// 回结束点
		}
	}

	if(err != ERR_NONE)
	{
		ReportError(err);
		return;
	}

	if (err == ERR_NONE)
	{
		err = STA_PROCESS_FINISH;		// 执行完成
		ReportError(err);
	}
}

// 检测
void OpticalCheckSensorAndPos(void)
{
	if (g_pOpticalMcPara->autoFindZero != 0) 	// 开机自动找零	: 0, 不使能 1, 使能
	{
		g_workCtrl.posEnFlag = 0;
		//OpticalAllToReset();
	}
}

int IsInManualModel(void)
{
	return 0;
}

void SetLightStaByCode(int errCode)
{
	if ((g_workCtrl.cursta.workStatus & (WORK_STA_BUSY|WORK_STA_WORKING)) != 0)
	{
		g_workCtrl.almSta = 0;
	}

}

void SetLightSta(void)
{

//	FLASH_LIGHT(g_workCtrl.almflashRed, g_workCtrl.almredSta, AlarmRedLightOn, AlarmRedLightOff);			// 三色灯红灯
//	FLASH_LIGHT(g_workCtrl.almflashGreen, g_workCtrl.almgreenSta, AlarmGreenLightOn, AlarmGreenLightOff);	// 三色灯绿灯
//	FLASH_LIGHT(g_workCtrl.almflashYello, g_workCtrl.almyelloSta, AlarmYelloLightOn, AlarmYelloLightOff);	// 三色灯黄灯
//	FLASH_LIGHT(g_workCtrl.almflashBee, g_workCtrl.almbeeSta, AlarmBeeOn, AlarmBeeOff);						// 蜂鸣器

}

//回起点
int GotoStartPos(void)
{
	int err;
	err = ERR_NONE;
#if (OPTICAL_MC_TYPE == OPTICAL_MC_SINGLE || OPTICAL_MC_TYPE == OPTICAL_MC_ONE || OPTICAL_MC_TYPE == OPTICAL_MC_TWO || OPTICAL_MC_TYPE == OPTICAL_MC_WIRE)

	err = IsAutoMoveEnable();	// 回起点前判断

	// 文件是否有效
	if (err == ERR_NONE &&
		IsDatFileAvailable() != TRUE)	// 文件有效
	{
		err = ERR_NO_DATA;	// 无数据文件
		ReportError(err);
		return err;
	}

	SetDatIdxToStart();			// 数据回起点

	if (err == ERR_NONE)
	{
		g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
		ReportError(0);
		DelayRef(10);

#if ((MODULE_X != 0) &&  (MODULE_Y != 0)  && (TURN_R1 != 0))
		err = MoveXYRS1P1ToWorkPos(NULL, NULL, 0, 0);
#endif
	}

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

#endif
	ReportError(err);
	return err;
}

// 去定位点
int GotoAnchorPos(void)
{
	int err;

	err = ERR_NONE;

#if (OPTICAL_MC_TYPE == OPTICAL_MC_WIRE )
	{
#if (MODULE_X != 0)
		int posx;
#endif

#if ((MODULE_X != 0) && (MODULE_Y != 0))
		int posy;
#endif

#if ((MODULE_X != 0) && (MODULE_Y != 0))
		int posr;
#endif
		int  poss1;
		int  posp1;

		err = IsAutoMoveEnable();	// 回定位点前判断

		// 文件是否有效
		if (err == ERR_NONE &&
			IsDatFileAvailable() != TRUE)	// 文件有效
		{
			err = ERR_NO_DATA;	// 无数据文件
			ReportError(err);
			return err;
		}
		if (err == ERR_NONE)
		{
			err = MoveU1ToUpper(0);		// 抬起机头
		}
		if (err == ERR_NONE)
		{
			g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
			ReportError(0);
			DelayRef(10);

#if (MODULE_X != 0)
			posx = g_datFile.pFileHead->fileHead.anchorX - g_pOpticalMcPara->offsetX;
			printf("g_datFile.pFileHead->fileHead.anchorX =%d\r\n",g_datFile.pFileHead->fileHead.anchorX);
#endif

#if ((MODULE_X != 0) && (MODULE_Y != 0))
			posy = g_datFile.pFileHead->fileHead.anchorY - g_pOpticalMcPara->offsetY;	// 定位点;
			printf("g_datFile.pFileHead->fileHead.anchorY =%d\r\n",g_datFile.pFileHead->fileHead.anchorY);
#endif


#if ((MODULE_X != 0) &&  (MODULE_Y != 0)  && (TURN_R1 != 0))
			posr = 0;
#endif
			poss1 = 0;
			posp1 = 0;
			err = OpticalCloseAll();
			if (err == ERR_NONE)
			{
#if ((MODULE_X != 0) &&  (MODULE_Y != 0) && (TURN_R1 != 0))
				err = MoveXYR1S1P1ToPos(posx, posy, posr, poss1, posp1, NULL, 0);
#endif
			}
		}

		g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志
	}

#endif
	ReportError(err);
	return err;
}

// 去工作点
int GotoWorkPos(void)
{
	int err;
	err = ERR_NONE;

#if (OPTICAL_MC_TYPE == OPTICAL_MC_SINGLE || OPTICAL_MC_TYPE == OPTICAL_MC_ONE || OPTICAL_MC_TYPE == OPTICAL_MC_TWO || OPTICAL_MC_TYPE == OPTICAL_MC_WIRE)
	{
		int rslt;
		DataItem curDat;

		g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
		//g_workCtrl.opticalStatus |= CUTTER_RUNNING;			// 正在运行标志

		ReportError(0);
		DelayRef(10);

		ReadAndFreshMotosPos();
		err = IsAutoMoveEnable();	// 回工作点前判断

		if (err == ERR_NONE)
		{
			rslt = GetADataItem(0, GetDatIndex(), &curDat);
			if (rslt != 0)
			{
				memset(&curDat, 0, sizeof(DataItem));
			}

			err = MoveXYRS1P1ToWorkPos(&curDat, NULL, 0, 0);
		}

		//g_workCtrl.opticalStatus &= (~CUTTER_RUNNING);		// 正在运行标志
		g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);	// 清除正在忙标志
	}

#endif
	ReportError(err);
	return err;
}

// 回偏移点
int GotoOffsetPos(void)
{
	int err;
	err = ERR_NONE;
	FixMatCylinderOn();      //防止光纤回退


#if (OPTICAL_MC_TYPE == OPTICAL_MC_WIRE)
	{
		int posx, posy, posr1, poss1, posp1;
		g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
		//g_workCtrl.opticalStatus |= CUTTER_RUNNING;			// 正在运行标志

		ReportError(0);
		DelayRef(10);

		ReadAndFreshMotosPos();
		err = IsAutoMoveEnable();	// 回偏移点前判断

		if (err == ERR_NONE)
		{
			if ((g_pOpticalMcPara->offsetEn & POS_EN_X) != 0)
			{
				posx = g_pOpticalMcPara->offsetX;	// 偏移点X
			}
			else
			{
				posx = g_motosPara.motosPos[MTX_IDX];
			}

			if ((g_pOpticalMcPara->offsetEn & POS_EN_Y) != 0)
			{
				posy = g_pOpticalMcPara->offsetY;	// 偏移点Y
			}
			else
			{
				posy = g_motosPara.motosPos[MTY_IDX];
			}

			err = OpticalCloseAll();

			posr1 = 0;	// ROT_001DEG_TO_PULSE(g_pOpticalMcPara->rSensorAng);
			poss1 = 0;
			posp1 = 0;

			if (err == ERR_NONE)
			{
				err = MoveXYR1S1P1ToPos(posx, posy, posr1, poss1, posp1, NULL, 0);
			}
		}

		//g_workCtrl.opticalStatus &= (~CUTTER_RUNNING);		// 正在运行标志
		g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);	// 清除正在忙标志
		if (err == ERR_NONE)
		{
			printf("send cutprossflag = 0\r\n");
		}
	}
#endif
	ReportError(err);

	return err;
}
int GotoFinishPos(void)
{
	int err;
	err = OpticalGotoFinishPos();
	return err;
}
// 去结束点
int OpticalGotoFinishPos(void)
{

	if (g_pOpticalMcPara->finishStopSel == 1)	// 回起点
	{
		return GotoStartPos();
	}
	else if (g_pOpticalMcPara->finishStopSel == 2)	// 定位点
	{
		return GotoAnchorPos();
	}
	else if (g_pOpticalMcPara->finishStopSel == 3)	// 上料点
	{
		//return OpticalGotoPullBegPos();
	}
	else if (g_pOpticalMcPara->finishStopSel == 4)	// 偏移点
	{
		printf("goto offset when finish\r\n");
		return GotoOffsetPos();
	}
	else if (g_pOpticalMcPara->finishStopSel == 5)	// 归零点
	{
		return MoveXYToPos(g_pOpticalMcPara->xSensorPos, g_pOpticalMcPara->ySensorPos, NULL);
		printf("goto AllToZero when finish\r\n");

	}
	return ERR_NONE;
}

// 前进回退(针迹索引)
int FastJump(u32 idx)
{
	int err;
	err = ERR_NONE;
	g_optWorkCtrl.cutingFlag = 0;	             // 已经剪线标志

	err = MoveU1ToUpper(0);		// 抬起机头

	g_workCtrl.cursta.workStatus |= WORK_STA_BUSY;		// 正在忙标志
	ReportError(err);
	DelayRef(10);

	printf("jump to file idx=%d, stitches=%d\r\n", idx, GetFileItemNums());
	if (IsDatFileAvailable() != TRUE)	// 文件有效
	{
		printf("no file\r\n");
		err = ERR_NO_DATA;		// 无数据文件
	}
	else
	{
		int rslt;
		if (GetFileItemNums() > 0)
		{
			if (idx > GetFileItemNums())
			{
				idx = GetFileItemNums() - 1;
			}
		}
		else
		{
			idx = 0;
		}

		g_workCtrl.cursta.dataIdx = idx;
		printf("act jump to file idx=%d\r\n", idx);

		if (g_workCtrl.cursta.dataIdx != 0)
		{// 向前找到吐丝数据
			DataItem chackDat;
	  		do
			{
	  			GetADataItem(0, g_workCtrl.cursta.dataIdx, &chackDat);

				if(chackDat.ctrl == DATA_OPTI)                  //判断是否在吐丝处偏移
				{
					break;
				}
				else
				{
					g_workCtrl.cursta.dataIdx--;
					if (g_workCtrl.cursta.dataIdx == 0)
					{
						break;
					}
				}
			}while(1);
			printf("find near opti data to file idx=%d\r\n", g_workCtrl.cursta.dataIdx);
		}

		g_workCtrl.savedpara.dataIdx = g_workCtrl.cursta.dataIdx;
		SaveWorkNvPara();

		rslt = GetWorkPosFromIndex();
		if (rslt == 0)
		{
			DataItem curDat;

			printf("before move to work\r\n");

			rslt = GetADataItem(0, GetDatIndex(), &curDat);
			if (rslt != 0)
			{
				err = STA_WORK_OVER;	// 结束
			}
			else
			{
				err = MoveXYRS1P1ToWorkPos(&curDat, NULL, 0, 0);
			}
			printf("after move to work\r\n");
		}
	}

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY);		// 清除正在忙标志

	ReportError(err);
	return err;
}

int StopAndLightCheck(int sdflag)
{
	int rslt = ERR_NONE;

	if (rslt == ERR_NONE)
	{
		if (IsStopButton() == SENSOR_ON)
		{
			//printf("stop button\r\n");
			rslt = STA_NORMAL_STOP;
		}
	}

	if (rslt == ERR_NONE)
	{
		rslt = OpticalLightCheck();
	}

	if (sdflag != 0)
	{
		if (rslt == ERR_NONE)
		{
			Cutter1StopOff();
			Cutter2StopOff();
		}
		else
		{
			Cutter1StopOn();
			Cutter2StopOn();
		}
	}
	return rslt;
}



// 工作控制
int OpticalStartWork(void)
{
	int err;

	err = ERR_NONE;

	g_workCtrl.cursta.workStatus |= (WORK_STA_BUSY|WORK_STA_WORKING);		// 正在忙标志
	ReportError(err);
	DelayRef(10);

	g_workCtrl.cursta.runSteps = 0;
	g_optWorkCtrl.onceflag = 0;                  // 一次性执行标记
	g_optWorkCtrl.motoRunFlag = 0;               // 吐丝标志
//	g_optWorkCtrl.cutingFlag = 0;	             // 已经剪线标志
	g_optWorkCtrl.motopreFlag = 0;	             // 已经压料标志
//	g_optWorkCtrl.lowingFlag = 1;				 // 需要下降机头标志

	err = OpticalWork(WKMD_NORMAL);		// 三合一裁床绘画切割，要确定N2完成与否

	g_workCtrl.cursta.workStatus &= ~(WORK_STA_BUSY|WORK_STA_WORKING);		// 清除正在忙标志


   ReportError(err);
   ReportError(ERR_NONE);
   return err;
}


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

// 上位机通讯命令

// 电机归零
void MotoToZero(u16 obj)
{
	{// 手动命令下如果遮挡光幕,则不执行动作
		int err;
		err = OpticalLightCheck();
		if (err != ERR_NONE)
		{
			ReportError(err);
			return;
		}
	}

	switch(obj)
	{

#if (OPTICAL_MC_TYPE == OPTICAL_MC_WIRE)
		case MT_LX:		// X向移动电机
		{
			FindXZero();
			break;
		}
		case MT_LY:		// y向移动电机
		{
			FindYZero(-1);
			break;
		}
		case MT_LU2:    //升降电机
		{
			FindU1Zero(1);
			break;
		}
		case MT_LR2:	// 跟踪电机
		{
			FindR1Zero();
			break;
		}
#endif
		default:
			break;
	}
}

// 电机手动移动
void MoveMotoByHand(u16 obj, u16 dir, u16 spd)
{
	FixMatCylinderOn();//顶料夹住，防止光纤收缩
	switch(obj)
	{
		case MT_LX:		// X向移动电机
		{
			if (dir == MT_MOVE_DIR_NEGA)		// 正向
			{
				MoveXByHand(1, spd);
			}
			else if (dir == MT_MOVE_DIR_POSI)
			{
				MoveXByHand(-1, spd);
			}
			else
			{
			}
			break;
		}

		case MT_LY:		// Y向移动电机
		{
			if (dir == MT_MOVE_DIR_NEGA)		// 正向
			{
				MoveYByHand(1, spd);
			}
			else if (dir == MT_MOVE_DIR_POSI)
			{
				MoveYByHand(-1, spd);
			}
			else
			{
			}
			break;
		}

		case MT_LU2:		// 升降电机
		{
			if (dir == MT_MOVE_DIR_POSI)		// 正向
			{
				MoveU1ByHand(1, spd);
			}
			else if (dir == MT_MOVE_DIR_NEGA)
			{
				MoveU1ByHand(-1, spd);
			}
			else
			{
			}
			break;
		}

		case MT_LR2:		// 跟踪电机
		{
			if (dir == MT_MOVE_DIR_POSI)		// 正向
			{
				MoveR1ByHand(1, spd);
			}
			else if (dir == MT_MOVE_DIR_NEGA)
			{
				MoveR1ByHand(-1, spd);
			}
			else
			{
			}
			break;
		}

		case MT_LBF:			// 送料电机
		{
			if (dir == MT_MOVE_DIR_POSI)		// 正向
			{
				MoveSByHand(1, spd);
			}
			else if (dir == MT_MOVE_DIR_NEGA)
			{
				MoveSByHand(-1, spd);
			}
			else
			{
			}
			break;
		}

		case MT_PBF:			// 拉料电机
		{
			if (dir == MT_MOVE_DIR_POSI)		// 正向
			{
				MovePByHand(1, spd);
			}
			else if (dir == MT_MOVE_DIR_NEGA)
			{
			//	UpClipMatClipCylinderOff();
				MovePByHand(-1, spd);
			}
			else
			{
			}
			//UpClipMatClipCylinderOn();
			break;
		}
		case MT_MBF:			// 松料电机
		{
			if (dir == MT_MOVE_DIR_POSI)		// 正向
			{
				LooseMotorRevOn();   //松料电机   转动
			//	printf("LooseMotor run %d\r\n");

			}
			else if (dir == MT_MOVE_DIR_NEGA)
			{
				LooseMotorRevOff();		// 松料电机  停止
			}
			else
			{
			}
			break;
		}

		default:
			break;
	}
	FixMatCylinderOff();
}

// 输出控制
void OutputByHand(u16 obj, u16 act, u16 para)
{
// 手动命令下如果遮挡光幕,则不执行动作
	int err;
	err = OpticalLightCheck();		//
	if (err != ERR_NONE)
	{
		ReportError(err);
		return;
	}

    err = ERR_NONE;

	switch(obj)
	{
		case OT_FAN1_CTRL:			// 吸风控制
		{
			if (act == UP_OPEN_ON)
			{
				Fan1MotoOn();			// 负压吸风开
			}
			else // if (act == DOWN_CLOSE_OFF)	// 关闭
			{
				Fan1MotoOff();			// 负压吸风关
			}
			break;
		}
		case OT_LOOSE:			// 松料电机运转
		{
			if (act == UP_OPEN_ON)
			{
				LooseMotorRevOn();			// 松料电机运转开
			//	printf("LooseMotor run %d\r\n");
			}
			else                	// 关闭
			{
				LooseMotorRevOff();			// 松料电机运转关
			}
			break;
		}

		case OT_TRIMMING:			// 剪线控制控制
		{
			if (act == UP_OPEN_ON)
			{
				UpClipMatClipCylinderOn();		// 上夹料开
				DelayRef(100);
				TrimmingCylinderOn();			// 剪线气缸开
			}
			else                	// 关闭
			{
				TrimmingCylinderOff();			// 剪线气缸关
			}
			break;
		}
		case OT_OWNCLIP:			// 下夹料
		{
			if (act == UP_OPEN_ON)
			{
				DownClipMatClipCylinderOn();			// 下夹料气缸开
			}
			else                	// 关闭
			{
				DownClipMatClipCylinderOff();			// 下夹料气缸关
			}
			break;
		}
		case OT_PULLINGCYLINDER:			// 拉料
		{
			if (act == UP_OPEN_ON)
			{
				IsPullingCylinderOn();			// 拉料气缸开
			}
			else                	// 关闭
			{
				IsPullingCylinderOff();			// 拉料气缸关
			}
			break;
		}
		case OT_PRESS_M_POLE:			// 压料
		{
			if (act == UP_OPEN_ON)
			{
				PreMatCylinderOff();			// 压料气缸上升
			}
			else                	// 关闭
			{
				PreMatCylinderOn();			// 压料气缸下降
			}
			break;
		}
		case OT_UPCLIP:			// 上夹料
		{
			if (act == UP_OPEN_ON)
			{
				UpClipMatClipCylinderOn();			// 上夹料气缸夹紧
			}
			else                	// 关闭
			{
				UpClipMatClipCylinderOff();			// 上夹料气缸关
			}
			break;
		}
		case OT_BLOWING:			// 放料
		{
			if (act == UP_OPEN_ON)
			{
				IsBlowingCylinderOn();			// 放料气缸开
			}
			else                	// 关闭
			{
				IsBlowingCylinderOff();			// 放料气缸关
			}
			break;
		}
		case OT_POINT:			// 去定位点
		{
			if (act == UP_OPEN_ON)
			{
			//	Point1(WKMD_NORMAL);			// 去定位点1
			}
			else
			{
			//	Point2(WKMD_NORMAL);			// 去定位点2
			}
			break;

		}
		case OT_FEED:			// 送料电机旋转
		{
			if (act == UP_OPEN_ON)
			{
				MoveSAsOffset(10000, NULL);			   // 正转
			}
			else
			{
				MoveSAsOffset(-10000, NULL);			// 反转
			}
			break;
		}
		case OT_PULL:			// 拉料电机旋转
		{
			if (act == UP_OPEN_ON)
			{
				MovePAsOffset(10000, NULL);			// 正转
			}
			else
			{
				MovePAsOffset(-10000, NULL);			// 反转
			}
			break;
		}
		case OT_HEAD_UPDOWN:			// 机头升降控制
		{

			if (act == UP_OPEN_ON)
			{
				MoveU1ToPos(g_pOpticalMcPara->rkHighPos, NULL);//抬起机头
				//MoveU1AsOffset(200, NULL);		// 机头上升
			}
			else // if (act == DOWN_CLOSE_OFF)	// 关闭
			{
				MoveU1ToPos(g_pOpticalMcPara->rkLowPos, NULL);//抬起机头
				//MoveU1AsOffset(-200, NULL);		// 机头下降
			}
			break;

		}
		case OT_HEAD_SPIN:			// 机头旋转控制
		{

			if (act == UP_OPEN_ON)
			{
				MoveR1AsOffset(200, NULL);		// 机头正向旋转
			}
			else // if (act == DOWN_CLOSE_OFF)	// 关闭
			{
				MoveR1AsOffset(-200, NULL);		// 机头反向旋转
			}
			break;

		}
		default:
			break;
	}
}

void MotosServoCtrl(u16 obj, u16 onoff)
{
	switch(obj)
	{
		case MT_LX:		// X向移动电机
		{
			if (onoff == MT_SERVO_ON)		// ON
			{
				MotoServoCtrl(MTX_IDX, SERVO_ON);
			}
			else if (onoff == MT_SERVO_OFF)	// OFF
			{
				MotoServoCtrl(MTX_IDX, SERVO_OFF);
			}
			break;
		}
		case MT_LY:		// y向移动电机
		{
			if (onoff == MT_SERVO_ON)		// ON
			{
				MotoServoCtrl(MTY_IDX, SERVO_ON);
			}
			else if (onoff == MT_SERVO_OFF)	// OFF
			{
				MotoServoCtrl(MTY_IDX, SERVO_OFF);
			}
			break;
		}
		case MT_LU1:		// 升降电机
		{
			if (onoff == MT_SERVO_ON)		// ON
			{
				MotoServoCtrl(MTY_IDX, SERVO_ON);
			}
			else if (onoff == MT_SERVO_OFF)	// OFF
			{
				MotoServoCtrl(MTY_IDX, SERVO_OFF);
			}
			break;
		}

		case MT_LR1:		// 跟踪旋转电机
		{
			if (onoff == MT_SERVO_ON)		// ON
			{
				MotoServoCtrl(MTY_IDX, SERVO_ON);
			}
			else if (onoff == MT_SERVO_OFF)	// OFF
			{
				MotoServoCtrl(MTY_IDX, SERVO_OFF);
			}
			break;
		}

		default:
			break;
	}
}

// 手动动作
void OpticalManualAction(u16 obj, u16 para)
{


	switch(obj)
	{


	default:
		break;
	}

}


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

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// 调试测试

#if (1)

void TestWorkRun(char * para1, char * para2)
{
	int p1, p2;

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

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

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

	if (p1 == p2)
	{
	}

	//--------------------------------
	if (p1 == 0)
	{//
		OpticalStartWork();		// 启动工作
	}
	else if (p1 == 1)
	{
		OpticalWork(WKMD_NORMAL);		//
	}
	else if (p1 == 2)					//去结束点
	{
		OpticalGotoFinishPos();
	}
	else if (p1 == 3)
	{
	}

	else
	{

	}
}

void TestMotoRun(char * para1, char * para2)
{
	int p1, p2;

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

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

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

	if (p1 == p2)
	{
	}

	//--------------------------------
	if (p1 == 0)
	{//电机归零
		if (p2 == 0)
		{//全部归零
			AllToZero();
		}
		else if (p2 == 1)
		{// X电机
			FindXZero();
		}
		else if (p2 == 2)
		{// Y电机
			FindYZero(-1);
		}

		else if (p2 == 5)
		{
			FindU1Zero(1);
		}
		else if (p2 == 6)
		{
			FindR1Zero();
		}
		else if (p2 == 7)
		{
		}
		else if (p2 == 8)
		{

		}
		else if (p2 == 9)
		{
		}
	}
		else if (p1 == 101)
		{// X电机偏移
			MoveXAsOffset(p2,NULL);
		}
		else if (p1 == 102)
		{// Y电机偏移
			MoveYAsOffset(p2,NULL);
		}
		else if (p1 == 103)
		{// 升降电机偏移
			MoveU1AsOffset(p2, NULL);
		}
		else if (p1 ==104)
		{//跟踪电机偏移
			MoveR1AsOffset(p2, NULL);
		}
		else if (p1 ==105)
		{//送料电机偏移
			MoveSAsOffset(p2, NULL);
		}
		else if (p1 ==106)
		{//拉料电机偏移
			MovePAsOffset(p2, NULL);
		}
		else if (p1 ==107)
		{//升降拉料电机偏移
			MoveU1S1AsOffset(p2, NULL);
		}
		else if (p1 ==108)
		{//上夹料拉料电机偏移
			MoveP1S1AsOffset(p2, NULL);
		}



		else if (p1 == 201)
		{//升降电机去绝对值位置
			MoveU1ToPos(p2, NULL);
		}
		else if (p1 == 202)
		{//升降电机去绝对值位置
			MoveP1S1AsOffset(p2, NULL);
		}

	else if (p1 == 1)
	{
		if (p2 == 0)
		{
			MoveMotoByHand(MT_LX, MT_MOVE_DIR_POSI, 1);
		}
		else
		{
			MoveMotoByHand(MT_LX, MT_MOVE_DIR_NEGA, 1);
		}
	}
	else if (p1 == 2)
	{
		if (p2 == 0)
		{
			MoveMotoByHand(MT_LY, MT_MOVE_DIR_POSI, 1);
		}
		else
		{
			MoveMotoByHand(MT_LY, MT_MOVE_DIR_NEGA, 1);
		}
	}
	else if (p1 == 3)
	{
		if (p2 == 0)
		{
		}
		else if (p2 == 1)
		{

		}
		else if (p2 == 2)
		{// 右刀高度测试

		}
	}

	else
	{

	}
}

void XMotoRunTo(char * para1, char * para2)
{
	int p1, p2;

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

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

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

	if (p1 == p2)
	{
	}

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

	int err = OpticalCloseAll();
	if (err == ERR_NONE)
	{
		err = MoveXToPos(p1,NULL);
	}

}

void YMotoRunTo(char * para1, char * para2)
{
	int p1;

	if (para1 == NULL)
	{
		return;
	}

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

	if (strcmp(para1, "") == 0)
	{
		return;
	}
	p1 = atoi(para1);

	//--------------------------------
	MoveYToPos(p1, NULL);
}

//-------------------------------------------------------
void XYMotoRunTo(char * para1, char * para2)
{
	int p1, p2;

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

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

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

	if (p1 == p2)
	{
	}

	//--------------------------------
	{
		int err = OpticalCloseAll();
		if (err == ERR_NONE)
		{
			err = MoveXYToPos(p1, p2, NULL);
		}
	}
}

void U1MotoRunTo(char * para1, char * para2)
{
	int p1;

	if (para1 == NULL)
	{
		return;
	}

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

	if (strcmp(para1, "") == 0)
	{
		return;
	}
	p1 = atoi(para1);

	//--------------------------------
	MoveU1ToPos(p1, NULL);
}

void R1MotoRunTo(char * para1, char * para2)
{
	int p1;

	if (para1 == NULL)
	{
		return;
	}

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

	if (strcmp(para1, "") == 0)
	{
		return;
	}
	p1 = atoi(para1);

	//--------------------------------
	MoveR1ToPos(p1, NULL);
}
void S1MotoRunTo(char * para1, char * para2)
{
	int p1;

	if (para1 == NULL)
	{
		return;
	}

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

	if (strcmp(para1, "") == 0)
	{
		return;
	}
	p1 = atoi(para1);

	//--------------------------------
	MoveSToPos(p1, NULL);
}
void P1MoveAsOffset(char * para1, char * para2)
{
	int p1;

	if (para1 == NULL)
	{
		return;
	}

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

	if (strcmp(para1, "") == 0)
	{
		return;
	}
	p1 = atoi(para1);

	//--------------------------------
	MovePAsOffset(p1, NULL);
}


void ShowMotoInfo(char * para1, char * para2)
{
	printf("motos info:\r\n");
	printf("\tXpos=%d\r\n", g_motosPara.motosPos[MTX_IDX]);
	printf("\tYpos=%d\r\n", g_motosPara.motosPos[MTY_IDX]);
	printf("\tRpos=%d\r\n", g_motosPara.motosPos[MTR1_IDX]);
	printf("\tUpos=%d\r\n", g_motosPara.motosPos[MTU1_IDX]);
	printf("\tPpos=%d\r\n", g_motosPara.motosPos[MTP1_IDX]);
	printf("\tSpos=%d\r\n", g_motosPara.motosPos[MTS1_IDX]);
}



//----------------------------------------
//刷新传感器信号
// mode = 0	: 刷新传感器的状态
// mode = 1	: 得到有效传感器的位图

#define	CASE_COREIP(cs)	\
{\
	case cs:\
	{\
		if (((GetInput##cs##Status() == SENSOR_ON) || (mode != 0)) && (CORE_INPUT##cs##_IDX != DSS_DEF_IDX))\
		{\
			buf[CORE_INPUT##cs##_IDX] |= CORE_INPUT##cs##_BIT;\
		}\
	break;\
	}\
}

#define	CASE_EXIP(cs)	\
{\
	case cs:\
	{\
		if (((GetExInput(cs) == SENSOR_ON) || (mode != 0)) && (EX_INPUT##cs##_IDX != DSS_DEF_IDX))\
		{\
			buf[EX_INPUT##cs##_IDX] |= EX_INPUT##cs##_BIT;\
		}\
	break;\
	}\
}

typedef char InStr[INPUT_STR_LEN];


// mode	1:位图
//		0:信号
void SensorsStatus(int mode, u8 * buf)
{
	int i;

	if (buf == NULL)
	{
		return;
	}

	memset(buf, 0, MAX_EXDP_LEN);

	for (i=1;i<=16;i++)
	{
		switch(i)
		{
		CASE_COREIP(1);
		CASE_COREIP(2);
		CASE_COREIP(3);
		CASE_COREIP(4);
		CASE_COREIP(5);
		CASE_COREIP(6);
		CASE_COREIP(7);
		CASE_COREIP(8);
		CASE_COREIP(9);
		CASE_COREIP(10);
		CASE_COREIP(11);
		CASE_COREIP(12);
		CASE_COREIP(13);
		CASE_COREIP(14);
		CASE_COREIP(15);
		CASE_COREIP(16);
		default:
			break;
		}
	}

#if (USE_IOEX != 0)
	{
#if (USE_IOEX >= 1)
		for (i=1;i<=16;i++)
		{
			switch(i)
			{// N7 (1)
			CASE_EXIP(1);
			CASE_EXIP(2);
			CASE_EXIP(3);
			CASE_EXIP(4);
			CASE_EXIP(5);
			CASE_EXIP(6);
			CASE_EXIP(7);
			CASE_EXIP(8);
			CASE_EXIP(9);
			CASE_EXIP(10);
			CASE_EXIP(11);
			CASE_EXIP(12);
			CASE_EXIP(13);
			CASE_EXIP(14);
			CASE_EXIP(15);
			CASE_EXIP(16);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 2)
		for (i=17;i<=32;i++)
		{
			switch(i)
			{// N7 (2)
			CASE_EXIP(17);
			CASE_EXIP(18);
			CASE_EXIP(19);
			CASE_EXIP(20);
			CASE_EXIP(21);
			CASE_EXIP(22);
			CASE_EXIP(23);
			CASE_EXIP(24);
			CASE_EXIP(25);
			CASE_EXIP(26);
			CASE_EXIP(27);
			CASE_EXIP(28);
			CASE_EXIP(29);
			CASE_EXIP(30);
			CASE_EXIP(31);
			CASE_EXIP(32);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 3)
		for (i=33;i<=48;i++)
		{
			switch(i)
			{// N7 (3)
			CASE_EXIP(33);
			CASE_EXIP(34);
			CASE_EXIP(35);
			CASE_EXIP(36);
			CASE_EXIP(37);
			CASE_EXIP(38);
			CASE_EXIP(39);
			CASE_EXIP(40);
			CASE_EXIP(41);
			CASE_EXIP(42);
			CASE_EXIP(43);
			CASE_EXIP(44);
			CASE_EXIP(45);
			CASE_EXIP(46);
			CASE_EXIP(47);
			CASE_EXIP(48);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 4)
		for (i=49;i<=64;i++)
		{
			switch(i)
			{// N7 (4)
			CASE_EXIP(49);
			CASE_EXIP(50);
			CASE_EXIP(51);
			CASE_EXIP(52);
			CASE_EXIP(53);
			CASE_EXIP(54);
			CASE_EXIP(55);
			CASE_EXIP(56);
			CASE_EXIP(57);
			CASE_EXIP(58);
			CASE_EXIP(59);
			CASE_EXIP(60);
			CASE_EXIP(61);
			CASE_EXIP(62);
			CASE_EXIP(63);
			CASE_EXIP(64);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 5)
		for (i=65;i<=80;i++)
		{
			switch(i)
			{// N7 (5)
			CASE_EXIP(65);
			CASE_EXIP(66);
			CASE_EXIP(67);
			CASE_EXIP(68);
			CASE_EXIP(69);
			CASE_EXIP(70);
			CASE_EXIP(71);
			CASE_EXIP(72);
			CASE_EXIP(73);
			CASE_EXIP(74);
			CASE_EXIP(75);
			CASE_EXIP(76);
			CASE_EXIP(77);
			CASE_EXIP(78);
			CASE_EXIP(79);
			CASE_EXIP(80);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 6)
		for (i=81;i<=96;i++)
		{
			switch(i)
			{// N7 (6)
			CASE_EXIP(81);
			CASE_EXIP(82);
			CASE_EXIP(83);
			CASE_EXIP(84);
			CASE_EXIP(85);
			CASE_EXIP(86);
			CASE_EXIP(87);
			CASE_EXIP(88);
			CASE_EXIP(89);
			CASE_EXIP(90);
			CASE_EXIP(91);
			CASE_EXIP(92);
			CASE_EXIP(93);
			CASE_EXIP(94);
			CASE_EXIP(95);
			CASE_EXIP(96);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 7)
		for (i=97;i<=112;i++)
		{
			switch(i)
			{// N7 (7)
			CASE_EXIP(97);
			CASE_EXIP(98);
			CASE_EXIP(99);
			CASE_EXIP(100);
			CASE_EXIP(101);
			CASE_EXIP(102);
			CASE_EXIP(103);
			CASE_EXIP(104);
			CASE_EXIP(105);
			CASE_EXIP(106);
			CASE_EXIP(107);
			CASE_EXIP(108);
			CASE_EXIP(109);
			CASE_EXIP(110);
			CASE_EXIP(111);
			CASE_EXIP(112);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 8)
		for (i=113;i<=128;i++)
		{
			switch(i)
			{// N7 (8)
			CASE_EXIP(113);
			CASE_EXIP(114);
			CASE_EXIP(115);
			CASE_EXIP(116);
			CASE_EXIP(117);
			CASE_EXIP(118);
			CASE_EXIP(119);
			CASE_EXIP(120);
			CASE_EXIP(121);
			CASE_EXIP(122);
			CASE_EXIP(123);
			CASE_EXIP(124);
			CASE_EXIP(125);
			CASE_EXIP(126);
			CASE_EXIP(127);
			CASE_EXIP(128);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 9)
		for (i=129;i<=144;i++)
		{
			switch(i)
			{// N7 (9)
			CASE_EXIP(129);
			CASE_EXIP(130);
			CASE_EXIP(131);
			CASE_EXIP(132);
			CASE_EXIP(133);
			CASE_EXIP(134);
			CASE_EXIP(135);
			CASE_EXIP(136);
			CASE_EXIP(137);
			CASE_EXIP(138);
			CASE_EXIP(139);
			CASE_EXIP(140);
			CASE_EXIP(141);
			CASE_EXIP(142);
			CASE_EXIP(143);
			CASE_EXIP(144);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 10)
		for (i=145;i<=160;i++)
		{
			switch(i)
			{// N7 (10)
			CASE_EXIP(145);
			CASE_EXIP(146);
			CASE_EXIP(147);
			CASE_EXIP(148);
			CASE_EXIP(149);
			CASE_EXIP(150);
			CASE_EXIP(151);
			CASE_EXIP(152);
			CASE_EXIP(153);
			CASE_EXIP(154);
			CASE_EXIP(155);
			CASE_EXIP(156);
			CASE_EXIP(157);
			CASE_EXIP(158);
			CASE_EXIP(159);
			CASE_EXIP(160);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 11)
		for (i=161;i<=176;i++)
		{
			switch(i)
			{// N7 (11)
			CASE_EXIP(161);
			CASE_EXIP(162);
			CASE_EXIP(163);
			CASE_EXIP(164);
			CASE_EXIP(165);
			CASE_EXIP(166);
			CASE_EXIP(167);
			CASE_EXIP(168);
			CASE_EXIP(169);
			CASE_EXIP(170);
			CASE_EXIP(171);
			CASE_EXIP(172);
			CASE_EXIP(173);
			CASE_EXIP(174);
			CASE_EXIP(175);
			CASE_EXIP(176);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 12)
		for (i=177;i<=192;i++)
		{
			switch(i)
			{// N7 (12)
			CASE_EXIP(177);
			CASE_EXIP(178);
			CASE_EXIP(179);
			CASE_EXIP(180);
			CASE_EXIP(181);
			CASE_EXIP(182);
			CASE_EXIP(183);
			CASE_EXIP(184);
			CASE_EXIP(185);
			CASE_EXIP(186);
			CASE_EXIP(187);
			CASE_EXIP(188);
			CASE_EXIP(189);
			CASE_EXIP(190);
			CASE_EXIP(191);
			CASE_EXIP(192);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 13)
		for (i=193;i<=208;i++)
		{
			switch(i)
			{// N7 (13)
			CASE_EXIP(193);
			CASE_EXIP(194);
			CASE_EXIP(195);
			CASE_EXIP(196);
			CASE_EXIP(197);
			CASE_EXIP(198);
			CASE_EXIP(199);
			CASE_EXIP(200);
			CASE_EXIP(201);
			CASE_EXIP(202);
			CASE_EXIP(203);
			CASE_EXIP(204);
			CASE_EXIP(205);
			CASE_EXIP(206);
			CASE_EXIP(207);
			CASE_EXIP(208);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 14)
		for (i=209;i<=224;i++)
		{
			switch(i)
			{// N7 (14)
			CASE_EXIP(209);
			CASE_EXIP(210);
			CASE_EXIP(211);
			CASE_EXIP(212);
			CASE_EXIP(213);
			CASE_EXIP(214);
			CASE_EXIP(215);
			CASE_EXIP(216);
			CASE_EXIP(217);
			CASE_EXIP(218);
			CASE_EXIP(219);
			CASE_EXIP(220);
			CASE_EXIP(221);
			CASE_EXIP(222);
			CASE_EXIP(223);
			CASE_EXIP(224);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 15)
		for (i=225;i<=240;i++)
		{
			switch(i)
			{// N7 (15)
			CASE_EXIP(225);
			CASE_EXIP(226);
			CASE_EXIP(227);
			CASE_EXIP(228);
			CASE_EXIP(229);
			CASE_EXIP(230);
			CASE_EXIP(231);
			CASE_EXIP(232);
			CASE_EXIP(233);
			CASE_EXIP(234);
			CASE_EXIP(235);
			CASE_EXIP(236);
			CASE_EXIP(237);
			CASE_EXIP(238);
			CASE_EXIP(239);
			CASE_EXIP(240);
			default:
				break;
			}
		}
#endif

#if (USE_IOEX >= 16)
		for (i=241;i<=256;i++)
		{
			switch(i)
			{// N7 (16)
			CASE_EXIP(241);
			CASE_EXIP(242);
			CASE_EXIP(243);
			CASE_EXIP(244);
			CASE_EXIP(245);
			CASE_EXIP(246);
			CASE_EXIP(247);
			CASE_EXIP(248);
			CASE_EXIP(249);
			CASE_EXIP(250);
			CASE_EXIP(251);
			CASE_EXIP(252);
			CASE_EXIP(253);
			CASE_EXIP(254);
			CASE_EXIP(255);
			CASE_EXIP(256);
			default:
				break;
			}
		}
#endif
	}
#endif // N7
}
//-------------------------------------------------------

void ShowFileInfo(char * para1, char * para2)
{
	int p1, p2;

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

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

	if (p1 == p2)
	{
	}

	if (p1 == 0)
	{
		OutFileInfo();
	}
	if (p1 == 10)
	{
		if (p2 >= 0)
		{
			int idx = p2;
			int rslt;
			DataItem ditem;

			printf("GetADataItem(%d):", idx);
			rslt = GetADataItem(0, idx, &ditem);
			if (rslt == 0)
			{
				printf("ctrl=0x%x, attr=0x%x, dx=%d,dy=%d,dr=%d,len=%d,action=0x%x\r\n", ditem.ctrl, ditem.attr, ditem.dx, ditem.dy,ditem.dr,ditem.len,ditem.action);
			}
			else
			{
				printf("err idx\r\n");
			}
		}
	}
}

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

#define	DEF_BEGX	25000
#define	DEF_BEGY	50000


#define	DEF_SIZEX	100000
#define	DEF_SIZEY	100000

#define	DEF_DEX		2000
#define	DEF_DEY		2000


int g_testBegx = DEF_BEGX;
int g_testBegy = DEF_BEGY;

int g_testSizex = DEF_SIZEX;
int g_testSizey = DEF_SIZEY;

int g_testDex = DEF_DEX;
int g_testDey = DEF_DEY;

void TestPara(char * para1, char * para2)
{
	int p1, p2;

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

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

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

	if (p1 == p2)
	{

	}
	else
	{
		PreMatCylinderOff();
		return;
	}


	switch (p1)
	{
		case 0:
		{
			g_testBegx = DEF_BEGX;
			g_testBegy = DEF_BEGY;
			g_testSizex = DEF_SIZEX;
			g_testSizey = DEF_SIZEY;
			break;
		}
		case 1:
		{
			g_testBegx = p2;
			break;
		}
		case 2:
		{
			g_testBegy = p2;
			break;
		}
		case 3:
		{
			g_testSizex = p2;
			break;
		}
		case 4:
		{
			g_testSizey = p2;
			break;
		}
		case 5:
		{
			g_testDex = p2;
			break;
		}
		case 6:
		{
			g_testDey = p2;
			break;
		}
		case 7:
		{
			GetWorkPosFromIndex();
			printf("workpos r1 = %d, dataidx=%d\r\n", g_workCtrl.workPos[MTR1_IDX], g_workCtrl.cursta.dataIdx);
			break;
		}
		case 8:
		{
			g_workCtrl.cursta.dataIdx = p2;
		}
		default:
			break;

	}
}
void Simulation(char * para1, char * para2)
{
	int p1, p2;

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

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

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

	if(p1 == 0)//
	{
		if(p2 == 0)
		{
			// 电机位置有效
			g_workCtrl.posEnFlag = ALL_MOTO_MOD;
			// 零位坐标值赋值
			g_workCtrl.zeroPosList[MTX_IDX] = g_pOpticalMcPara->xSensorPos;
			g_workCtrl.zeroPosList[MTY_IDX] = g_pOpticalMcPara->ySensorPos;
			g_workCtrl.zeroPosList[MTR1_IDX] = g_pOpticalMcPara->r1SensorAng;
			g_workCtrl.zeroPosList[MTU1_IDX] = g_pOpticalMcPara->u1SensorPos;
			SetOpticalZeroPosValue(g_workCtrl.zeroPosList);

		}
	}
}

void Reset(char * para1, char * para2)
{
	AllToReset();
}
#endif

#endif// (CUR_MACHINE == MACH_OPTICAL)
