optical/EMBOS/Users/EmbFunc/busmotos/leisaictrl.c

#include "leisaictrl.h"

#if (BUSCTRL_MOTO == LEISAI)

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

typedef struct
{
	u16 motoreturndat;	//<2F><><EFBFBD><EFBFBD><EFBFBD>ذ<EFBFBD><D8B0><EFBFBD><EFBFBD><EFBFBD>
	s16 motocurpuspos;

}LSDriverCtrl;

LSDriverCtrl g_lsdriverctrl;




int InitLeisaiMoto(u8 devIdx)	//<2F><>ʼ<EFBFBD><CABC>
{
	int err;
	memset(&g_lsdriverctrl, 0, sizeof(LSDriverCtrl));
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0X6000, 0, &g_lsdriverctrl.motoreturndat);
	DelayMs(20);
	return err;
}

/*<2A>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ijλ<C4B3><CEBB> (<28><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>)
 * decIdx:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD>1-127
 * pos:Ŀ<EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 * spd:<EFBFBD>ٶ<EFBFBD> rpm
 * */

int MoveLeisaiToAbsolutePos(u8 devIdx, s32 pos, u16 spd)
{
	int err = 0;
	u32 uPos;
	uPos = (u32)pos;
	u16 dataBuffWr[8];
	dataBuffWr[0] = 0x0001;		//<2F>˶<EFBFBD>ģʽ
	dataBuffWr[1] = HIWORD(uPos);//λ<>ø<EFBFBD>λ
	dataBuffWr[2] = LOWORD(uPos);//λ<>õ<EFBFBD>λ
	dataBuffWr[3] = spd;	//<2F>ٶ<EFBFBD>
//	dataBuffWr[4] = 120;	//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
//	dataBuffWr[5] = 120;	//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
	dataBuffWr[4] = 200;	//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
	dataBuffWr[5] = 200;	//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
	dataBuffWr[6] = 0;		//<2F>ӳ<EFBFBD>ʱ<EFBFBD><CAB1>
	dataBuffWr[7] = 0x0010;	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

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

	return err;
}

/*<2A>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ijλ<C4B3>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>
 * decIdx:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD>1-127
 * pos:Ŀ<EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 * spd:<EFBFBD>ٶ<EFBFBD>
 * */

int MoveLeisaiToRelativePos(u8 devIdx, s32 pos, u16 spd)
{
	int err = 0;
	u32 uPos;
	uPos = (u32)pos;
	u16 dataBuffWr[8];
	dataBuffWr[0] = 0x0041;		//<2F>˶<EFBFBD>ģʽ <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
	dataBuffWr[1] = HIWORD(uPos);//λ<>ø<EFBFBD>λ
	dataBuffWr[2] = LOWORD(uPos);//λ<>õ<EFBFBD>λ
	dataBuffWr[3] = spd;	//<2F>ٶ<EFBFBD>
	dataBuffWr[4] = 1000;	//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
	dataBuffWr[5] = 1000;	//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
	dataBuffWr[6] = 0;		//<2F>ӳ<EFBFBD>ʱ<EFBFBD><CAB1>
	dataBuffWr[7] = 0x0010;	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

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

	return err;
}

//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬ <20><>ǰָ<C7B0><D6B8><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30>δ<EFBFBD><CEB4><EFBFBD>ɣ<EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>ɣ<EFBFBD>ȡָ<C8A1><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
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;
}

//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>λ<EFBFBD><CEBB> <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);	//<2F>㶯<EFBFBD>ٶ<EFBFBD>
	}
	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);	//<2F>㶯<EFBFBD>ٶ<EFBFBD>
	}
	if(err >= 0)
	{
		err = ModbusComm16(0, devIdx, MODBUS_WR, 0X1801, 0X4002, &g_lsdriverctrl.motoreturndat);
	}
	return err;
}

//<2F><><EFBFBD><EFBFBD>ֹͣ
int LeisaiMotoStop(u8 devIdx)
{
	int err = 0;
	err = ModbusComm16(0, devIdx, MODBUS_WR, 0x6002, 0x0040, &g_lsdriverctrl.motoreturndat);
	return err;
}

//<2F><>ǰλ<C7B0><CEBB><EFBFBD>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD><EFBFBD>
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;
}

//<2F><><EFBFBD><EFBFBD>
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;
}
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD>
int GetLeisaiCurErrcode(u8 devIdx, u16 * errcode)
{
	int err = 0;
	err = ModbusComm16(0, devIdx, MODBUS_RD, 0x2203, 0, errcode);
	return err;
}

#endif