SampleSV.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * National Institute of Advanced Industrial Science and Technology (AIST)
00009  * General Robotix Inc. 
00010  */
00019 #include "SampleSV.h"
00020 
00021 #include <iostream>
00022 
00023 #define DOF (4)
00024 #define STEERING_ID 0
00025 #define WHEEL_ID 1
00026 
00027 #define STEERING_FILE "etc/steer.dat"
00028 
00029 #define STEERING_P_GAIN 100.0
00030 #define STEERING_D_GAIN 1.0
00031 #define WHEEL_P_GAIN 100.0
00032 #define WHEEL_D_GAIN 0.5
00033 #define WHEEL_REF_VEL 6.28  // [rad/s]
00034 
00035 #define TIMESTEP 0.002
00036 
00037 namespace {
00038   const bool CONTROLLER_BRIDGE_DEBUG = false;
00039 }
00040 
00041 
00042 // Module specification
00043 // <rtc-template block="module_spec">
00044 static const char* samplepd_spec[] =
00045   {
00046     "implementation_id", "SampleSV",
00047     "type_name",         "SampleSV",
00048     "description",       "Sample SV component",
00049     "version",           "0.1",
00050     "vendor",            "AIST",
00051     "category",          "Generic",
00052     "activity_type",     "DataFlowComponent",
00053     "max_instance",      "10",
00054     "language",          "C++",
00055     "lang_type",         "compile",
00056     // Configuration variables
00057 
00058     ""
00059   };
00060 // </rtc-template>
00061 
00062 SampleSV::SampleSV(RTC::Manager* manager)
00063   : RTC::DataFlowComponentBase(manager),
00064     // <rtc-template block="initializer">
00065     m_steerIn("steer", m_steer),
00066     m_velIn("vel", m_vel),
00067 #if 1
00068         m_rangeIn("range", m_range),
00069 #endif
00070     m_torqueOut("torque", m_torque),
00071     
00072     // </rtc-template>
00073     wheel_ref(0.0)
00074 {
00075   if( CONTROLLER_BRIDGE_DEBUG )
00076   {
00077     std::cout << "SampleSV::SampleSV" << std::endl;
00078   }
00079   // Registration: InPort/OutPort/Service
00080   // <rtc-template block="registration">
00081   // Set service provider to Ports
00082   
00083   // Set service consumers to Ports
00084   
00085   // Set CORBA Service Ports
00086   
00087   // </rtc-template>
00088 }
00089 
00090 SampleSV::~SampleSV()
00091 {
00092         closeFiles();
00093 }
00094 
00095 
00096 RTC::ReturnCode_t SampleSV::onInitialize()
00097 {
00098   // <rtc-template block="bind_config">
00099   // Bind variables and configuration variable
00100   if( CONTROLLER_BRIDGE_DEBUG )
00101   {
00102     std::cout << "onInitialize" << std::endl;
00103   }
00104   
00105   // Set InPort buffers
00106   addInPort("steer", m_steerIn);
00107   addInPort("vel", m_velIn);
00108 #if 1
00109   addInPort("range", m_rangeIn);
00110 #endif
00111 
00112   // Set OutPort buffer
00113   addOutPort("torque", m_torqueOut);
00114   // </rtc-template>
00115 
00116   m_torque.data.length(DOF);
00117   m_steer.data.length(2);
00118   m_vel.data.length(2);
00119   return RTC::RTC_OK;
00120 }
00121 
00122 
00123 
00124 /*
00125 RTC::ReturnCode_t SampleSV::onFinalize()
00126 {
00127   return RTC::RTC_OK;
00128 }
00129 */
00130 
00131 /*
00132 RTC::ReturnCode_t SampleSV::onStartup(RTC::UniqueId ec_id)
00133 {
00134   return RTC::RTC_OK;
00135 }
00136 */
00137 
00138 /*
00139 RTC::ReturnCode_t SampleSV::onShutdown(RTC::UniqueId ec_id)
00140 {
00141   return RTC::RTC_OK;
00142 }
00143 */
00144 
00145 RTC::ReturnCode_t SampleSV::onActivated(RTC::UniqueId ec_id)
00146 {
00147 
00148   std::cout << "on Activated" << std::endl;
00149   openFiles();
00150   if(m_steerIn.isNew()){
00151     m_steerIn.read();
00152   }
00153   if(m_velIn.isNew()){
00154     m_velIn.read();
00155   }
00156   wheel_ref = 0.0;
00157 
00158   return RTC::RTC_OK;
00159 }
00160 
00161 
00162 RTC::ReturnCode_t SampleSV::onDeactivated(RTC::UniqueId ec_id)
00163 {
00164   std::cout << "on Deactivated" << std::endl;
00165   closeFiles();
00166   return RTC::RTC_OK;
00167 }
00168 
00169 
00170 
00171 RTC::ReturnCode_t SampleSV::onExecute(RTC::UniqueId ec_id)
00172 {
00173   if( CONTROLLER_BRIDGE_DEBUG )
00174   {
00175     std::cout << "SampleSV::onExecute" << std::endl;
00176   }
00177 
00178   if(m_steerIn.isNew()){
00179     m_steerIn.read();
00180   }
00181   if( m_velIn.isNew() ){
00182     m_velIn.read();
00183   }
00184 #if 1
00185   if (m_rangeIn.isNew()){
00186           m_rangeIn.read();
00187           std::cout << "received range data(" << m_range.data.length() << ")" << std::endl;
00188   }
00189 #endif
00190 
00191   //double q_ref, dq_ref;
00192   double dummy;
00193   steer >> dummy; // skip time
00194 
00195   double steer_ref;
00196   steer >> steer_ref;
00197 
00198   for(int i=0; i<DOF; i++) m_torque.data[i] = 0.0;
00199 
00200   m_torque.data[STEERING_ID] = (steer_ref - m_steer.data[STEERING_ID]) * STEERING_P_GAIN - m_vel.data[STEERING_ID] * STEERING_D_GAIN;
00201   m_torque.data[WHEEL_ID] = (wheel_ref - m_steer.data[WHEEL_ID]) * WHEEL_P_GAIN + (WHEEL_REF_VEL - m_vel.data[WHEEL_ID]) * WHEEL_D_GAIN;
00202   wheel_ref += WHEEL_REF_VEL * TIMESTEP;
00203   m_torqueOut.write();
00204 
00205   return RTC::RTC_OK;
00206 }
00207 
00208 
00209 /*
00210   RTC::ReturnCode_t SampleSV::onAborting(RTC::UniqueId ec_id)
00211   {
00212   return RTC::RTC_OK;
00213   }
00214 */
00215 
00216 /*
00217   RTC::ReturnCode_t SampleSV::onError(RTC::UniqueId ec_id)
00218   {
00219   return RTC::RTC_OK;
00220   }
00221 */
00222 
00223 /*
00224   RTC::ReturnCode_t SampleSV::onReset(RTC::UniqueId ec_id)
00225   {
00226   return RTC::RTC_OK;
00227   }
00228 */
00229 
00230 /*
00231   RTC::ReturnCode_t SampleSV::onStateUpdate(RTC::UniqueId ec_id)
00232   {
00233   return RTC::RTC_OK;
00234   }
00235 */
00236 
00237 /*
00238   RTC::ReturnCode_t SampleSV::onRateChanged(RTC::UniqueId ec_id)
00239   {
00240   return RTC::RTC_OK;
00241   }
00242 */
00243 
00244 void SampleSV::openFiles()
00245 {
00246   steer.open( STEERING_FILE );
00247   if (!steer.is_open())
00248     std::cerr << STEERING_FILE <<" not opened" << std::endl;
00249 }
00250 
00251 void SampleSV::closeFiles()
00252 {
00253   if( steer.is_open() )
00254   {
00255     steer.close();
00256     steer.clear();
00257   }
00258 }
00259 
00260 
00261 extern "C"
00262 {
00263 
00264   DLL_EXPORT void SampleSVInit(RTC::Manager* manager)
00265   {
00266     coil::Properties profile(samplepd_spec);
00267     manager->registerFactory(profile,
00268                              RTC::Create<SampleSV>,
00269                              RTC::Delete<SampleSV>);
00270   }
00271 
00272 };
00273 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56