/**
 * @~japanese
 * @file openEL_motor.c
 * @brief		OpenEL モーター共有部
 * @Version 3.0.0
 *
 * @~english
 * @file openEL_motor.c
 * @brief		OpenEL Motor Common File
 * @Version 3.0.0
 */
/*

Copyright (c) 2017,2018 Japan Embedded Systems Technology Association(JASA)
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
    Neither the name of the Association nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#define OPENEL_SW_SURFACE_FRIEND (1)
#include "openEL.h"
#include <stdio.h>

/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置決め運転
@param[in]	portID : ポートID
@param[in]	angle : 絶対位置指令 [ユーザー単位]
@param[in]	speed : 速度 [ユーザー単位/s] 範囲 0より大きい～製品仕様
@param[in]	tmAcc : 加減速時間 [ms](I32),[s](F64) 0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		速度、加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
enum ReturnCode HalSetPosition(HALCOMPONENT_T *halComponent, halfloat position) {
	printf("HalSetPosition()\n");
	//	int32_t posI32;
	int32_t this;
	this = halComponent->halComponent_core.handle;
	printf("this=%x\n", this);
	struct elMotorFncTblSt *pMotorFbl;
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[this].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(this, ELMOTOR_PROPERTY)) ) {
		return HAL_ERROR;
	}
#endif
	pMotorFbl = elPortTbl[this].fncTbl.pMotor;
	return pMotorFbl->pFncSetPosition(&halComponent, position);
}

/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置の取得(I32)
@param[in]	portID  : ポートID
@param[in]	idx : 0:現在の検出位置 / 1:指定された目標位置 / 2:現在の指令位置
@param[out] pOutPos
@return		エラーコード 0:正常 / 0以外:異常
@task		ユーザースレッド (ノンブロッキング)
@detail
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
enum ReturnCode HalGetPosition(HALCOMPONENT_T *halComponent, halfloat *position) {
	int32_t this;
	this = halComponent->halComponent_core.handle;
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[this].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(this,ELMOTOR_PROPERTY)) ) {
		return HAL_ERROR;
	}
#endif
	return elPortTbl[this].fncTbl.pMotor->pFncGetPosition(&halComponent, &position);
}
/*---------------------------------------------------------------------------*/


#if 0
/*---------------------------------------------------------------------------*/
/** OpenELモーター 制御有効/無効
@param[in]	portID  ポートID
@param[in]	mode 位置決め制御 0:無効 / 1:トルク制御 / 2:速度制御 / 3:位置制御
@return		エラーコード 0:正常 / 0以外:異常(指令を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		位置制御有効はいわゆるサーボONです。
			SON有効
			　位置決め制御は運転(移動)がないときもかかっており、
			　外力に反発するトルクを出して指令位置に留まろうとします。
			　この状態をサーボロックと言います。
			SON無効
			　モーターは通電していません。
			　外力で動かせる状態です。
			トルク制御の実装は任意です。
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorSetCtlMode(uint32_t portID,uint32_t mode);
/*---------------------------------------------------------------------------*/
/** モーター 有効な制御の取得
@param[in]	portID  ポートID
@param[in]	mode 位置決め制御
@return		制御状態 0:無効 / 1:トルク制御 / 2:速度制御 / 3:位置制御
@task		ユーザースレッド (ノンブロッキング)
@detail
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
uint32_t elMotorGetCtlMode(uint32_t portID);
/*---------------------------------------------------------------------------*/
/** OpenELモーター 停止指令(I32)
@param[in]	portID  : ポートID
@param[in]	enBrk 1:ブレーキ有効 / 0:ブレーキ無効
@param[in]	ctlAft : 停止後のサーボ制御 0:無効 / 1:有効
@param[in]	tmDec : 減速時間 [ms](I32),[s](F64) 0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(指令を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
			減速時間 = 0 の場合は最大トルクで減速する
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorSetBrake_I32(uint32_t portID,uint32_t ctlAftBrk,int32_t tmDec) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetBrake(physicalPortID,ctlAftBrk,tmDec);
}
/** OpenELモーター 停止指令(F64) */
int32_t elMotorSetBrake_F64(uint32_t portID,uint32_t ctlAftBrk,double tmDec) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	int32_t tmDecI32 = (int32_t)( tmDec/1000.0 );
	return elPortTbl[portID].fncTbl.pMotor->pFncSetBrake(physicalPortID,ctlAftBrk,tmDecI32);
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置決め運転
@param[in]	portID : ポートID
@param[in]	angle : 絶対位置指令 [ユーザー単位]
@param[in]	speed : 速度 [ユーザー単位/s] 範囲 0より大きい～製品仕様
@param[in]	tmAcc : 加減速時間 [ms](I32),[s](F64) 0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		速度、加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorSetPosition_I32(uint32_t portID,int32_t position,int32_t speed,int32_t tmAcc) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetPosition(physicalPortID,position,speed,tmAcc);
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置決め運転
@param[in]	portID : ポートID
@param[in]	angle : 絶対位置指令 [ユーザー単位]
@param[in]	speed : 速度 [ユーザー単位/s] 範囲 0より大きい～製品仕様
@param[in]	tmAcc : 加減速時間 [ms](I32),[s](F64) 0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		速度、加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorSetPosition_F64(uint32_t portID,double position,double speed,double tmAcc) {
	int32_t posI32,speedI32,tmAccI32;
	double kMov;
	struct elMotorFncTblSt *pMotorFbl;
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif

	pMotorFbl = elPortTbl[portID].fncTbl.pMotor;
	pMotorFbl->pFncGetParam(physicalPortID,ELMOTOR_PRM_K_MOVE,  &kMov, sizeof(kMov) );

	posI32   = (int32_t)( kMov * position );
	speedI32 = (int32_t)( kMov * speed );
	tmAccI32 = (int32_t)( 1000.0 * tmAcc );

	return pMotorFbl->pFncSetPosition(physicalPortID,posI32,speedI32,tmAccI32);
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター 速度制御運転
@param[in]	portID  : ポートID
@param[in]	speed : 速度 [ユーザー単位/s] 範囲 製品仕様
@param[in]	tmAcc : 加減速時間 [ms](I32),[s](F64)  0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		速度0の場合は減速後、停する
			加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorSetSpeed_I32(uint32_t portID,int32_t speed,int32_t tmAcc) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetSpeed(physicalPortID,speed,tmAcc);
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置決め運転
@param[in]	portID : ポートID
@param[in]	speed : 速度 [ユーザー単位/s] 範囲 0より大きい～製品仕様
@param[in]	tmAcc : 加減速時間 [s] 0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		速度、加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorSetSpeed_F64(uint32_t portID,double speed,double tmAcc) {
	int32_t speedI32,tmAccI32;
	double kMov;
	struct elMotorFncTblSt *pMotorFbl;
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif

	pMotorFbl = elPortTbl[portID].fncTbl.pMotor;
	pMotorFbl->pFncGetParam(physicalPortID,ELMOTOR_PRM_K_MOVE,  &kMov, sizeof(kMov) );

	speedI32 = (int32_t)( kMov * speed );
	tmAccI32 = (int32_t)( 1000.0 * tmAcc );

	return pMotorFbl->pFncSetSpeed(physicalPortID,speedI32,tmAccI32);
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター トルク制御運転
@param[in]	portID  : ポートID
@param[in]	trq : トルク 単位は 0.1[%]I(I32) Nm,N(F64) 範囲は製品使用
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
//int32_t elMotorSetTorque_I32(uint32_t portID,int32_t torque);
//int32_t elMotorSetTorque_F64(uint32_t portID,double torque);
/*---------------------------------------------------------------------------*/
/** OpenELモーター 停止指令(I32)
@param[in]	portID  : ポートID
@param[in]	enBrk 1:ブレーキ有効 / 0:ブレーキ無効
@param[in]	ctlAB : 停止後のサーボ制御 0:無効 / 1:有効
@param[in]	tmDec : 減速時間 [ms](I32),[s](F64) 0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(指令を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
			減速時間 = 0 の場合は最大トルクで減速する
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
//int32_t elMotorSetBrakeD_I32(uint32_t portID,uint32_t ctlAB,int32_t tmDec);
//int32_t elMotorSetBrakeD_F64(uint32_t portID,uint32_t ctlAB,double tmDec);
//int32_t elMotorSetBrake(uint32_t portID);
/*---------------------------------------------------------------------------*/
/** OpenELモーター 停止状態の取得
@param[in]	portID  : ポートID
@param[in]	enBrk 1:ブレーキ有効 / 0:ブレーキ無効
@param[in]	tmDec : 減速時間 [s]  0.0以上～製品仕様
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		加減速時間を設定しない関数の場合はパラメーターに従う
			パラメーターID:16で浮動小数点にしている場合はエラー
			減速時間 = 0.0 の場合は最大トルクで減速する
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
//uint32_t elMotorGetBrake(uint32_t portID);
/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置座標の設定
@param[in]	angle : 絶対位置 [ユーザー単位]
@return		エラーコード 0:正常 / 0以外:異常(運転を受け付けられない状態)
@task		ユーザースレッド (ノンブロッキング)
@detail		現在の停止位置で、位置座標を確定させる
			指令位置、検出位置ともに任意の値にする。
			0の場合はResetになります。
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
//int32_t elMotorPreSetPosition_I32(uint32_t portID,int32_t angle);
//double  elMotorPreSetPosition_F64(uint32_t portID,double angle);
//#define elMotorPreSetAngle_I32 elMotorPreSetPosition_I32
//#define elMotorPreSetAngle_F64 elMotorPreSetPosition_F64
/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置の取得(I32)
@param[in]	portID  : ポートID
@param[in]	idx : 0:現在の検出位置 / 1:指定された目標位置 / 2:現在の指令位置
@param[out] pOutPos
@return		エラーコード 0:正常 / 0以外:異常
@task		ユーザースレッド (ノンブロッキング)
@detail
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorGetPosition_I32(uint32_t portID,uint32_t idx,int32_t *pOutPos) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetPosition(physicalPortID,idx,pOutPos);
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター 位置の取得(F64)
@param[in]	portID  : ポートID
@param[in]	idx : 0:現在の検出位置 / 1:指定された目標位置 / 2:現在の指令位置
@param[out] pOutPosition
@return		エラーコード 0:正常 / 0以外:異常
@task		ユーザースレッド (ノンブロッキング)
@detail
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorGetPosition_F64(uint32_t portID,uint32_t idx,double  *pOutPosition) {
	struct elMotorFncTblSt *pMotorFbl;
	double kMov;
	int32_t posI32;
	int32_t retVal;
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif

	pMotorFbl = elPortTbl[portID].fncTbl.pMotor;
	pMotorFbl->pFncGetParam(physicalPortID,ELMOTOR_PRM_K_MOVE,  &kMov, sizeof(kMov) );
	retVal = pMotorFbl->pFncGetPosition(physicalPortID,idx,&posI32);
	*pOutPosition = (double)posI32/kMov;
	return retVal;
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター 速度の取得(I32)
@param[in]	portID  : ポートID
@param[in]	idx : 0:現在の検出速度 / 1:指定された目標速度 / 2:現在の指令速度
@param[out] pOutSpeed 速度
@return		エラーコード 0:正常 / 0以外:異常
@task		ユーザースレッド (ノンブロッキング)
@detail
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorGetSpeed_I32(uint32_t portID,int32_t idx,int32_t *pOutSpeed) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetSpeed(physicalPortID,idx,pOutSpeed);
}
/*---------------------------------------------------------------------------*/
/** OpenELモーター 速度の取得(F64)
@param[in]	portID  : ポートID
@param[in]	idx : 0:現在の検出速度 / 1:指定された目標速度 / 2:現在の指令速度
@param[out] pOutSpeed 速度
@return		エラーコード 0:正常 / 0以外:異常
@task		ユーザースレッド (ノンブロッキング)
@detail
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorGetSpeed_F64(uint32_t portID,int32_t idx,double  *pOutSpeed) {
	struct elMotorFncTblSt *pMotorFbl;
	double kMov;
	int32_t speedI32;
	int32_t retVal;
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif

	pMotorFbl = elPortTbl[portID].fncTbl.pMotor;
	pMotorFbl->pFncGetParam(physicalPortID,ELMOTOR_PRM_K_MOVE,  &kMov, sizeof(kMov) );
	retVal = pMotorFbl->pFncGetSpeed(physicalPortID,idx,&speedI32);
	*pOutSpeed = (double)speedI32/kMov;
	return retVal;
}
/*---------------------------------------------------------------------------*/
/** OpenEL Motor go to HOME
 * @param[in]	portID  : ポートID
 * @param speed 速度
 * @retval エラーコード EL_OK:正常 / EL_OK以外:異常(運転を受け付けられない状態)
 */
int32_t elMotorGoHome_I32(uint32_t portID,uint32_t idx,int32_t speed) {
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}

	return elPortTbl[portID].fncTbl.pMotor->pFncGoHome(physicalPortID,idx,speed);
}
/*---------------------------------------------------------------------------*/
/** OpenEL Motor go to HOME
 * @param[in]	portID  : ポートID
 * @param speed 速度
 * @retval エラーコード EL_OK:正常 / EL_OK以外:異常(運転を受け付けられない状態)
 */
int32_t elMotorGoHome_F64(uint32_t portID,uint32_t idx,double speed) {
	int32_t speedI32;
	double kMov;
	struct elMotorFncTblSt *pMotorFbl;
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}

	pMotorFbl = elPortTbl[portID].fncTbl.pMotor;
	pMotorFbl->pFncGetParam(physicalPortID,ELMOTOR_PRM_K_MOVE,  &kMov, sizeof(kMov) );

	speedI32 = (int32_t)( kMov * speed );

	return elPortTbl[portID].fncTbl.pMotor->pFncGoHome(physicalPortID,idx,speedI32);
}
/*---------------------------------------------------------------------------*/
/** OpenEL Motor callback function setup
 * @param[in] portID : port ID
 * @param[in] idx : selector 0:arriving the position , 1:through the position , 2:arriving the velocity
 * @param[in] pFnc : callback function pointer
 * @param[in] val : position or velocity
 */
int32_t elMotorSetCallBack_I32(uint32_t portID,uint32_t idx,ELMOTOR_P_FNC_CALLBACK pFncCB,int32_t val) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetCallBack(physicalPortID,idx,pFncCB,val);
}
/*---------------------------------------------------------------------------*/
/** OpenEL Motor callback function setup
 * @param[in] portID : port ID
 * @param[in] idx : selector 0:arriving the position , 1:through the position , 2:arriving the velocity
 * @param[in] pFnc : callback function pointer
 * @param[in] val : position or velocity
 */
int32_t elMotorSetCallBack_F64(uint32_t portID,uint32_t idx,ELMOTOR_P_FNC_CALLBACK pFncCB,double val) {
	struct elMotorFncTblSt *pMotorFbl;
	double kMov;
	int32_t valI32;
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	pMotorFbl = elPortTbl[portID].fncTbl.pMotor;
	pMotorFbl->pFncGetParam(physicalPortID,ELMOTOR_PRM_K_MOVE,  &kMov, sizeof(kMov) );
	valI32 = (int32_t)( kMov*val );
	return pMotorFbl->pFncSetCallBack(physicalPortID,idx,pFncCB,valI32);
}
/*---------------------------------------------------------------------------*/


//i 後でopenEL.cに移動する
/* parameter read/write */
/*---------------------------------------------------------------------------*/
/** write  parameter
@param[in]	portID  : port ID
@param[in]	prmID  : parameter ID
@param[in]	*pInVal : Value
@return		error code  0:OK / not 0:error (can not execute)
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorSetParam_U16(uint32_t portID,uint32_t prmID,uint16_t val) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetParam(physicalPortID,prmID,&val,sizeof(val) );
}
int32_t elMotorSetParam_I16(uint32_t portID,uint32_t prmID,int16_t  val) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetParam(physicalPortID,prmID,&val,sizeof(val) );
}
int32_t elMotorSetParam_U32(uint32_t portID,uint32_t prmID,uint32_t val) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetParam(physicalPortID,prmID,&val,sizeof(val) );
}
int32_t elMotorSetParam_I32(uint32_t portID,uint32_t prmID,uint32_t val) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetParam(physicalPortID,prmID,&val,sizeof(val) );
}
int32_t elMotorSetParam_F32(uint32_t portID,uint32_t prmID,float    val) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetParam(physicalPortID,prmID,&val,sizeof(val) );
}
int32_t elMotorSetParam_F64(uint32_t portID,uint32_t prmID,double val) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncSetParam(physicalPortID,prmID,&val,sizeof(val) );
}
/*---------------------------------------------------------------------------*/
/** read parameter
@param[in]	portID  : port ID
@param[in]	prmID  : parameter ID
@param[in]	*pInVal : Value
@return		error code  0:OK / not 0:error (can not execute)
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
int32_t elMotorGetParam_U16(uint32_t portID,uint32_t prmID,uint16_t *pOutVal) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetParam(physicalPortID,prmID,pOutVal,sizeof(*pOutVal) );
}
int32_t elMotorGetParam_I16(uint32_t portID,uint32_t prmID,int16_t  *pOutVal) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetParam(physicalPortID,prmID,pOutVal,sizeof(*pOutVal) );
}
int32_t elMotorGetParam_U32(uint32_t portID,uint32_t prmID,uint32_t *pOutVal) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetParam(physicalPortID,prmID,pOutVal,sizeof(*pOutVal) );
}
int32_t elMotorGetParam_I32(uint32_t portID,uint32_t prmID,int32_t  *pOutVal) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetParam(physicalPortID,prmID,pOutVal,sizeof(*pOutVal) );
}
int32_t elMotorGetParam_F32(uint32_t portID,uint32_t prmID,float    *pOutVal) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetParam(physicalPortID,prmID,pOutVal,sizeof(*pOutVal) );
}
int32_t elMotorGetParam_F64(uint32_t portID,uint32_t prmID,double   *pOutVal) {
#if 1 == EL_SW_FAST
	uint32_t physicalPortID = elPortTbl[portID].physicalPortID;
#else
	uint32_t physicalPortID;
	if ( 0 == (physicalPortID = elGetPhyicalPortID(portID,ELMOTOR_PROPERTY)) ) {
		return EL_ERR_INIT;
	}
#endif
	return elPortTbl[portID].fncTbl.pMotor->pFncGetParam(physicalPortID,prmID,pOutVal,sizeof(*pOutVal) );
}
/*---------------------------------------------------------------------------*/
#endif
