// -*- C++ -*-
/*!
 * @file  RefHardRh2.cpp
 * @brief RefHard VelocutyVersion RH2
 * @date $Date$
 *
 * @author N.Takekawa (takekawa-777@aist.go.jp)
 *
 * Copyright (c) 2008, National Institute of Advanced Industrial Science and Tec
 * hnology (AIST). All rights reserved. This program is made available under the te
 * rms of the Eclipse Public License v1.0 which accompanies this distribution, and 
 * is available at http://www.eclipse.org/legal/epl-v10.html
 *
 * $Id$
 */

#include "RefHardRh2.h"

// Module specification
// <rtc-template block="module_spec">
static const char* refhardrh2_spec[] =
  {
    "implementation_id", "RefHardRh2",
    "type_name",         "RefHardRh2",
    "description",       "RefHard VelocutyVersion RH2",
    "version",           "4.0.0",
    "vendor",            "AIST INVENT",
    "category",          "Motor",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    "exec_cxt.periodic.rate", "1000.0",
    // Configuration variables
    "conf.default.DeviceFileName", "/dev/ttyUSB0",
    "conf.default.gain_P", "10",
    "conf.default.gain_I", "0",
    "conf.default.gain_D", "0",
    "conf.default.gain_VP", "50",
    "conf.default.gain_VI", "0",
    "conf.default.gain_VD", "0",
    "conf.default.Velocity_limit", "10.0",
    "conf.default.Acceleration_limit", "50.0",
    "conf.default.WheelTread", "0.441",
    "conf.default.WheelRadiusLeft", "0.1",
    "conf.default.WheelRadiusRight", "0.1",
    "conf.default.GearRatio", "66.22",
    "conf.default.EncoderCount", "2000",
    "conf.default.EncoderConstant_L", "1.0",
    "conf.default.EncoderConstant_R", "1.0",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
RefHardRh2::RefHardRh2(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_InTargetVelocityIn("InTargetVelocity", m_InTargetVelocity),
    m_OutWheelAngleOut("OutWheelAngle", m_OutWheelAngle),
    m_BumpProvPort("BumpProv"),
    m_InventGUIProvPort("InventGUIProv")

    // </rtc-template>
{
}

/*!
 * @brief destructor
 */
RefHardRh2::~RefHardRh2()
{
}


/*!
 * The device port is opened
 */
RTC::ReturnCode_t RefHardRh2::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("InTargetVelocity", m_InTargetVelocityIn);
  
  // Set OutPort buffer
  registerOutPort("OutWheelAngle", m_OutWheelAngleOut);
  
  // Set service provider to Ports
  m_BumpProvPort.registerProvider("BumpRefHard", "RefHard", m_BumpRefHard);
  m_InventGUIProvPort.registerProvider("InventGUIRefHard", "RefHard", m_InventGUIRefHard);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  registerPort(m_BumpProvPort);
  registerPort(m_InventGUIProvPort);
  
  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("DeviceFileName", m_DeviceFileName, "/dev/ttyUSB0");
  bindParameter("gain_P", m_gain_P, "10");
  bindParameter("gain_I", m_gain_I, "0");
  bindParameter("gain_D", m_gain_D, "0");
  bindParameter("gain_VP", m_gain_VP, "50");
  bindParameter("gain_VI", m_gain_VI, "0");
  bindParameter("gain_VD", m_gain_VD, "0");
  bindParameter("Velocity_limit", m_Velocity_limit, "10.0");
  bindParameter("Acceleration_limit", m_Acceleration_limit, "50.0");
  bindParameter("WheelTread", m_WheelTread, "0.441");
  bindParameter("WheelRadiusLeft", m_WheelRadiusLeft, "0.1");
  bindParameter("WheelRadiusRight", m_WheelRadiusRight, "0.1");
  bindParameter("GearRatio", m_GearRatio, "66.22");
  bindParameter("EncoderCount", m_EncoderCount, "2000");
  bindParameter("EncoderConstant_L", m_EncoderConstant_L, "1.0");
  bindParameter("EncoderConstant_R", m_EncoderConstant_R, "1.0");
  
  // </rtc-template>

/*--------------------Addition---------------------*/
	// port date length
	m_OutWheelAngle.data.length(DOFNUM);
	// prot setting
	std::cout.setf(std::ios_base::fixed,std::ios_base::floatfield);
	std::cout.precision(11);
	#if CALLBACK
//	m_InTargetVelocityIn.setOnWriteConvert(&m_debug);
	#endif
/*--------------------Addition---------------------*/

/*--------------------Addition---------------------*/

 	// -- Check of file name -- //
	std::string fileName= "RefHardRh2.conf";
	if(access(fileName.c_str(), 0)){
		std::cerr << "[ERROR] not found [" << fileName << "]" << std::endl;
		isDev= false;
  	return RTC::RTC_OK;
	}
	// -- File open -- //
	ifstream ConfigurationFile;
	ConfigurationFile.open( fileName.c_str() );
	// -- Extraction of device name -- //
	std::string DeviceName;
	while(1){
		ConfigurationFile >> DeviceName;
		//string::size_type index = DeviceName.find( "DeviceFileName" );
		string::size_type index = DeviceName.find( "conf.default.DeviceFileName:" );
		if( index != string::npos ){
			ConfigurationFile >> DeviceName;
			break;
		}
		if( ConfigurationFile.eof() ) break;
	}
	ConfigurationFile.close();
	// -- Check of Device Name -- //
	if( DeviceName.empty() ){
		std::cerr << "[ERROR] Retrieval failure of DeviceName" << std::endl;
		isDev= false;
  	return RTC::RTC_OK;
	}
  // -- Device Open -- //
	isDev= OpenDev( DeviceName, BAUDRATE );

/*--------------------Addition---------------------*/

  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t RefHardRh2::onFinalize()
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RefHardRh2::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*!
 * close of device port
 */

RTC::ReturnCode_t RefHardRh2::onShutdown(RTC::UniqueId ec_id)
{
	RTC_INFO((" onShutdown"));
	//std::cout << "[INFO] onShutdown" << std::endl;
	// -- Driving stop -- //
	m_veloc[0]= m_veloc[1]= 0;
	if( isDev && svm_set_veloc(serif, m_veloc)==0 )
		RTC_INFO((" The motor is stopped"));
		//std::cout << "[INFO] The motor is stopped" << std::endl;
	// -- Device close -- //
	if(isDev) svm_close(&serif_old);

  return RTC::RTC_OK;
}

/*!
 * Initialization of data
 */

RTC::ReturnCode_t RefHardRh2::onActivated(RTC::UniqueId ec_id)
{
	RTC_INFO((" onActivated"));
	//std::cout << "[INFO] onActivated" << std::endl;

	// -- plot of configuration paramter -- //
 	const char *config_id= m_configsets.getActiveId();
	RTC_INFO(("------------- configuration -------------"));
	RTC_INFO((" Selected Configuration : %s", config_id));
	RTC_INFO((" gain_P : %d", m_gain_P));
	RTC_INFO((" gain_I : %d", m_gain_I));
	RTC_INFO((" gain_D : %d", m_gain_D));
	RTC_INFO((" gain_VP : %d", m_gain_VP));
	RTC_INFO((" gain_VI : %d", m_gain_VI));
	RTC_INFO((" gain_VD : %d", m_gain_VD));
	RTC_INFO((" Velocity_limit : %f", m_Velocity_limit));
	RTC_INFO((" Acceleration_limit : %f", m_Acceleration_limit));
	RTC_INFO((" WheelTread : %f", m_WheelTread));
	RTC_INFO((" WheelRadiusLeft : %f", m_WheelRadiusLeft));
	RTC_INFO((" WheelRadiusRight : %f", m_WheelRadiusRight));
	RTC_INFO((" GearRatio : %f ", m_GearRatio));
	RTC_INFO((" EncoderCount :%f ", m_EncoderCount));
	RTC_INFO((" EncoderConstant_L : %f", m_EncoderConstant_L));
	RTC_INFO((" EncoderConstant_R : %f", m_EncoderConstant_R));
	RTC_INFO(("------------- configuration -------------"));
	//std::cout << "------------- configuration -------------" << std::endl;
	//std::cout << " Selected Configuration : " << config_id << std::endl;
	//std::cout << " gain_P : " << m_gain_P << std::endl;
	//std::cout << " gain_I : " << m_gain_I << std::endl;
	//std::cout << " gain_D : " << m_gain_D << std::endl;
	//std::cout << " gain_VP : " << m_gain_VP << std::endl;
	//std::cout << " gain_VI : " << m_gain_VI << std::endl;
	//std::cout << " gain_VD : " << m_gain_VD << std::endl;
	//std::cout << " Velocity_limit : " << m_Velocity_limit << std::endl;
	//std::cout << " Acceleration_limit : " << m_Acceleration_limit << std::endl;
	//std::cout << " WheelTread : " << m_WheelTread << std::endl;
	//std::cout << " WheelRadiusLeft : " << m_WheelRadiusLeft << std::endl;
	//std::cout << " WheelRadiusRight : " << m_WheelRadiusRight << std::endl;
	//std::cout << " GearRatio : " << m_GearRatio << std::endl;
	//std::cout << " EncoderCount : " << m_EncoderCount << std::endl;
	//std::cout << " EncoderConstant_L : " << m_EncoderConstant_L << std::endl;
	//std::cout << " EncoderConstant_R : " << m_EncoderConstant_R << std::endl;
	//std::cout << "------------- configuration -------------" << std::endl;
  	
  // -- set of RefHw paramter -- //
	m_Rh[0].Direction= 1.0;
	m_Rh[1].Direction= 1.0;
	m_Rh[0].Correction= m_EncoderConstant_L;
	m_Rh[1].Correction= m_EncoderConstant_R;
	m_Rh[0].WheelRadius= m_WheelRadiusLeft;
	m_Rh[1].WheelRadius= m_WheelRadiusRight;
	m_Rh[0].BodyRadius= 0.5 * m_WheelTread;
	m_Rh[1].BodyRadius= 0.5 * m_WheelTread;
	for(int id=0; id<DOFNUM;id++){
		m_Rh[id].toAngle= double( m_EncoderCount * 4.0 / (M_PI*2.0) * m_GearRatio );
		m_Rh[id].toVeloc= (M_PI*2.0) / double( m_EncoderCount * 4.0 * 0.01 * m_GearRatio );
	}
	
	// -- set of Motor Parameter -- //
	svm_param_t param[DOFNUM];
	for(int id=0; id<DOFNUM; id++){
		param[id].p_gain= (uint16_t)m_gain_P;
		param[id].d_gain= (uint16_t)m_gain_D;
		param[id].i_gain= (uint16_t)m_gain_I;
		param[id].vp_gain= (uint16_t)m_gain_VP;
		param[id].vd_gain= (uint16_t)m_gain_VD;
		param[id].vi_gain= (uint16_t)m_gain_VI;
		param[id].veloc_limit= (uint16_t)( m_Velocity_limit / m_Rh[id].toVeloc );
	 	param[id].accel_limit= (uint16_t)( 65000 ); // 固定値
	}
	isDev= ParameterSet( SVM_VELOC, param );
	
	// -- data initialize -- //
  for(int id=0; id<DOFNUM; id++){
  	// Wheel Velocity
 		m_WheelAngularVel[id]= 0.0;
		m_OldWheelAngularVel[id]= 0.0;
		// Wheel angle
		m_OutWheelAngle.data[id]= 0.0;
	}
	// Target Velocity data
	m_TransVel= 0.0;
	m_RotVel= 0.0;
	// Drive Control
	m_DriveCount= 0;
	m_TimeStep= 0.0116;
	
  // -- Time initialize -- //
	#if DEBUG_TIME
	Time_Execute_old= TM2TIME(GetTime());
	#endif
	
	// -- Debugging file open -- //
	#if DEBUG
	VelocityLOG.open("VelocityLOG.dat");
	#endif

  return RTC::RTC_OK;
}

/*!
 * Driving stop
 */

RTC::ReturnCode_t RefHardRh2::onDeactivated(RTC::UniqueId ec_id)
{
	RTC_INFO((" onDeactivated"));
	//std::cout << "[INFO] onDeactivated" << std::endl;
	// -- Driving stop -- //
	m_veloc[0]= m_veloc[1]= 0;
	if( isDev && svm_set_veloc(serif, m_veloc)==0 )
		RTC_INFO((" The motor is stopped"));
		//std::cout << "[INFO] The motor is stopped" << std::endl;
	// -- Debugging file close -- //
	#if DEBUG
	VelocityLOG.close();
	#endif
  return RTC::RTC_OK;
}

/*!
 * The speed instruction is transmitted to the motor
 */

RTC::ReturnCode_t RefHardRh2::onExecute(RTC::UniqueId ec_id)
{
	/***********************************************/
	//  Device Chcek
	/***********************************************/
	if( !isDev ){
		RTC_ERROR((" Device error"));
		//std::cout << "[ERROR] Device error" << std::endl;
		return RTC::RTC_OK;
	}

	/***********************************************/
	//  Get Angle
	/***********************************************/
	if( isDev && svm_get_posit(serif, m_posit)==0 ){
		RTC_INFO((" get Angle data"));
		//std::cout << "[INFO] get Angle data" << std::endl;
		// set of time data
		m_OutWheelAngle.tm= GetTime();
		// set of angle data
		for(int id=0; id<DOFNUM;id++)
			m_OutWheelAngle.data[id]= (double)( m_posit[id] / m_Rh[id].toAngle * m_Rh[id].Direction * m_Rh[id].Correction );
		// write
		m_OutWheelAngleOut.write();
	}else{
		RTC_ERROR((" not get Wheel Angle data"));
		//std::cout << "[ERROR] not get Wheel Angle data" << std::endl;
		isDev= false;
		return RTC::RTC_OK;
	}

	/*---------------------------------------------*/
	//  Get Data Port [Drive] Velocity
	/*---------------------------------------------*/
	if( isDev && m_InTargetVelocityIn.isNew() ){
		RTC_INFO((" get Velocity data"));
		//std::cout << "[INFO] get Velocity data" << std::endl;
		m_InTargetVelocityIn.read();
		// set of TargetVelocity data
		m_TransVel= m_InTargetVelocity.data.vx;  // [m/s]
		m_RotVel= m_InTargetVelocity.data.va;    // [rad/s]
		// Count reset
		m_DriveCount= 0;
		// Stop Flag reset
		isStop= false;
	}else{
		RTC_INFO((" not get Velocity data"));
		//std::cout << "[INFO] not get Velocity data" << std::endl;
		// count increase
		m_DriveCount++;

/*  GamePadからの入力でこの部分が反応して動きがガクガクするため
		とりあえずコメントアウト（2011.4.24）		
		if( m_DriveCount > 9 ){
			m_TransVel= 0.0;
			m_RotVel= 0.0;
			isStop= true;
			RTC_WARN((" WheelVelocity zero"));
			//std::cout << "[WARNING] WheelVelocity zero" << std::endl;
		}*/
	}

	/*---------------------------------------------*/
	//  Get Service Port [Bump] Stop
	/*---------------------------------------------*/
	if( isDev && m_BumpRefHard.isStop ){
		m_TransVel= 0.0;
		m_RotVel= 0.0;
		m_BumpRefHard.isStop= false;
		isStop= true;
	}
	
	/*---------------------------------------------*/
	//  Get Service Port [InventGUI] Stop
	/*---------------------------------------------*/
	if( isDev && m_InventGUIRefHard.isStop ){
		m_TransVel= 0.0;
		m_RotVel= 0.0;
		m_InventGUIRefHard.isStop= false;
		isStop= true;
	}
	
	/*---------------------------------------------*/
	//  Get Service Port [InventGUI] Go
	/*---------------------------------------------*/
	if( isDev && m_InventGUIRefHard.isGo ){
		m_InventGUIRefHard.isGo= false;
		isStop= false;
	}
	
	/***********************************************/
	//  Set Velocity Data
	/***********************************************/
	if( isDev ){
		// Driving Stop ?
		if( isStop ){
			m_veloc[0]= m_veloc[1]= 0;
		}else{
			for(int id=0; id<DOFNUM; id++){
				// -- set of Angular Velocity data -- //
				if (m_RotVel == 0.0)
					m_WheelAngularVel[id]= m_TransVel / m_Rh[id].WheelRadius;
				else{
					if(id == 0) m_WheelAngularVel[id]= ((m_TransVel / m_RotVel) - m_Rh[id].BodyRadius) * m_RotVel / m_Rh[id].WheelRadius;
					if(id == 1) m_WheelAngularVel[id]= ((m_TransVel / m_RotVel) + m_Rh[id].BodyRadius) * m_RotVel / m_Rh[id].WheelRadius;
				}
				// -- Acceleration limitation -- //
				double LimitVel= m_Acceleration_limit * m_TimeStep;
				double LimitAcc= LimitVel / m_Rh[id].WheelRadius;
				double WheelAcc= fabs(m_WheelAngularVel[id] - m_OldWheelAngularVel[id]);
				if( WheelAcc > LimitAcc ){
					if( m_WheelAngularVel[id] > m_OldWheelAngularVel[id] ){
						RTC_INFO(("Limitation No.%d :WheelAngularVel [%f] -> [%f]",id,m_WheelAngularVel[id],m_OldWheelAngularVel[id] + LimitAcc));
						//printf("Limitation No.%d :WheelAngularVel [%f] -> [%f]\n",id,m_WheelAngularVel[id],m_OldWheelAngularVel[id] + LimitAcc);
						m_WheelAngularVel[id]= m_OldWheelAngularVel[id] + LimitAcc;
					}else{
						RTC_INFO(("Limitation No.%d :WheelAngularVel [%f] -> [%f]",id,m_WheelAngularVel[id],m_OldWheelAngularVel[id] - LimitAcc));
						//printf("Limitation No.%d :WheelAngularVel [%f] -> [%f]\n",id,m_WheelAngularVel[id],m_OldWheelAngularVel[id] - LimitAcc);
						m_WheelAngularVel[id]= m_OldWheelAngularVel[id] - LimitAcc;
					}
				}
				m_OldWheelAngularVel[id]= m_WheelAngularVel[id];
				// -- set of Velocity Command -- //
				m_veloc[id]= (int16_t)( m_WheelAngularVel[id] / m_Rh[id].toVeloc );
			} // end of for
		} // end of if
		// -- Velocity set [motor] -- //
		if( isDev && svm_set_veloc(serif, m_veloc)!=0 ){
			RTC_ERROR((" Velocity set error"));
			//std::cout << "[ERROR] Velocity set error" << std::endl;
			isDev= false;
			return RTC::RTC_OK;
		}
	}
	
	/***********************************************/
	//  Data plot
	/***********************************************/
	RTC_INFO((" --- Angle Data"));
	RTC_INFO((" Time:%f", TM2TIME(m_OutWheelAngle.tm)));
	RTC_INFO((" sec:%ld nsec:%ld", m_OutWheelAngle.tm.sec, m_OutWheelAngle.tm.nsec));
	RTC_INFO((" L-Angle:%f R-Angle:%f", m_OutWheelAngle.data[0], m_OutWheelAngle.data[1]));
	RTC_INFO((" --- Velocity Data"));
	RTC_INFO((" Time:%f", TM2TIME(m_InTargetVelocity.tm)));
	RTC_INFO((" sec:%ld nsec:%ld", m_InTargetVelocity.tm.sec, m_InTargetVelocity.tm.nsec));
	RTC_INFO((" m/s:%f rad/s:%f", m_TransVel, m_RotVel));
	RTC_INFO((" R-WheelVel:%f L-WheelVel:%f", m_WheelAngularVel[0], m_WheelAngularVel[1]));
	RTC_INFO((" --- Motor Data"));
	RTC_INFO((" L-Encoder:%f R-Encoder:%f", m_posit[0], m_posit[1]));
	RTC_INFO((" L-veloc:%f R-veloc:%f", m_veloc[0],  m_veloc[1]));
	//std::cout << " --- Angle Data" << std::endl;
	//std::cout << " Time:" << TM2TIME(m_OutWheelAngle.tm) << std::endl;
	//std::cout << " sec:" << m_OutWheelAngle.tm.sec << " nsec:" << m_OutWheelAngle.tm.nsec << std::endl;
	//std::cout << " L-Angle:" << m_OutWheelAngle.data[0] << " R-Angle:" << m_OutWheelAngle.data[1] << std::endl;
	//std::cout << " --- Velocity Data" << std::endl;
	//std::cout << " Time:" << TM2TIME(m_InTargetVelocity.tm) << std::endl;
	//std::cout << " sec:" << m_InTargetVelocity.tm.sec << " nsec:" << m_InTargetVelocity.tm.nsec << std::endl;
	//std::cout << " m/s:" << m_TransVel << " rad/s:" << m_RotVel << std::endl;
	//std::cout << " R-WheelVel:" << m_WheelAngularVel[0] << " L-WheelVel:" << m_WheelAngularVel[1] << std::endl;
	//std::cout << " --- Motor Data" << std::endl;
	//std::cout << " L-Encoder:" << m_posit[0] << " R-Encoder:" << m_posit[1] << std::endl;
	//std::cout << " L-veloc:" << m_veloc[0] << " R-veloc:" << m_veloc[1] << std::endl;
	
	#if DEBUG
	VelocityLOG << TM2TIME(m_OutWheelAngle.tm) << 
	" " << m_WheelAngularVel[0] << 
	" " << m_WheelAngularVel[1] <<
	" " << m_OutWheelAngle.data[0] <<
	" " << m_OutWheelAngle.data[1] << std::endl;
	#endif
	
	#if DEBUG_TIME
	Time_Execute= TM2TIME(GetTime());
	std::cout << " ExecuteTime: " << Time_Execute << std::endl;
	std::cout << " ExecuteTime(Cycle): " << Time_Execute - Time_Execute_old << std::endl;
	Time_Execute_old= Time_Execute;
	#endif
	
	/*
	if( m_BumpRefHard.isBump ){
		sleep(3);
		m_BumpRefHard.isBump= false;
		isStop= false;
	}
	*/


  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t RefHardRh2::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RefHardRh2::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RefHardRh2::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RefHardRh2::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RefHardRh2::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*********************************************************************/
//													OpenDev
/*********************************************************************/
bool RefHardRh2::OpenDev( std::string Device, int Baudrate )
{
	// -- setserial [RS485] -- //
	RTC_INFO((" Device Port setup"));
	//std::cout << "[INFO] Device Port setup" << std::endl;
	char call[50]= "setserial ";
	strcat(call, Device.c_str());
	strcat(call, " port 0x1");
	int ret= system(call);
	if( !WIFEXITED(ret) ) return false;
	// -- Device File Open -- //
	if( access(Device.c_str(), 0) ){
		RTC_ERROR((" Device File %s not found", Device.c_str()));
		//std::cout << "[ERROR] Device File " << Device << " not found" << std::endl;
		return false;
	}
	RTC_INFO((" Device File : %s", Device.c_str()));
	//std::cout << "[INFO] Device File : " << Device << std::endl;
  if( svm_open( &serif, Device.c_str(), DOFNUM, Baudrate ) != 0 ){
  	RTC_ERROR((" Device Open Error"));
  	//std::cout << "[ERROR] Device Open Error" << std::endl;
		return false;
  }
  RTC_INFO((" Device Open"));
  //std::cout << "[INFO] Device Open" << std::endl;
  serif_old= serif; // required to exit by Ctrl-C
	return true;
}

/*********************************************************************/
//													ParameterSet
/*********************************************************************/
bool RefHardRh2::ParameterSet( svm_mode_t Mode, svm_param_t param[] )
{
	// -- set of configuration -- //
	RTC_INFO((" Set config"));
	//std::cout << "[INFO] Set config" << std::endl;
	svm_config_t config[DOFNUM];
	for(int id=0; id<DOFNUM; id++){
		config[id].version= (uint8_t)0;
		config[id].lower_limit= -0x80000000;// [(int32_t)-2147483648] (ISO C90)
		config[id].upper_limit= 0x7fffffff;// [(int32_t)2147483647] (ISO C90)
		config[id].calib_offset= (int32_t)0;
	}
	config[0].enc_dir= (int8_t)-1;
	config[0].drive_dir= (int8_t)-1;
	config[1].enc_dir= (int8_t)1;
	config[1].drive_dir= (int8_t)1;
	// set
	if( svm_set_config(serif, config) < 0 ){
		RTC_ERROR((" Set config Error"))
		//std::cout << "[ERROR] Set config Error" << std::endl;
		svm_close(&serif);
		return false;
	}
	
	// -- set of parameters -- //
	RTC_INFO((" Set Parameters"));
	//std::cout << "[INFO] Set Parameters" << std::endl;
  if( svm_set_param(serif, param) < 0 ){
  	RTC_ERROR((" Set Parameters Error"));
  	//std::cout << "[ERROR] Set Parameters Error" << std::endl;
    svm_close(&serif);
		return false;
  }
  
	// -- set mode -- //
	RTC_INFO(("[INFO] Set mode"));
	//std::cout << "[INFO] Set mode" << std::endl;
  if( svm_set_mode(serif, Mode) < 0 ){
    RTC_ERROR((" Set mode Error"));
    //std::cout << "[ERROR] Set mode Error" << std::endl;
    svm_close(&serif);
		return false;
  }
  
	// -- Read Parameters -- //
	svm_param_t m_Getparam[DOFNUM];
	RTC_INFO((" Read Parameters"));
	//std::cout << "[INFO] Read Parameters" << std::endl;
  if( svm_read_param(serif, 0, m_Getparam) < 0 ){
  	RTC_ERROR((" Read Parameters Error"));
  	//std::cout << "[ERROR] Read Parameters Error" << std::endl;
    svm_close(&serif);
		return false;
	}
	RTC_INFO(("Parameters [L] Motor"));
	RTC_INFO(("p_gain          : %10d",m_Getparam[0].p_gain));
	RTC_INFO(("d_gain          : %10d",m_Getparam[0].d_gain));
	RTC_INFO(("i_gain          : %10d",m_Getparam[0].i_gain));
	RTC_INFO(("vp_gain         : %10d",m_Getparam[0].vp_gain));
	RTC_INFO(("vd_gain         : %10d",m_Getparam[0].vd_gain));
	RTC_INFO(("vi_gain         : %10d",m_Getparam[0].vi_gain));
	RTC_INFO(("veloc_limit     : %10d",m_Getparam[0].veloc_limit));
	RTC_INFO(("accel_limit     : %10d",m_Getparam[0].accel_limit));
	//printf("Parameters [L] Motor\n");
	//printf("p_gain          : %10d\n",m_Getparam[0].p_gain);
	//printf("d_gain          : %10d\n",m_Getparam[0].d_gain);
	//printf("i_gain          : %10d\n",m_Getparam[0].i_gain);
	//printf("vp_gain         : %10d\n",m_Getparam[0].vp_gain);
	//printf("vd_gain         : %10d\n",m_Getparam[0].vd_gain);
	//printf("vi_gain         : %10d\n",m_Getparam[0].vi_gain);
	//printf("veloc_limit     : %10d\n",m_Getparam[0].veloc_limit);
	//printf("accel_limit     : %10d\n",m_Getparam[0].accel_limit);
  if( svm_read_param(serif, 1, m_Getparam) < 0 ){
  	RTC_ERROR((" Read Parameters Error"));
  	//std::cout << "[ERROR] Read Parameters Error" << std::endl;
    svm_close(&serif);
		return false;
	}
	RTC_INFO(("Parameters [R] Motor"));
	RTC_INFO(("p_gain          : %10d",m_Getparam[0].p_gain));
	RTC_INFO(("d_gain          : %10d",m_Getparam[0].d_gain));
	RTC_INFO(("i_gain          : %10d",m_Getparam[0].i_gain));
	RTC_INFO(("vp_gain         : %10d",m_Getparam[0].vp_gain));
	RTC_INFO(("vd_gain         : %10d",m_Getparam[0].vd_gain));
	RTC_INFO(("vi_gain         : %10d",m_Getparam[0].vi_gain));
	RTC_INFO(("veloc_limit     : %10d",m_Getparam[0].veloc_limit));
	RTC_INFO(("accel_limit     : %10d",m_Getparam[0].accel_limit));
	//printf("Parameters [R] Motor\n");
	//printf("p_gain          : %10d\n",m_Getparam[0].p_gain);
	//printf("d_gain          : %10d\n",m_Getparam[0].d_gain);
	//printf("i_gain          : %10d\n",m_Getparam[0].i_gain);
	//printf("vp_gain         : %10d\n",m_Getparam[0].vp_gain);
	//printf("vd_gain         : %10d\n",m_Getparam[0].vd_gain);
	//printf("vi_gain         : %10d\n",m_Getparam[0].vi_gain);
	//printf("veloc_limit     : %10d\n",m_Getparam[0].veloc_limit);
	//printf("accel_limit     : %10d\n",m_Getparam[0].accel_limit);
	
	// -- Read config -- //
	svm_config_t m_Getconfig[DOFNUM];
	RTC_INFO((" Read configuration"));
	//std::cout << "[INFO] Read configuration" << std::endl;
  if( svm_read_config(serif, 0, m_Getconfig) < 0 ){
  	std::cout << "[ERROR] Read configuration Error" << std::endl;
    svm_close(&serif);
		return false;
  }
	RTC_INFO(("configuration [L] Motor\n"));
	RTC_INFO(("version         : %10d\n",m_Getconfig[0].version));
	RTC_INFO(("lower_limit     : %10d\n",m_Getconfig[0].lower_limit));
	RTC_INFO(("upper_limit     : %10d\n",m_Getconfig[0].upper_limit));
	RTC_INFO(("enc_dir         : %10d\n",m_Getconfig[0].enc_dir));
	RTC_INFO(("drive_dir       : %10d\n",m_Getconfig[0].drive_dir));
	RTC_INFO(("calib_offset    : %10d\n",m_Getconfig[0].calib_offset));
	//printf("configuration [L] Motor\n");
	//printf("version         : %10d\n",m_Getconfig[0].version);
	//printf("lower_limit     : %10d\n",m_Getconfig[0].lower_limit);
	//printf("upper_limit     : %10d\n",m_Getconfig[0].upper_limit);
	//printf("enc_dir         : %10d\n",m_Getconfig[0].enc_dir);
	//printf("drive_dir       : %10d\n",m_Getconfig[0].drive_dir);
	//printf("calib_offset    : %10d\n",m_Getconfig[0].calib_offset);
  if( svm_read_config(serif, 1, m_Getconfig) < 0 ){
  	RTC_ERROR((" Read configuration Error"));
  	//std::cout << "[ERROR] Read configuration Error" << std::endl;
    svm_close(&serif);
		return false;
  }
	RTC_INFO(("configuration [R] Motor\n"));
	RTC_INFO(("version         : %10d\n",m_Getconfig[0].version));
	RTC_INFO(("lower_limit     : %10d\n",m_Getconfig[0].lower_limit));
	RTC_INFO(("upper_limit     : %10d\n",m_Getconfig[0].upper_limit));
	RTC_INFO(("enc_dir         : %10d\n",m_Getconfig[0].enc_dir));
	RTC_INFO(("drive_dir       : %10d\n",m_Getconfig[0].drive_dir));
	RTC_INFO(("calib_offset    : %10d\n",m_Getconfig[0].calib_offset));
	//printf("configuration [R] Motor\n");
	//printf("version         : %10d\n",m_Getconfig[0].version);
	//printf("lower_limit     : %10d\n",m_Getconfig[0].lower_limit);
	//printf("upper_limit     : %10d\n",m_Getconfig[0].upper_limit);
	//printf("enc_dir         : %10d\n",m_Getconfig[0].enc_dir);
	//printf("drive_dir       : %10d\n",m_Getconfig[0].drive_dir);
	//printf("calib_offset    : %10d\n",m_Getconfig[0].calib_offset);
		
	return true;
}

extern "C"
{
 
  void RefHardRh2Init(RTC::Manager* manager)
  {
    coil::Properties profile(refhardrh2_spec);
    manager->registerFactory(profile,
                             RTC::Create<RefHardRh2>,
                             RTC::Delete<RefHardRh2>);
  }
  
};


