#include "stdafx.h"
#include "ComiCmcSdk.h"
#include "ComiEcatSdk_Api.h"
#include <MMSystem.h>

CComiCmc::CComiCmc()
{
	TEcSlvProdInfo ProdInfo;
	int nErr;
	
	int nSlvNum = ecNet_ScanSlaves(NETID, NULL);
	for(int i=1; i<nSlvNum; i++)	{
		ecSlv_GetProdInfo_A(NETID, i, &ProdInfo, NULL);
		if(ProdInfo.ProdCode == ECAT_CMC) {
			m_nSlaveIndex = i;
			m_nSlaveAddress = ecSlv_SlvIdx2PhysAddr(NETID, i, &nErr);
			return;
		}
	}
	m_nSlaveIndex = 0xFFFF;
	m_nSlaveAddress = 0xFFFF;
}

BOOL IsTimeOut(ULONG dwStartT_ms, ULONG dwTimeOutVal_ms)
{
	ULONG dwElpsTime;
	ULONG dwCurTime = timeGetTime();
	if(dwCurTime < dwStartT_ms){
		if(dwCurTime > dwTimeOutVal_ms){
			return true;
		}else{
			dwElpsTime = dwCurTime + (ULONG)((ULONG)0xffffffff - dwStartT_ms) + 1;
		}
	}else{
		dwElpsTime = dwCurTime - dwStartT_ms;
	}

	if(dwElpsTime > dwTimeOutVal_ms)
		return true;
	else
		return false;
}

TCmc_AxisInPDO* CComiCmc::GetInPDO(int nAxis)
{
	if(m_nSlaveAddress == 0xffff)
		return NULL;
	t_byte *pInPDO = (t_byte *)ecSlv_InPDO_GetBufPtr(NETID, m_nSlaveAddress, 0, NULL);
	pInPDO = pInPDO + (sizeof(TCmc_AxisInPDO) * nAxis);

	return (TCmc_AxisInPDO *)pInPDO;
}

TCmc_AxisOutPDO* CComiCmc::GetOutPDO(int nAxis)
{
	if(m_nSlaveAddress == 0xffff)
		return NULL;
	t_byte *pOutPDO = (t_byte *)ecSlv_OutPDO_GetBufPtr(NETID, m_nSlaveAddress, 0, NULL);
	pOutPDO = pOutPDO + (sizeof(TCmc_AxisOutPDO) * nAxis);

	return (TCmc_AxisOutPDO *)pOutPDO;
}

USHORT* CComiCmc::GetDiPDO()
{
	if(m_nSlaveAddress == 0xffff)
		return NULL;
	t_byte *pInPDO = (t_byte *)ecSlv_InPDO_GetBufPtr(NETID, m_nSlaveAddress, 0, NULL);
	pInPDO = pInPDO + (sizeof(TCmc_AxisInPDO) * 6);

	return (USHORT*)pInPDO;
}

USHORT* CComiCmc::GetDoPDO()
{
	if(m_nSlaveAddress == 0xffff)
		return NULL;
	t_byte *pOutPDO = (t_byte *)ecSlv_OutPDO_GetBufPtr(NETID, m_nSlaveAddress, 0, NULL);
	pOutPDO = pOutPDO + (sizeof(TCmc_AxisOutPDO) * 6);

	return (USHORT*)pOutPDO;
}

LONG CComiCmc::GetNumAxis()
{
	if(m_nSlaveIndex == 0xFFFF)
		return 0;
	else
		return 6;
}

USHORT CComiCmc::GetSlaveAddress()
{
	return m_nSlaveAddress;
}

USHORT CComiCmc::GetSlaveIndex()
{
	return m_nSlaveIndex;
}

BOOL CComiCmc::SetSvon(int nAxis, BOOL SvonState, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	USHORT wOldCtlWord = pOutPDO->CtlWord;
	if(SvonState == 0){
		pOutPDO->CtlWord = (wOldCtlWord & 0xfff0) | 0x0007;
	}else{
		pOutPDO->CtlWord = (wOldCtlWord & 0xfff0) | 0x000f;		
	}	
	return true;
}

BOOL CComiCmc::GetSvon(int nAxis, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	return ecGetBit(pOutPDO->CtlWord, 3);	
}

BOOL CComiCmc::MoveStart(int nAxis, int Distance, int *pErrCode)
{
	nAxis -= 1;

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	// OP-MODE가 "Profile Position Mode" 인지 체크 //
	TCmc_AxisInPDO *pInPDO = GetInPDO(nAxis);
	ULONG dwStartT = timeGetTime();
	while(pInPDO && pInPDO->OpMode != 1){
		pOutPDO->OpMode = 1;
		Sleep(1);
		if(IsTimeOut(dwStartT, 1000)){
			if(pErrCode) *pErrCode = CMC_ERR_OPMODE_TIMEOUT;
			return false;
		}
	}	

	pOutPDO->TargPos = Distance;

	ecSetBit(pOutPDO->CtlWord, 6, 1); // Relative

	if(ecGetBit(pOutPDO->CtlWord, 4)){
		ecSetBit(pOutPDO->CtlWord, 4, 0);
		Sleep(PDO_INTERVAL);
	}

	ecSetBit(pOutPDO->CtlWord, 4, 1);
	Sleep(PDO_INTERVAL);
	ecSetBit(pOutPDO->CtlWord, 4, 0);
	Sleep(PDO_INTERVAL);
	return true;
}

BOOL CComiCmc::MoveToStart(int nAxis, int Position, int *pErrCode)
{
	nAxis -= 1;

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	TCmc_AxisInPDO *pInPDO = GetInPDO(nAxis);
	ULONG dwStartT = timeGetTime();
	while(pInPDO && pInPDO->OpMode != 1){
		pOutPDO->OpMode = 1;
		Sleep(1);
		if(IsTimeOut(dwStartT, 1000)){
			if(pErrCode) *pErrCode = CMC_ERR_OPMODE_TIMEOUT;
			return false;
		}
	}

	pOutPDO->TargPos = Position;

	ecSetBit(pOutPDO->CtlWord, 6, 0); // Absolute

	// Move Start Trigger //
	if(ecGetBit(pOutPDO->CtlWord, 4)){
		ecSetBit(pOutPDO->CtlWord, 4, 0);
		Sleep(PDO_INTERVAL);
	}
	ecSetBit(pOutPDO->CtlWord, 4, 1);
	Sleep(PDO_INTERVAL);
	ecSetBit(pOutPDO->CtlWord, 4, 0);
	Sleep(PDO_INTERVAL);
	return true;
}

BOOL CComiCmc::VMoveStart(int nAxis, int *pErrCode)
{
	nAxis -= 1;

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	TCmc_AxisInPDO *pInPDO = GetInPDO(nAxis);
	ULONG dwStartT = timeGetTime();
	while(pInPDO && pInPDO->OpMode != 3){
		pOutPDO->OpMode = 3;
		Sleep(1);
		if(IsTimeOut(dwStartT, 1000)){
			if(pErrCode) *pErrCode = CMC_ERR_OPMODE_TIMEOUT;
			return false;
		}
	}

	return true;
}

BOOL CComiCmc::HomeMoveStart(int nAxis, int *pErrCode)
{
	nAxis -= 1;

	t_i32 TmpErrCode = 0;

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	// OP-MODE가 "Homing Mode" 인지 체크 //
	TCmc_AxisInPDO *pInPDO = GetInPDO(nAxis);
	ULONG dwStartT = timeGetTime();
	while(pInPDO && pInPDO->OpMode != 6){ // "Homing Mode"가 아니면 "Homing Mode"로 전환
		pOutPDO->OpMode = 6;
		Sleep(1);
		if(IsTimeOut(dwStartT, 1000)){
			if(pErrCode) *pErrCode = CMC_ERR_OPMODE_TIMEOUT;
			return false;
		}
	}	

	// Move Start Trigger //
	if(ecGetBit(pOutPDO->CtlWord, 4)){
		ecSetBit(pOutPDO->CtlWord, 4, 0);
		Sleep(PDO_INTERVAL);
	}
	ecSetBit(pOutPDO->CtlWord, 4, 1); // Homing operation start
	Sleep(PDO_INTERVAL);
	ecSetBit(pOutPDO->CtlWord, 4, 0);
	Sleep(PDO_INTERVAL);
	return true;
}

BOOL CComiCmc::Stop(int nAxis, int *pErrCode)
{
	nAxis -= 1;

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	int nData = 0;
	int ErrCode = 0;

	pOutPDO->TargVel = 0;

	// 0x605D Halt Option Code
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x605D+(nAxis*0x800), 0, false, 2, &nData, &ErrCode);
	if( nData != 1 ) // 감속정지가 아니면
	{
		nData = 1;
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x605D+(nAxis*0x800), 0, false, 2, &nData, &ErrCode);
	}

	ecSetBit(pOutPDO->CtlWord, 8, 1); // Halt
	Sleep(PDO_INTERVAL);
	ecSetBit(pOutPDO->CtlWord, 8, 0);
	return true;
}

BOOL CComiCmc::EmgStop(int nAxis, int *pErrCode)
{
	nAxis -= 1;

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	int nData = 0;
	int ErrCode = 0;

	// 0x605A Quick Stop Option Code
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x605A+(nAxis*0x800), 0, false, 2, &nData, &ErrCode);
	if( nData != 2 ) // 급정지가 아니면
	{
		nData = 2;
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x605A+(nAxis*0x800), 0, false, 2, &nData, &ErrCode);
	}

	ecSetBit(pOutPDO->CtlWord, 2, 1); // Halt
	Sleep(PDO_INTERVAL);
	ecSetBit(pOutPDO->CtlWord, 2, 0);
	return true;
}

BOOL CComiCmc::SetSpeedEnv(int nAxis, int WorkSpeed, int Accel, int Decel, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x6081+(nAxis*0x800), 0, false, 4, &WorkSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x6083+(nAxis*0x800), 0, false, 4, &Accel, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x6084+(nAxis*0x800), 0, false, 4, &Decel, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::GetSpeedEnv(int nAxis, int *pWorkSpeed, int *pAccel, int *pDecel, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6081+(nAxis*0x800), 0, false, 4, pWorkSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6083+(nAxis*0x800), 0, false, 4, pAccel, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6084+(nAxis*0x800), 0, false, 4, pDecel, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::SetMaxSpeed(int nAxis, int MaxSpeed, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x607F+(nAxis*0x800), 0, false, 4, &MaxSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::GetMaxSpeed(int nAxis, int *pMaxSpeed, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x607F+(nAxis*0x800), 0, false, 4, pMaxSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::SetTargetVelocity(int nAxis, int Velocity, int *pErrCode)
{
	nAxis -= 1;

	TCmc_AxisOutPDO *pOutPDO = GetOutPDO(nAxis);
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	pOutPDO->TargVel = Velocity;

	return true;
}

BOOL CComiCmc::GetTargetVelocity(int nAxis, int *pVelocity, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x60FF+(nAxis*0x800), 0, false, 4, pVelocity, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::SetHomeMode(int nAxis, int HomeMethod, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x6098+(nAxis*0x800), 0, false, 2, &HomeMethod, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::GetHomeMode(int nAxis, int *pHomeMethod, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6098+(nAxis*0x800), 0, false, 2, pHomeMethod, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::SetHomeSpeedEnv(int nAxis, int WorkSpeed, int StopSpeed, int AccDec, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x6099+(nAxis*0x800), 1, false, 4, &WorkSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x6099+(nAxis*0x800), 2, false, 4, &StopSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x609A+(nAxis*0x800), 0, false, 4, &AccDec, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::GetHomeSpeedEnv(int nAxis, int *pWorkSpeed, int *pStopSpeed, int *pAccDec, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6099+(nAxis*0x800), 1, false, 4, pWorkSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6099+(nAxis*0x800), 2, false, 4, pStopSpeed, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x609A+(nAxis*0x800), 0, false, 4, pAccDec, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::StGetPosition(int nAxis, int TargCntr, int *pPosition, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	if(TargCntr) 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6064+(nAxis*0x800), 0, false, 4, pPosition, &TmpErrCode);
	else 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x6062+(nAxis*0x800), 0, false, 4, pPosition, &TmpErrCode);
	
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::StGetCurSpeed(int nAxis, int TargCntr, int *pCurSpeed, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	if(TargCntr) 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x606C+(nAxis*0x800), 0, false, 4, pCurSpeed, &TmpErrCode);
	else 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x606B+(nAxis*0x800), 0, false, 4, pCurSpeed, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::StGetMotFlag(int nAxis, void *pMotFlag, int *pErrCode)
{
	nAxis -= 1;
	
	TCmc_AxisInPDO *pInPDO = GetInPDO(nAxis);
	if(pInPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_INPDO_PTR;
		return false;
	}

	memcpy(pMotFlag, &pInPDO->StsWord, sizeof(pInPDO->StsWord));

	return true;
}

BOOL CComiCmc::SetPolarity(int nAxis, int Polarity, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x607E+(nAxis*0x800), 0, false, 1, &Polarity, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::GetPolarity(int nAxis, int *pPolarity, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x607E+(nAxis*0x800), 0, false, 1, pPolarity, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}
	return true;
}

BOOL CComiCmc::SetSoftLimit(int nAxis, int TargLimit, int PosLimit, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	if(TargLimit) 
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x607D+(nAxis*0x800), 2, false, 4, &PosLimit, &TmpErrCode);
	else 
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x607D+(nAxis*0x800), 1, false, 4, &PosLimit, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::GetSoftLimit(int nAxis, int TargLimit, int *pPosLimit, int *pErrCode)
{
	nAxis -= 1;

	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	if(TargLimit) 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x607D+(nAxis*0x800), 2, false, 4, pPosLimit, &TmpErrCode);
	else 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x607D+(nAxis*0x800), 1, false, 4, pPosLimit, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::ReadMotState(int nAxis, int *pState, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x2022, nAxis, false, 1, pState, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}



BOOL CComiCmc::SetWorkCurrent(int nAxis, int TargCurrent, int CurrValue, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	if(TargCurrent) 
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x2011, nAxis, false, 2, &CurrValue, &TmpErrCode);
	else 
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x2010, nAxis, false, 2, &CurrValue, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::GetWorkCurrent(int nAxis, int TargCurrent, int *pCurrValue, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	if(TargCurrent) 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x2011, nAxis, false, 2, pCurrValue, &TmpErrCode);
	else 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x2010, nAxis, false, 2, pCurrValue, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::SetStopCurrent(int nAxis, int CurrValue, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x2012, nAxis, false, 2, &CurrValue, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::GetStopCurrent(int nAxis, int *pCurrValue, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x2012, nAxis, false, 2, pCurrValue, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::SetStepDivision(int nAxis, int StepDiv, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x2015, nAxis, false, 1, &StepDiv, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::GetStepDivision(int nAxis, int *pStepDiv, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x2015, nAxis, false, 1, pStepDiv, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::SetSensorChan(int nSensor, int SelectChan, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x200A, nSensor+1, false, 2, &SelectChan, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::GetSensorChan(int nSensor, int *pConnectChan, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x200A, nSensor+1, false, 2, pConnectChan, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::SetSensorLogic(int nSensor, int Logic, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x200B, nSensor+1, false, 1, &Logic, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::GetSensorLogic(int nSensor, int *pLogic, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x200B, nSensor+1, false, 1, pLogic, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::ReadEncoderCnt(int nChannel, int TargCntr, int *pCountValue, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	if(TargCntr) 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x2031, nChannel+1, false, 2, pCountValue, &TmpErrCode);
	else 
		ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x2030, nChannel+1, false, 4, pCountValue, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::SetCntClear(int nChannel, int TargCntr, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_ui8 nData = 0;
	t_i32 TmpErrCode = 0;
	if(TargCntr) 
		return false;
	else {
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x2032, nChannel+1, false, 1, &nData, &TmpErrCode);
		nData = 1;
		ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x2032, nChannel+1, false, 1, &nData, &TmpErrCode);
	}
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}







USHORT CComiCmc::DiGetNumChannel()
{
	if(m_nSlaveIndex == 0xFFFF)
		return 0;
	else
		return 16;
}

USHORT CComiCmc::DoGetNumChannel()
{
	if(m_nSlaveIndex == 0xFFFF)
		return 0;
	else
		return 12;
}

BOOL CComiCmc::DiGetOne(int DiChannel, int *pErrCode)
{
	USHORT* pInPDO = GetDiPDO();
	if(pInPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_INPDO_PTR;
		return false;
	}

	return ecGetBit(*pInPDO, DiChannel);
}

USHORT CComiCmc::DiGetMulti(int IniChannel, int NumChannel, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(!(16 - IniChannel >= NumChannel)) {
		if(pErrCode)	*pErrCode = CMC_ERR_INVALID_PARAMETER;
		return false;
	}

	USHORT* pInPDO = GetDiPDO();
	if(pInPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_INPDO_PTR;
		return false;
	}

	USHORT TempData = 0;
	for(int i=0; i<NumChannel; i++) {
		TempData += (ecGetBit(*pInPDO, IniChannel+i) << i);
	}

	return TempData;
}

BOOL CComiCmc::DoGetOne(int DoChannel, int *pErrCode)
{
	USHORT* pOutPDO = GetDoPDO();
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	return ecGetBit(*pOutPDO, DoChannel);
}

USHORT CComiCmc::DoGetMulti(int IniChannel, int NumChannel, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(!(12 - IniChannel >= NumChannel)) {
		if(pErrCode)	*pErrCode = CMC_ERR_INVALID_PARAMETER;
		return false;
	}

	USHORT* pOutPDO = GetDoPDO();
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	USHORT TempData = 0;
	for(int i=0; i<NumChannel; i++) {
		TempData += (ecGetBit(*pOutPDO, IniChannel+i) << i);
	}

	return TempData;
}

BOOL CComiCmc::DoPutOne(int DoChannel, BOOL OutState, int *pErrCode)
{
	USHORT* pOutPDO = GetDoPDO();
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	ecSetBit(*pOutPDO, DoChannel, OutState);
	Sleep(PDO_INTERVAL);

	return true;
}

BOOL CComiCmc::DoPutMulti(int IniChannel, int NumChannel, USHORT OutState, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(!(12 - IniChannel >= NumChannel)) {
		if(pErrCode)	*pErrCode = CMC_ERR_INVALID_PARAMETER;
		return false;
	}

	USHORT* pOutPDO = GetDoPDO();
	if(pOutPDO == NULL){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_OUTPDO_PTR;
		return false;
	}

	for(int i=0; i<NumChannel; i++)
		ecSetBit(*pOutPDO, IniChannel+i, (OutState>>i)&0x1);
	Sleep(PDO_INTERVAL);

	return true;
}

BOOL CComiCmc::ReadFwVersion(char *pVersion, int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_i32 TmpErrCode = 0;
	ecSlv_ReadCoeSdo(NETID, m_nSlaveAddress, 0x100A, 0, false, 8, pVersion, &TmpErrCode);
	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}

BOOL CComiCmc::SetSettingSave(int *pErrCode)
{
	if(pErrCode) *pErrCode = 0;
	if(m_nSlaveAddress == 0xffff){
		if(pErrCode) *pErrCode = CMC_ERR_INVALID_SLAVE_ADDR;
		return false;
	}

	t_ui16 nData = 0;
	t_i32 TmpErrCode = 0;

	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x20F0, 0, false, 2, &nData, &TmpErrCode);
	nData = 1;
	ecSlv_WriteCoeSdo(NETID, m_nSlaveAddress, 0x20F0, 0, false, 2, &nData, &TmpErrCode);

	if(TmpErrCode != 0) {
		if(pErrCode) *pErrCode = TmpErrCode;
		return false;
	}

	return true;
}