#include "leisaictrl.h"

#if (BUSCTRL_MOTO == LEISAI)

#include "modbus_m.h"
#include "delay.h"

typedef struct
{
	u16 motoreturndat;	//电机回包数据
	s16 motocurpuspos;

}LSDriverCtrl;

LSDriverCtrl g_lsdriverctrl;




int InitLeisaiMoto(u8 devIdx)	//初始化
{
	int err;
	memset(&g_lsdriverctrl, 0, sizeof(LSDriverCtrl));
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0X6000, 0, &g_lsdriverctrl.motoreturndat);
	DelayMs(20);
	return err;
}

/*移动电机到某位置 (绝对位置)
 * decIdx:电机设备地址：1-127
 * pos:目标位置（脉冲数）
 * spd:速度 rpm
 * */

int MoveLeisaiToAbsolutePos(u8 devIdx, s32 pos, u16 spd)
{
	int err = 0;
	u32 uPos;
	uPos = (u32)pos;
	u16 dataBuffWr[8];
	dataBuffWr[0] = 0x0001;		//运动模式
	dataBuffWr[1] = HIWORD(uPos);//位置高位
	dataBuffWr[2] = LOWORD(uPos);//位置低位
	dataBuffWr[3] = spd;	//速度
//	dataBuffWr[4] = 120;	//加速时间
//	dataBuffWr[5] = 120;	//减速时间
	dataBuffWr[4] = 200;	//加速时间
	dataBuffWr[5] = 200;	//减速时间
	dataBuffWr[6] = 0;		//延迟时间
	dataBuffWr[7] = 0x0010;	//触发控制

	err = ModbusCommMulti(0, devIdx, MODBUS_WR_MULTI, 0x6200, &(dataBuffWr[0]), 8, NULL, 8);

	return err;
}

/*移动电机到某位置（相对位置）
 * decIdx:电机设备地址：1-127
 * pos:目标位置（脉冲数）
 * spd:速度
 * */

int MoveLeisaiToRelativePos(u8 devIdx, s32 pos, u16 spd)
{
	int err = 0;
	u32 uPos;
	uPos = (u32)pos;
	u16 dataBuffWr[8];
	dataBuffWr[0] = 0x0041;		//运动模式 相对位置
	dataBuffWr[1] = HIWORD(uPos);//位置高位
	dataBuffWr[2] = LOWORD(uPos);//位置低位
	dataBuffWr[3] = spd;	//速度
	dataBuffWr[4] = 1000;	//加速时间
	dataBuffWr[5] = 1000;	//减速时间
	dataBuffWr[6] = 0;		//延迟时间
	dataBuffWr[7] = 0x0010;	//触发控制

	err = ModbusCommMulti(0, devIdx, MODBUS_WR_MULTI, 0x6200, &(dataBuffWr[0]), 8, NULL, 8);

	return err;
}

//读取电机运行状态 当前指令是否完成0：未完成；1：完成，取指令完成位
int GetLeisaiCurStatus(u8 devIdx, int * statuflg)
{
	int err = 0;
	u16 sta = 0;
	err = ModbusComm16(0, devIdx, MODBUS_RD, 0x1003, 0, &sta);
	if(err < 0)
	{
		return err;
	}
	else
	{
		if((sta & 0x10) == 0x10)
		{
			*statuflg = 1;
		}
		else
		{
			*statuflg = 0;
		}
	}
	return err;
}

//读取电机位置 单位：脉冲
int GetLeisaiCurPusPos(u8 devIdx, s32 * puspos)
{
	int err = 0;
	u32 upuspos = 0;
	u16 upusPosH,upusPosL;
	err = ModbusComm32(0, devIdx, MODBUS_RD, 0x602C, 0, &upuspos);
	upusPosH = HIWORD(upuspos);
	upusPosL = LOWORD(upuspos);
	upuspos = ((DWORD)((WORD)(upusPosH)) | (((DWORD)((WORD)(upusPosL))) << 16));
	*puspos = (s32)upuspos;
	return err;
}

int MoveLeisaiJOGP(u8 devIdx, u16 spd)
{
	int err = 0;
	if(spd != 0)
	{
		err = ModbusComm16(0, devIdx, MODBUS_WR, 0x01E1, spd, &g_lsdriverctrl.motoreturndat);	//点动速度
	}
	if(err >= 0)
	{
		err = ModbusComm16(0, devIdx, MODBUS_WR, 0X1801, 0X4001, &g_lsdriverctrl.motoreturndat);
	}
	return err;
}

int MoveLeisaiJOGN(u8 devIdx, u16 spd)
{
	int err = 0;
	if(spd != 0)
	{
		err = ModbusComm16(0, devIdx, MODBUS_WR, 0x01E1, spd, &g_lsdriverctrl.motoreturndat);	//点动速度
	}
	if(err >= 0)
	{
		err = ModbusComm16(0, devIdx, MODBUS_WR, 0X1801, 0X4002, &g_lsdriverctrl.motoreturndat);
	}
	return err;
}

//电机停止
int LeisaiMotoStop(u8 devIdx)
{
	int err = 0;
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6002, 0x0040, &g_lsdriverctrl.motoreturndat);
	return err;
}

//当前位置手动设零
int LeisaiMotoSetCurPosZero(u8 devIdx)
{
	int err = 0;
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6002, 0x0021, &g_lsdriverctrl.motoreturndat);
	if(g_lsdriverctrl.motoreturndat != 0x21)
	{
		err = -3;
	}
	return err;
}

//回零
int LeisaiMotoToPosZero(u8 devIdx)
{
	int err = 0;
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6002, 0x0020, &g_lsdriverctrl.motoreturndat);
	return err;
}

int LeisaiMotoSetLim(u8 devIdx, s32 posp, s32 posn)
{
	int err = 0;
	u32 uPosp, uPosn;
	u16 uPospH, uPospL, uPosnH, uPosnL;

	uPosp = (u32)posp;
	uPosn = (u32)posn;

	uPospH = HIWORD(uPosp);
	uPospL = LOWORD(uPosp);
	uPosnH = HIWORD(uPosn);
	uPosnL = LOWORD(uPosn);

	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6006, uPospH, &g_lsdriverctrl.motoreturndat);
	if(err < 0)
	{
		return err;
	}
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6007, uPospL, &g_lsdriverctrl.motoreturndat);
	if(err < 0)
	{
		return err;
	}
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6008, uPosnH, &g_lsdriverctrl.motoreturndat);
	if(err < 0)
	{
		return err;
	}
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6009, uPosnL, &g_lsdriverctrl.motoreturndat);
	if(err < 0)
	{
		return err;
	}
	DelayMs(20);

	err = ModbusComm16(0, devIdx, MODBUS_WR, 0X6000, 2, &g_lsdriverctrl.motoreturndat);
	return err;
}
//读取电机故障代码
int GetLeisaiCurErrcode(u8 devIdx, u16 * errcode)
{
	int err = 0;
	err = ModbusComm16(0, devIdx, MODBUS_RD, 0x2203, 0, errcode);
	return err;
}

#endif