// -*- C++ -*-
/*!
 * @file  RH2ArmControl.cpp
 * @brief ArmController
 * @date $Date$
 *
 * @author N.Takekawa (takekawa-777@aist.go.jp)
 *
 * Copyright (c) 2008, National Institute
 * of Advanced Industrial Science and Technology (AIST).
 * All rights reserved. This program is made available under the terms 
 * 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 "RH2ArmControl.h"

// Module specification
// <rtc-template block="module_spec">
static const char* rh2armcontrol_spec[] =
  {
    "implementation_id", "RH2ArmControl",
    "type_name",         "RH2ArmControl",
    "description",       "ArmController",
    "version",           "3.0.0",
    "vendor",            "AIST INVENT",
    "category",          "Controller",
    "activity_type",     "EVENT_DRIVEN",
    "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.Pgain_1", "10",
    "conf.default.Pgain_2", "10",
    "conf.default.Pgain_3", "14",
    "conf.default.Pgain_4", "14",
    "conf.default.Pgain_5", "10",
    "conf.default.Pgain_6", "50",
    "conf.default.Pgain_7", "50",
    "conf.default.Igain_1", "0",
    "conf.default.Igain_2", "0",
    "conf.default.Igain_3", "0",
    "conf.default.Igain_4", "0",
    "conf.default.Igain_5", "0",
    "conf.default.Igain_6", "0",
    "conf.default.Igain_7", "0",
    "conf.default.Dgain_1", "0",
    "conf.default.Dgain_2", "0",
    "conf.default.Dgain_3", "0",
    "conf.default.Dgain_4", "0",
    "conf.default.Dgain_5", "0",
    "conf.default.Dgain_6", "0",
    "conf.default.Dgain_7", "0",
    "conf.default.VPgain_1", "20",
    "conf.default.VPgain_2", "50",
    "conf.default.VPgain_3", "100",
    "conf.default.VPgain_4", "150",
    "conf.default.VPgain_5", "50",
    "conf.default.VPgain_6", "150",
    "conf.default.VPgain_7", "250",
    "conf.default.VIgain_1", "0",
    "conf.default.VIgain_2", "0",
    "conf.default.VIgain_3", "0",
    "conf.default.VIgain_4", "0",
    "conf.default.VIgain_5", "0",
    "conf.default.VIgain_6", "0",
    "conf.default.VIgain_7", "0",
    "conf.default.VDgain_1", "0",
    "conf.default.VDgain_2", "0",
    "conf.default.VDgain_3", "0",
    "conf.default.VDgain_4", "0",
    "conf.default.VDgain_5", "0",
    "conf.default.VDgain_6", "0",
    "conf.default.VDgain_7", "0",
    "conf.default.VelocLimit_1", "1.04",
    "conf.default.VelocLimit_2", "0.52",
    "conf.default.VelocLimit_3", "0.4",
    "conf.default.VelocLimit_4", "0.88",
    "conf.default.VelocLimit_5", "1.11",
    "conf.default.VelocLimit_6", "2.45",
    "conf.default.VelocLimit_7", "0.013",
    "conf.default.AccelLimit_1", "2.09",
    "conf.default.AccelLimit_2", "1.84",
    "conf.default.AccelLimit_3", "2.04",
    "conf.default.AccelLimit_4", "4.41",
    "conf.default.AccelLimit_5", "5.57",
    "conf.default.AccelLimit_6", "12.27",
    "conf.default.AccelLimit_7", "0.08",
    "conf.default.GearRatio_1", "300.0",
    "conf.default.GearRatio_2", "333.0",
    "conf.default.GearRatio_3", "300.0",
    "conf.default.GearRatio_4", "360.0",
    "conf.default.GearRatio_5", "300.0",
    "conf.default.GearRatio_6", "100.0",
    "conf.default.GearRatio_7", "4.4",
    "conf.default.EncoderCount_1", "1000",
    "conf.default.EncoderCount_2", "1024",
    "conf.default.EncoderCount_3", "1024",
    "conf.default.EncoderCount_4", "512",
    "conf.default.EncoderCount_5", "512",
    "conf.default.EncoderCount_6", "512",
    "conf.default.EncoderCount_7", "512",
    // System Configuration
    "corba.nameservers", "localhost:2809",
    "naming.formats", "%n.rtc",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
RH2ArmControl::RH2ArmControl(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_InArmVelocityIn("InArmVelocity", m_InArmVelocity),
    m_OutArmAngleOut("OutArmAngle", m_OutArmAngle),

    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("InArmVelocity", m_InArmVelocityIn);
  
  // Set OutPort buffer
  registerOutPort("OutArmAngle", m_OutArmAngleOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>
  
/*--------------------Addition---------------------*/
  // port date length
  m_OutArmAngle.data.length(ARMNUM);
	// prot setting
	std::cout.setf(std::ios_base::fixed,std::ios_base::floatfield);
	std::cout.precision(11);
/*--------------------Addition---------------------*/

}

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


/*!
 * The device port is opened
 */
RTC::ReturnCode_t RH2ArmControl::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("DeviceFileName", m_DeviceFileName, "/dev/ttyUSB0");
  bindParameter("Pgain_1", m_Pgain_1, "10");
  bindParameter("Pgain_2", m_Pgain_2, "10");
  bindParameter("Pgain_3", m_Pgain_3, "14");
  bindParameter("Pgain_4", m_Pgain_4, "14");
  bindParameter("Pgain_5", m_Pgain_5, "10");
  bindParameter("Pgain_6", m_Pgain_6, "50");
  bindParameter("Pgain_7", m_Pgain_7, "50");
  bindParameter("Igain_1", m_Igain_1, "0");
  bindParameter("Igain_2", m_Igain_2, "0");
  bindParameter("Igain_3", m_Igain_3, "0");
  bindParameter("Igain_4", m_Igain_4, "0");
  bindParameter("Igain_5", m_Igain_5, "0");
  bindParameter("Igain_6", m_Igain_6, "0");
  bindParameter("Igain_7", m_Igain_7, "0");
  bindParameter("Dgain_1", m_Dgain_1, "0");
  bindParameter("Dgain_2", m_Dgain_2, "0");
  bindParameter("Dgain_3", m_Dgain_3, "0");
  bindParameter("Dgain_4", m_Dgain_4, "0");
  bindParameter("Dgain_5", m_Dgain_5, "0");
  bindParameter("Dgain_6", m_Dgain_6, "0");
  bindParameter("Dgain_7", m_Dgain_7, "0");
  bindParameter("VPgain_1", m_VPgain_1, "20");
  bindParameter("VPgain_2", m_VPgain_2, "50");
  bindParameter("VPgain_3", m_VPgain_3, "100");
  bindParameter("VPgain_4", m_VPgain_4, "150");
  bindParameter("VPgain_5", m_VPgain_5, "50");
  bindParameter("VPgain_6", m_VPgain_6, "150");
  bindParameter("VPgain_7", m_VPgain_7, "250");
  bindParameter("VIgain_1", m_VIgain_1, "0");
  bindParameter("VIgain_2", m_VIgain_2, "0");
  bindParameter("VIgain_3", m_VIgain_3, "0");
  bindParameter("VIgain_4", m_VIgain_4, "0");
  bindParameter("VIgain_5", m_VIgain_5, "0");
  bindParameter("VIgain_6", m_VIgain_6, "0");
  bindParameter("VIgain_7", m_VIgain_7, "0");
  bindParameter("VDgain_1", m_VDgain_1, "0");
  bindParameter("VDgain_2", m_VDgain_2, "0");
  bindParameter("VDgain_3", m_VDgain_3, "0");
  bindParameter("VDgain_4", m_VDgain_4, "0");
  bindParameter("VDgain_5", m_VDgain_5, "0");
  bindParameter("VDgain_6", m_VDgain_6, "0");
  bindParameter("VDgain_7", m_VDgain_7, "0");
  bindParameter("VelocLimit_1", m_VelocLimit_1, "1.04");
  bindParameter("VelocLimit_2", m_VelocLimit_2, "0.52");
  bindParameter("VelocLimit_3", m_VelocLimit_3, "0.4");
  bindParameter("VelocLimit_4", m_VelocLimit_4, "0.88");
  bindParameter("VelocLimit_5", m_VelocLimit_5, "1.11");
  bindParameter("VelocLimit_6", m_VelocLimit_6, "2.45");
  bindParameter("VelocLimit_7", m_VelocLimit_7, "0.013");
  bindParameter("AccelLimit_1", m_AccelLimit_1, "2.09");
  bindParameter("AccelLimit_2", m_AccelLimit_2, "1.84");
  bindParameter("AccelLimit_3", m_AccelLimit_3, "2.04");
  bindParameter("AccelLimit_4", m_AccelLimit_4, "4.41");
  bindParameter("AccelLimit_5", m_AccelLimit_5, "5.57");
  bindParameter("AccelLimit_6", m_AccelLimit_6, "12.27");
  bindParameter("AccelLimit_7", m_AccelLimit_7, "0.08");
  bindParameter("GearRatio_1", m_GearRatio_1, "300.0");
  bindParameter("GearRatio_2", m_GearRatio_2, "333.0");
  bindParameter("GearRatio_3", m_GearRatio_3, "300.0");
  bindParameter("GearRatio_4", m_GearRatio_4, "360.0");
  bindParameter("GearRatio_5", m_GearRatio_5, "300.0");
  bindParameter("GearRatio_6", m_GearRatio_6, "100.0");
  bindParameter("GearRatio_7", m_GearRatio_7, "4.4");
  bindParameter("EncoderCount_1", m_EncoderCount_1, "1000");
  bindParameter("EncoderCount_2", m_EncoderCount_2, "1024");
  bindParameter("EncoderCount_3", m_EncoderCount_3, "1024");
  bindParameter("EncoderCount_4", m_EncoderCount_4, "512");
  bindParameter("EncoderCount_5", m_EncoderCount_5, "512");
  bindParameter("EncoderCount_6", m_EncoderCount_6, "512");
  bindParameter("EncoderCount_7", m_EncoderCount_7, "512");

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

 	// -- Check of file name -- //
	std::string fileName= "RH2ArmControl.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" );
		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 );
	// -- Calibration -- //
	if(isDev) Calibration();
	
/*--------------------Addition---------------------*/

  // </rtc-template>
  return RTC::RTC_OK;
}

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

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

/*!
 * close of device port
 */

RTC::ReturnCode_t RH2ArmControl::onShutdown(RTC::UniqueId ec_id)
{
	RTC_INFO((" onShutdown"));
	//std::cout << "[INFO] onShutdown" << std::endl;
	// -- Driving stop -- //
	for(int id=0; id<ARMNUM;id++) m_veloc[id]= 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);
	
	// -- Debugging file close -- //
	#if DEBUG
	VelocityLOG.close();
	# endif
	
  return RTC::RTC_OK;
}

/*!
 * Initialization of data
 */

RTC::ReturnCode_t RH2ArmControl::onActivated(RTC::UniqueId ec_id)
{

	RTC_INFO((" onActivated"));
	//std::cout << "[INFO] onActivated" << std::endl;
	
	// -- plot of configuration paramter -- //
	RTC_INFO(("------------- configuration -------------"));
	RTC_INFO(("Pgain Igain Dgain VelocLimit AccelLimit GearRatio EncoderCount"));
	RTC_INFO(("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d",m_Pgain_1,m_Igain_1,m_Dgain_1,m_VPgain_1,m_VIgain_1,m_VDgain_1,m_VelocLimit_1,m_AccelLimit_1,m_GearRatio_1,m_EncoderCount_1));
	RTC_INFO(("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d",m_Pgain_2,m_Igain_2,m_Dgain_2,m_VPgain_2,m_VIgain_2,m_VDgain_2,m_VelocLimit_2,m_AccelLimit_2,m_GearRatio_2,m_EncoderCount_2));
	RTC_INFO(("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d",m_Pgain_3,m_Igain_3,m_Dgain_3,m_VPgain_3,m_VIgain_3,m_VDgain_3,m_VelocLimit_3,m_AccelLimit_3,m_GearRatio_3,m_EncoderCount_3));
	RTC_INFO(("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d",m_Pgain_4,m_Igain_4,m_Dgain_4,m_VPgain_4,m_VIgain_4,m_VDgain_4,m_VelocLimit_4,m_AccelLimit_4,m_GearRatio_4,m_EncoderCount_4));
	RTC_INFO(("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d",m_Pgain_5,m_Igain_5,m_Dgain_5,m_VPgain_5,m_VIgain_5,m_VDgain_5,m_VelocLimit_5,m_AccelLimit_5,m_GearRatio_5,m_EncoderCount_5));
	RTC_INFO(("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d",m_Pgain_6,m_Igain_6,m_Dgain_6,m_VPgain_6,m_VIgain_6,m_VDgain_6,m_VelocLimit_6,m_AccelLimit_6,m_GearRatio_6,m_EncoderCount_6));
	RTC_INFO(("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d",m_Pgain_7,m_Igain_7,m_Dgain_7,m_VPgain_7,m_VIgain_7,m_VDgain_7,m_VelocLimit_7,m_AccelLimit_7,m_GearRatio_7,m_EncoderCount_7));
	RTC_INFO(("------------- configuration -------------"));
	//std::cout << "------------- configuration -------------" << std::endl;
	//printf("Pgain Igain Dgain VelocLimit AccelLimit GearRatio EncoderCount\n");
	//printf("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d\n", m_Pgain_1, m_Igain_1, m_Dgain_1, m_VPgain_1, m_VIgain_1, m_VDgain_1, m_VelocLimit_1, m_AccelLimit_1, m_GearRatio_1, m_EncoderCount_1);
	//printf("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d\n", m_Pgain_2, m_Igain_2, m_Dgain_2, m_VPgain_2, m_VIgain_2, m_VDgain_2, m_VelocLimit_2, m_AccelLimit_2, m_GearRatio_2, m_EncoderCount_2);
	//printf("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d\n", m_Pgain_3, m_Igain_3, m_Dgain_3, m_VPgain_3, m_VIgain_3, m_VDgain_3, m_VelocLimit_3, m_AccelLimit_3, m_GearRatio_3, m_EncoderCount_3);
	//printf("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d\n", m_Pgain_4, m_Igain_4, m_Dgain_4, m_VPgain_4, m_VIgain_4, m_VDgain_4, m_VelocLimit_4, m_AccelLimit_4, m_GearRatio_4, m_EncoderCount_4);
	//printf("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d\n", m_Pgain_5, m_Igain_5, m_Dgain_5, m_VPgain_5, m_VIgain_5, m_VDgain_5, m_VelocLimit_5, m_AccelLimit_5, m_GearRatio_5, m_EncoderCount_5);
	//printf("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d\n", m_Pgain_6, m_Igain_6, m_Dgain_6, m_VPgain_6, m_VIgain_6, m_VDgain_6, m_VelocLimit_6, m_AccelLimit_6, m_GearRatio_6, m_EncoderCount_6);
	//printf("%3d %3d %3d %3d %3d %3d %2.3f %2.3f %2.3f %4d\n", m_Pgain_7, m_Igain_7, m_Dgain_7, m_VPgain_7, m_VIgain_7, m_VDgain_7, m_VelocLimit_7, m_AccelLimit_7, m_GearRatio_7, m_EncoderCount_7);
	//std::cout << "------------- configuration -------------" << std::endl;
	
	// -- paramter initialize -- //
	// Gear Ratio
	double GearRatio[ARMNUM];
  GearRatio[0]= m_GearRatio_1;
  GearRatio[1]= m_GearRatio_2;
  GearRatio[2]= m_GearRatio_3;
  GearRatio[3]= m_GearRatio_4;
  GearRatio[4]= m_GearRatio_5;
  GearRatio[5]= m_GearRatio_6;
  GearRatio[6]= m_GearRatio_7;
  // Encoder Count
  double Encoder[ARMNUM];
  Encoder[0]= double( m_EncoderCount_1 );
  Encoder[1]= double( m_EncoderCount_2 );
  Encoder[2]= double( m_EncoderCount_3 );
  Encoder[3]= double( m_EncoderCount_4 );
  Encoder[4]= double( m_EncoderCount_5 );
  Encoder[5]= double( m_EncoderCount_6 );
  Encoder[6]= double( m_EncoderCount_7 );
  
  // P gain
	m_param[0].p_gain= m_Pgain_1;
	m_param[1].p_gain= m_Pgain_2;
	m_param[2].p_gain= m_Pgain_3;
	m_param[3].p_gain= m_Pgain_4;
	m_param[4].p_gain= m_Pgain_5;
	m_param[5].p_gain= m_Pgain_6;
	m_param[6].p_gain= m_Pgain_7;
	// I gain
	m_param[0].i_gain= m_Igain_1;
	m_param[1].i_gain= m_Igain_2;
	m_param[2].i_gain= m_Igain_3;
	m_param[3].i_gain= m_Igain_4;
	m_param[4].i_gain= m_Igain_5;
	m_param[5].i_gain= m_Igain_6;
	m_param[6].i_gain= m_Igain_7;
	// D gain
	m_param[0].d_gain= m_Dgain_1;
	m_param[1].d_gain= m_Dgain_2;
	m_param[2].d_gain= m_Dgain_3;
	m_param[3].d_gain= m_Dgain_4;
	m_param[4].d_gain= m_Dgain_5;
	m_param[5].d_gain= m_Dgain_6;
	m_param[6].d_gain= m_Dgain_7;
	// VP gain
	m_param[0].vp_gain= m_VPgain_1;
	m_param[1].vp_gain= m_VPgain_2;
	m_param[2].vp_gain= m_VPgain_3;
	m_param[3].vp_gain= m_VPgain_4;
	m_param[4].vp_gain= m_VPgain_5;
	m_param[5].vp_gain= m_VPgain_6;
	m_param[6].vp_gain= m_VPgain_7;
	// VI gain
	m_param[0].vi_gain= m_VIgain_1;
	m_param[1].vi_gain= m_VIgain_2;
	m_param[2].vi_gain= m_VIgain_3;
	m_param[3].vi_gain= m_VIgain_4;
	m_param[4].vi_gain= m_VIgain_5;
	m_param[5].vi_gain= m_VIgain_6;
	m_param[6].vi_gain= m_VIgain_7;
	// VD gain
	m_param[0].vd_gain= m_VDgain_1;
	m_param[1].vd_gain= m_VDgain_2;
	m_param[2].vd_gain= m_VDgain_3;
	m_param[3].vd_gain= m_VDgain_4;
	m_param[4].vd_gain= m_VDgain_5;
	m_param[5].vd_gain= m_VDgain_6;
	m_param[6].vd_gain= m_VDgain_7;

  // arm paramter
	m_Arm[0].Direction= 1.0;
	m_Arm[1].Direction= 1.0;
	m_Arm[2].Direction= 1.0;
	m_Arm[3].Direction= 1.0;
	m_Arm[4].Direction= 1.0;
	m_Arm[5].Direction= 1.0;
	m_Arm[6].Direction= 1.0;
	for(int id=0; id<ARMNUM;id++){
		if(id==ARMNUM-1){ // 0.002m
			m_Arm[id].toAngle= Encoder[id] * 4.0 / 0.002 * GearRatio[id];
			m_Arm[id].toVeloc= 0.002 / ( Encoder[id] * 4.0 * 0.01 * GearRatio[id] );
		}else{ // 2π
			m_Arm[id].toAngle= Encoder[id] * 4.0 / (M_PI*2.0) * GearRatio[id];
			m_Arm[id].toVeloc= (M_PI*2.0) / ( Encoder[id] * 4.0 * 0.01 * GearRatio[id] );
		}
	}
  
	// Velocity Limit  [rad/sec] -> [enc/10ms]
	m_param[0].veloc_limit= (uint16_t)( m_VelocLimit_1 / m_Arm[0].toVeloc );
	m_param[1].veloc_limit= (uint16_t)( m_VelocLimit_2 / m_Arm[1].toVeloc );
	m_param[2].veloc_limit= (uint16_t)( m_VelocLimit_3 / m_Arm[2].toVeloc );
	m_param[3].veloc_limit= (uint16_t)( m_VelocLimit_4 / m_Arm[3].toVeloc );
	m_param[4].veloc_limit= (uint16_t)( m_VelocLimit_5 / m_Arm[4].toVeloc );
	m_param[5].veloc_limit= (uint16_t)( m_VelocLimit_6 / m_Arm[5].toVeloc );
	m_param[6].veloc_limit= (uint16_t)( m_VelocLimit_7 / m_Arm[6].toVeloc );
		
	// Acceleration Limit  [rad/sec^2] -> [enc/10ms^2]
	m_param[0].accel_limit= (uint16_t)( m_AccelLimit_1 / m_Arm[0].toVeloc );
	m_param[1].accel_limit= (uint16_t)( m_AccelLimit_2 / m_Arm[1].toVeloc );
	m_param[2].accel_limit= (uint16_t)( m_AccelLimit_3 / m_Arm[2].toVeloc );
	m_param[3].accel_limit= (uint16_t)( m_AccelLimit_4 / m_Arm[3].toVeloc );
	m_param[4].accel_limit= (uint16_t)( m_AccelLimit_5 / m_Arm[4].toVeloc );
	m_param[5].accel_limit= (uint16_t)( m_AccelLimit_6 / m_Arm[5].toVeloc );
	m_param[6].accel_limit= (uint16_t)( m_AccelLimit_7 / m_Arm[6].toVeloc );
	
	// -- set paramter -- //
  if( svm_set_param(serif, m_param) < 0 ){
  	RTC_ERROR((" Set Parameters Error"));
  	//std::cout << "[ERROR] Set Parameters Error" << std::endl;
    svm_close(&serif);
    isDev= false;
		return RTC::RTC_OK;
  }
  
	// -- data initialize -- //
	for(int id=0; id<ARMNUM;id++){
		// Velocity
		m_TargetVelocity[id]= 0.0;
		m_CurrentVelocity[id]= 0.0;
		// Angle
		m_OutArmAngle.data[id]= 0.0;
		m_OldAngle[id]= 0.0;
		// Motor data
		m_veloc[id]= 0;
		m_posit[id]= 0;
	}
	
	// -- Time initialize -- //
	m_oldTime= TM2TIME( GetTime() );
	#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 RH2ArmControl::onDeactivated(RTC::UniqueId ec_id)
{	
	RTC_INFO((" onDeactivated"));
	//std::cout << "[INFO] onDeactivated" << std::endl;
	// -- Driving stop -- //
	for(int id=0; id<ARMNUM;id++) m_veloc[id]= 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;
  return RTC::RTC_OK;
}

/*!
 * Instruction transmission to motor
 */

RTC::ReturnCode_t RH2ArmControl::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 ){
		// time
		m_OutArmAngle.tm= m_Time= GetTime();
		m_getTime= TM2TIME(m_Time);
		double diffTime= m_getTime - m_oldTime;
		m_oldTime= m_getTime;
		// data
		for(int id=0; id<ARMNUM;id++){
			// Angle data
			m_OutArmAngle.data[id]= (double)( m_posit[id] / m_Arm[id].toAngle * m_Arm[id].Direction );
			double diffAngle= m_OutArmAngle.data[id] - m_OldAngle[id];
			m_OldAngle[id]= m_OutArmAngle.data[id];
			// Velocity data
			m_CurrentVelocity[id]= diffAngle * ( 1.0 / diffTime ); // rad/sec
		}
		// write
		m_OutArmAngleOut.write();
		RTC_DEBUG((" get Angle data"));
		RTC_DEBUG((" sec:%ld nsec:%ld", m_Time.sec, m_Time.nsec));
		//#if DEBUG
		//std::cout << "[DEBUG] get Angle data" << std::endl;
		//std::cout << "[DEBUG] sec:" << m_Time.sec << " nsec:" << m_Time.nsec << std::endl;
		//#endif
	}else{
		isDev= false;
		return RTC::RTC_OK;
	}

	/*---------------------------------------------*/
	//  Get Data Port [Arm Velocity]
	/*---------------------------------------------*/
	if( isDev && m_InArmVelocityIn.isNew() ){
		m_InArmVelocityIn.read();
		// data
		for(int id=0; id<ARMNUM;id++)
			m_TargetVelocity[id]= m_InArmVelocity.data[id];
		RTC_DEBUG((" get velocity data"));
		RTC_DEBUG((" sec:%ld nsec:%ld", m_InArmVelocity.tm.sec, m_InArmVelocity.tm.nsec));
		//#if DEBUG
		//std::cout << "[DEBUG] get velocity data" << std::endl;
		//std::cout << "[DEBUG] sec:" << m_InArmVelocity.tm.sec << " nsec:" << m_InArmVelocity.tm.nsec << std::endl;
		//#endif
	}else{
		RTC_DEBUG((" not get velocity data"));
		//#if DEBUG
		//std::cout << "[DEBUG] not get velocity data" << std::endl;
		//#endif
	}

	/***********************************************/
	//  Set Velocity Data
	/***********************************************/
	if( isDev ){
		// veloc data
		for(int id=0; id<ARMNUM;id++)
			m_veloc[id]= (int16_t)( m_TargetVelocity[id] / m_Arm[id].toVeloc * m_Arm[id].Direction );
		// set of veloc data
		if( isDev && svm_set_veloc(serif, m_veloc)==0 ){
			RTC_DEBUG((" set_veloc()"));
			//#if DEBUG
			//std::cout << "[DEBUG] set_veloc()" << std::endl;
			//#endif
		}else{
			isDev= false;
			return RTC::RTC_OK;
		}
	}

	/***********************************************/
	//  Plot
	/***********************************************/
	for(int id=0; id<ARMNUM;id++){
		double diffV= m_CurrentVelocity[id] / m_TargetVelocity[id];
		RTC_INFO(("veloc[%6d] posit[%6d] angle[%2.4f]  Velocity: Target[%2.6f] Current[%2.6f] Rate[%3.3f]"
		 ,m_veloc[id], m_posit[id], m_OutArmAngle.data[id], m_TargetVelocity[id], m_CurrentVelocity[id], diffV ));
		//printf("veloc[%6d] posit[%6d] angle[%2.4f]  Velocity: Target[%2.6f] Current[%2.6f] Rate[%3.3f]\n"
		// ,m_veloc[id], m_posit[id], m_OutArmAngle.data[id], m_TargetVelocity[id], m_CurrentVelocity[id], diffV );
	}
	
	#if DEBUG
	VelocityLOG << m_getTime << " " << 
	m_TargetVelocity[0] << " " << 
	m_TargetVelocity[1] << " " << 
	m_TargetVelocity[2] << " " << 
	m_TargetVelocity[3] << " " << 
	m_TargetVelocity[4] << " " << 
	m_TargetVelocity[5] << " " << 
	m_TargetVelocity[6] << " " << 
	m_CurrentVelocity[0] << " " << 
	m_CurrentVelocity[1] << " " << 
	m_CurrentVelocity[2] << " " << 
	m_CurrentVelocity[3] << " " << 
	m_CurrentVelocity[4] << " " << 
	m_CurrentVelocity[5] << " " << 
	m_CurrentVelocity[6] << std::endl;
	#endif
	
	#if DEBUG_TIME
	Time_Execute= TM2TIME(GetTime());
	RTC_DEBUG((" ExecuteTime: %f", Time_Execute));
	RTC_DEBUG((" ExecuteTime(Cycle): %f", Time_Execute-Time_Execute_old));
	Time_Execute_old= Time_Execute;
	//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
	
  return RTC::RTC_OK;
}

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

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

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

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

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


/**********************************/
//  OpenDev
/**********************************/
bool RH2ArmControl::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 -- //
	RTC_INFO((" Device Open"));
	//std::cout << "[INFO] Device Open" << std::endl;
	if( access(Device.c_str(), 0) ){
		RTC_ERROR((" Device File not found [%s]", Device.c_str()));
		//std::cout << "[ERROR] Device File not found [" << Device << "]" << 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(), ARMNUM, Baudrate ) != 0 ){
		RTC_ERROR((" Device Open Error"));
		//std::cout << "[ERROR] Device Open Error" << std::endl;
		return false;
	}
	
	// -- required to exit by Ctrl-C -- //
	serif_old= serif; 
	
	return true;
}

/**********************************/
//  Calibration
/**********************************/
bool RH2ArmControl::Calibration( void )
{
	RTC_INFO((" Calibration"));
	//std::cout << "[INFO] Calibration" << std::endl;
  // -- check motor -- //
  for(int id=0; id<ARMNUM; id++){
    if( serif->enabled[id] == SVM_ENABLED ) continue;
    if( svm_enable_module( serif, id, SVM_ENABLED ) < 0 ){
		  RTC_ERROR((" Set Configuration Error"));
		  //std::cout << "[ERROR] Set Configuration Error" << std::endl;
		  svm_close(&serif);
		  return false;
    }
  }
  
  // -- set configuration -- //
  RTC_INFO((" Set Configuration"));
  //std::cout << "[INFO] Set Configuration" << std::endl;
	svm_config_t Calib_config[ARMNUM]= {
		{ 0,-533333,533333,   1,  1, 0},  // DOF1/No.0
		{ 0,-246272,492544,   1,  1, 0},  // DOF2/No.1
		{ 0,      0,510000,  -1, -1, 0},  // DOF3/No.2
		{ 0,-327554,327554,   1,  1, 0},  // DOF4/No.3
		{ 0,-154938,185996,   1,  1, 0},  // DOF5/No.4
		{ 0, -94004, 94004,   1,  1, 0},  // DOF6/No.5
		{ 0,   1000,444013,   1,  1, 0}   // DOF7/No.6 ベクトルセンサなしバージョン
	//{ 0,  20000,342425,  -1, -1, 0}   // DOF7/No.6 ベクトルセンサ付きバージョン
	};
	for(int id=0; id<ARMNUM; id++)
		m_config[id]= Calib_config[id];
	// set of current offset
	for(int id=0; id<ARMNUM; id++){
		svm_config_t conf;
		svm_read_config(serif, id, &conf);
		m_config[id].calib_offset= conf.calib_offset;
  }
	// set configuration
  if( svm_set_config(serif, m_config) < 0 ){
    RTC_ERROR((" Set Configuration Error"));
    //std::cout << "[ERROR] Set Configuration Error" << std::endl;
    svm_close(&serif);
    return false;
  }
  
	// -- set parameters -- //
	RTC_INFO((" Set Parameters"));
	//std::cout << "[INFO] Set Parameters" << std::endl;
	svm_param_t Calib_param[ARMNUM]= {
		{ 8,0,1,100,0, 1,0xffff,   4000},  // DOF1/No.0 
		{10,0,1, 50,0, 1,0xffff,   4000},  // DOF2/No.1 
		{14,0,1,100,0, 1,0xffff,   4000},  // DOF3/No.2
		{14,0,1,150,0, 1,0xffff,   4000},  // DOF4/No.3
		{10,0,0, 50,0, 0,0xffff,   4000},  // DOF5/No.4 
		{30,0,1,150,0, 1,0xffff,   4000},  // DOF6/No.5 
		{30,0,1,250,0,10,0xffff,   4000}   // DOF7/No.6
	};
  if( svm_set_param(serif, Calib_param) < 0 ){
  	RTC_ERROR((" Set Parameters Error"));
  	//std::cout << "[ERROR] Set Parameters Error" << std::endl;
    svm_close(&serif);
		return false;
  }
  
  // -- lock all modules -- //
  for(int id=0; id<ARMNUM; id++)
		svm_lock_module(serif, id);
	
	// -- Calibration parameters -- //
  int CYCLE_TIME= 20;
  int nowEnc[ARMNUM]={0,0,0,0,0,0,0};

	// -- 1st Calibration [No.4] -- //
	RTC_INFO(("Calibration 1st"));
	//std::cout << "Calibration 1st" << std::endl;
	svm_unlock_module(serif, 4, SVM_VELOC); // DOF5/No.4
	svm_unlock_module(serif, 6, SVM_VELOC); // DOF7/No.6
	calib(20*1000/CYCLE_TIME); // 20s timeout
	svm_get_posit(serif,nowEnc);
	nowEnc[4]=0;
	nowEnc[6]=0;
	rh_posEnc(nowEnc);
	svm_lock_module(serif, 4); // lock DOF5/No.4
	svm_lock_module(serif, 6); // lock DOF7/No.6


	
  // -- 2nd Calibration [No5,3,1]-- //
	RTC_INFO((" 2nd Calibration"));
	//std::cout << "[INFO] 2nd Calibration" << std::endl;
	svm_unlock_module(serif, 5, SVM_VELOC); // DOF6/No.5
	svm_unlock_module(serif, 3, SVM_VELOC); // DOF4/No.3
	svm_unlock_module(serif, 0, SVM_VELOC); // DOF1/No.0
	calib(20*1000/CYCLE_TIME); // 20s timeout
	svm_get_posit(serif,nowEnc);
	nowEnc[5]=0;
	nowEnc[3]=0;
	nowEnc[0]=0;
	rh_posEnc(nowEnc);
	svm_lock_module(serif, 5); // lock DOF6/No.5
	svm_lock_module(serif, 3); // lock DOF4/No.3
	svm_lock_module(serif, 0); // lock DOF1/No.0

	// -- 3rd Calibration [No.1,2] -- //
	RTC_INFO((" 3rd Calibration"));
	//std::cout << "[INFO] 3rd Calibration" << std::endl;
	svm_unlock_module(serif, 1, SVM_VELOC); // DOF2/No.1
	svm_unlock_module(serif, 2, SVM_VELOC); // DOF3/No.2
	calib(30*1000/CYCLE_TIME);
	svm_get_posit(serif,nowEnc);
	nowEnc[1]=0;
	nowEnc[2]=0;
	rh_posEnc(nowEnc);
	svm_lock_module(serif, 1); // lock DOF2/No.1
	svm_lock_module(serif, 2); // lock DOF3/No.2
	
  // -- unlock all modules -- //
  for(int id=0; id<ARMNUM; id++)
  	svm_unlock_module(serif, id, SVM_VELOC);
  
  // -- set of mode -- //
  RTC_INFO((" Set mode"));
  //std::cout << "[INFO] Set mode" << std::endl;
  if( svm_set_mode(serif, SVM_VELOC) < 0 ){
    RTC_ERROR((" Set mode Error"));
    //std::cout << "[ERROR] Set mode Error" << std::endl;
    svm_close(&serif);
		return false;
  }
  
	return true;
}

/**********************************/
//  calib
/**********************************/
void RH2ArmControl::calib(int timeout_count)
{
	// -- control parameters -- //
  int drive[ARMNUM];
  int32_t enc[ARMNUM], enc_pre[ARMNUM];
  int dof, num;
  int configflag=0;
  int done=0;
	int calib_drive[ARMNUM]= {-3000, -600, 2000,-4000, -1820,-2000,14000};
	int calib_threshold[ARMNUM]= {5,10,5,0,5,0,5};
	int calib_margin[ARMNUM]= {500,2000,2000,400,500,100,100}; // must be positive
	
  // --- count enabled motors -- //
  num=0;
	for(dof=0;dof<ARMNUM;dof++){
		if (serif->enabled[dof]==SVM_ENABLED) num++;
		enc[dof]=0;
		enc_pre[dof]=0;
	}
	if (num==0) return;
	
	// -- test drive mode -- //
	svm_set_mode( serif, SVM_TESTDRV );
	
	// -- reset enc_pre[] -- //
	svm_get_posit(serif, enc_pre);
	
	// -- start up motors -- //
	for(dof=0;dof<ARMNUM;dof++)
		drive[dof] = (serif->enabled[dof]==SVM_ENABLED ? calib_drive[dof] : 0);
	svm_set_testdrv(serif, drive);
	
	// -- wait for start-up motors -- //
	usleep(100000);
	
	// ----- loop ----- //
	while(1) {
		svm_get_posit(serif, enc);
		configflag=0;
		for(dof=0; dof<ARMNUM; dof++){
			// skip not enabled module
			if (serif->enabled[dof]!=SVM_ENABLED) continue ; 
			// diff
			int diff = enc[dof] - enc_pre[dof];
			//printf("%d: %3d/%10d/%d\n", dof, diff, enc[dof], drive[dof]);
			if(drive[dof]==0) continue ;
			if(diff >= -calib_threshold[dof] && diff <= calib_threshold[dof]){
				int32_t offset;
				if(drive[dof]<0) offset = m_config[dof].lower_limit - calib_margin[dof] - enc[dof];
				else offset = m_config[dof].upper_limit + calib_margin[dof] - enc[dof];
				offset += m_config[dof].calib_offset;
				m_config[dof].calib_offset = offset;
				configflag=1;
				drive[dof]=0;
				done++;
			}
		}
		for(dof=0; dof<ARMNUM; dof++)
			enc_pre[dof]=enc[dof];
		if(configflag){
			svm_set_config(serif, m_config);
			// need wait ?
			svm_set_testdrv(serif, drive);
		}
		if(done==num){
			RTC_INFO(("done"));
			//printf("done\n");
			break;
		}

	} // end of while
	
	// -- reverse motors -- //
	if(done==num){
		for(dof=0; dof<ARMNUM; dof++){
			if(serif->enabled[dof]==0) drive[dof]= 0;
			else drive[dof]=-calib_drive[dof];
		}
		svm_set_testdrv(serif, drive);
		usleep(200000);
	}
	
	// -- stop the motor -- //
	for(dof=0;dof<ARMNUM;dof++) drive[dof]= 0;
	svm_set_testdrv(serif, drive);
	
	return;
}

/**********************************/
//  rh_posEnc
/**********************************/
int RH2ArmControl::rh_posEnc(int *goalEnc)
/*{
  // -- set mode -- //
  svm_set_mode(serif, SVM_POSIT);
  
  // -- get start position -- //
  int startEnc[ARMNUM];
  svm_get_posit(serif, startEnc);
  
  // -- initialize -- //
  int32_t ref[ARMNUM];
  int32_t getEnc[ARMNUM];
  int dist[ARMNUM];
  int sign[ARMNUM];
 // bool isDrive[ARMNUM];
  for(int i=0; i<ARMNUM; i++){
    dist[i]= abs( goalEnc[i] - startEnc[i] );
   // if( dist[i] != 0 ) isDrive[i]= true;
    //else isDrive[i]= false;
    ref[i]= 0;
    sign[i]= goalEnc[i] > startEnc[i] ? 1 : -1;
  }

  // -- loop -- //
  int count= 0;
  int margin= 100;
  
  double enc_dt[ARMNUM]= {800.0,600.0,650.0,700.0,500.0,500.0,500.0};
  while(1){
  
  	int DriveCount= 0;
  	svm_get_posit(serif, getEnc);
  	
    for(int i=0; i<ARMNUM; i++){
    	int encDiff= abs( getEnc[i] - goalEnc[i] );
    	
    	//if( encDiff < margin ) isDrive[i]= false;
    	//if( !isDrive[i] ){
    	if( encDiff < margin ){
    		ref[i]= goalEnc[i];
    		DriveCount++;
    		continue;
    	}
    	
      double dt= double( dist[i] / enc_dt[i] );
      if( dt < 1.0 ) dt= 1.0;
      int ref_p= sign[i] * (int)( dist[i] * sin( M_PI / 2.0 * (double)count / dt ) );
      ref[i]= startEnc[i] + ref_p;

    }
    svm_set_posit(serif, ref);
    if( DriveCount == ARMNUM ) break;
    count++;
    
  } // end of while

  return 0;
}
*/
// とりあえず動くレベル
{
  // -- set mode -- //
  svm_set_mode(serif, SVM_POSIT);
  
  // -- get start position -- //
  int startEnc[ARMNUM];
  svm_get_posit(serif, startEnc);
  
  // -- initialize -- //
  int32_t ref[ARMNUM];
  int dist[ARMNUM];
  int sign[ARMNUM];
  for(int i=0; i<ARMNUM; i++){
    dist[i]= abs( goalEnc[i] - startEnc[i] );
    ref[i]= 0;
    sign[i]= goalEnc[i] > startEnc[i] ? 1 : -1;
  }

  // -- loop -- //
  int count= 0;
  int done= 0x7f;
  double enc_dt[ARMNUM]= {800.0,600.0,650.0,700.0,500.0,500.0,500.0};
  while(1){
    for(int i=0; i<ARMNUM; i++){
      if( (done&(0x01<<i)) == 0 ) continue;
      double dt= double( dist[i] / enc_dt[i] );
      if( dt < 1.0 ) dt= 1.0;
      int ref_p= sign[i] * (int)( dist[i] * sin( M_PI / 2.0 * (double)count / dt ) );
      ref[i]= startEnc[i] + ref_p;
      //printf("[%d] goalPosi[%d] dt=%f diff=%d ref=%d\n",i,dist[i],dt,ref_p,ref[i]);
      //printf("[%d] sin(%.1f)=%.1f, dref=%d, dist=%d, ref=%d \n",
      //   i, M_PI/2*count/dt, sin(M_PI/2*count/dt),(int)(dist[i] * sin(M_PI/2*count/dt)), dist[i], ref[i]);
      if( abs( int(dist[i]/enc_dt[i]) ) - count < 10 ) done &= (~(0x1<<i));
    }
    usleep(20000); // 移動待ち　20ms
    svm_set_posit(serif, ref);
    if(done==0) break;
    count++;
  }

  return 0;
}



extern "C"
{
 
  void RH2ArmControlInit(RTC::Manager* manager)
  {
    RTC::Properties profile(rh2armcontrol_spec);
    manager->registerFactory(profile,
                             RTC::Create<RH2ArmControl>,
                             RTC::Delete<RH2ArmControl>);
  }
  
};


