#include "ruanyictrl.h"

#if (BUSCTRL_MOTO == RUANYI)

#include "modbus_m.h"
#include "workctrl.h"

typedef struct
{
	u16 motoreturndat;	//电机回包数据
	u16 motoparaval;	//参数设置值
	u16 zerocodeval;	//零位编码值
	int curtozeroval;	//当前位置与零位编码值
	u32 curtozeroabs;

	u8 zeroflag;
	s16 motocurpuspos;
}RYDriverCtrl;

RYDriverCtrl g_rydriverctrl;

void InitRuanyiCom(void)	//初始化
{
	memset(&g_rydriverctrl, 0, sizeof(RYDriverCtrl));

}


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

int MoveRyiToAbsolutePos(u8 devIdx, s16 pos, u16 spd)
{
	int err = 0;
	static u16 oldRyspd = 500;
	if(spd != oldRyspd)
	{
		oldRyspd = spd;
		err = ModbusComm16(0,devIdx, MODBUS_WR, 8256, (u16)spd, &g_rydriverctrl.motoreturndat);  //更改最高速度
		DelayRef(300);
	}
	if(err >= 0 &&
			pos != 0)
	{
		err = ModbusComm16(0,devIdx, MODBUS_WR, 9473, (u16)pos, &g_rydriverctrl.motoreturndat);
		if(g_rydriverctrl.motoreturndat != (u16)pos)
		{
			err = -10;
		}
	}

	return err;
}



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

int MoveRyiToRelativePos(u8 devIdx, s16 pos, u16 spd)
{
	int err = 0;
	static u16 oldRyspd = 1500;
	if(spd != oldRyspd)
	{
		oldRyspd = spd;
		err = ModbusComm16(0,devIdx, MODBUS_WR, 8256, (u16)spd, &g_rydriverctrl.motoreturndat);  //更改最高速度
		DelayRef(300);
	}
	if(err >= 0 &&
			pos != 0)
	{
		err = ModbusComm16(0,devIdx, MODBUS_WR, 9475, (u16)pos, &g_rydriverctrl.motoreturndat);
		if(g_rydriverctrl.motoreturndat != (u16)pos)
		{
			err = -10;
		}
	}

	return err;
}

/*电机JOG+ 运行
 * decIdx:电机设备地址：1-127
 * spd:速度 0：不改变速度
 * */

int MoveRyiJOGP(u8 devIdx, u16 spd)
{
	int err = 0;
	if(spd != 0)
	{
		err = ModbusComm16(0,devIdx, MODBUS_WR, 8241, (u16)spd, &g_rydriverctrl.motoreturndat);	//点动速度
	}
	if(err >= 0)
	{
		err = ModbusComm16(0,devIdx, MODBUS_WR, BITMOVEADDR, BITJOGPCMD, &g_rydriverctrl.motoreturndat);
	}
	return err;
}

int MoveRyiJOGN(u8 devIdx, u16 spd)
{
	int err = 0;
	if(spd != 0)
	{
		err = ModbusComm16(0,devIdx, MODBUS_WR, 8241, (u16)spd, &g_rydriverctrl.motoreturndat);	//点动速度
	}
	if(err >= 0)
	{
		err = ModbusComm16(0,devIdx, MODBUS_WR, BITMOVEADDR, BITJOGNCMD, &g_rydriverctrl.motoreturndat);
	}
	return err;
}

//电机停止
int RyiMotoStop(u8 devIdx)
{
	int err = 0;
	err = ModbusComm16(0,devIdx, MODBUS_WR, BITMOVEADDR, BITSTOPCMD, &g_rydriverctrl.motoreturndat);
	return err;
}

//当前位置给定
int SetRyiCurPos(u8 devIdx, s16 pos)
{
	int err = 0;
	err = ModbusComm16(0,devIdx, MODBUS_WR, 9472, pos, &g_rydriverctrl.motoreturndat);
	return err;
}

//当前位置清零
int RyiMotoSetCurPosZero(u8 devIdx)
{
	int err = 0;
	err = ModbusComm16(0,devIdx, MODBUS_WR, BITMOVEADDR, BITSETCPOSZEROCMD, &g_rydriverctrl.motoreturndat);
	return err;
}

//到对应位置
int RyiMotoToPos(u8 devIdx, u16 pos)
{
	int err = 0;
	err = ModbusComm16(0,devIdx, MODBUS_WR, BITTOPOSADDR, pos, &g_rydriverctrl.motoreturndat);
	return err;
}


//读取电机位置 单位：脉冲
int GetCurPusPos(u8 devIdx, s16 * puspos)
{
	int err = 0;
	u16 upuspos = 0;
	err = ModbusComm16(0,devIdx, MODBUS_RD, 9729, 0, &upuspos);
	*puspos = (s16)upuspos;
	return err;
}

//读取电机状态 当前指令是否完成0：未完成；1：完成 2：堵转 3：错误
int GetCurStatus(u8 devIdx, int * statuflg)
{
	int err = 0;
	u16 sta = 0;
	err = ModbusComm16(0,devIdx, MODBUS_RD, 9728, 0, &sta);
	if(err < 0)
	{
		return err;
	}
	else
	{
		if(sta == 1)
		{
			*statuflg = 1;
		}
		else if(sta == 0)
		{
			*statuflg = 0;
		}
		else
		{
			*statuflg = -1;
		}
	}
	return err;
}

#endif