PA10Controller.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 "PA10Controller.h"
00020 
00021 #include <iostream>
00022 
00023 #define TIMESTEP 0.001
00024 
00025 #define ANGLE_FILE "etc/angle.dat"
00026 #define VEL_FILE   "etc/vel.dat"
00027 #define GAIN_FILE  "etc/PDgain.dat"
00028 
00029 // Module specification
00030 // <rtc-template block="module_spec">
00031 static const char* PA10Controller_spec[] =
00032   {
00033     "implementation_id", "PA10Controller",
00034     "type_name",         "PA10Controller",
00035     "description",       "PA10Controller component",
00036     "version",           "0.1",
00037     "vendor",            "AIST",
00038     "category",          "Generic",
00039     "activity_type",     "DataFlowComponent",
00040     "max_instance",      "10",
00041     "language",          "C++",
00042     "lang_type",         "compile",
00043     // Configuration variables
00044 
00045     ""
00046   };
00047 // </rtc-template>
00048 
00049 PA10Controller::PA10Controller(RTC::Manager* manager)
00050   : RTC::DataFlowComponentBase(manager),
00051     // <rtc-template block="initializer">
00052     m_angleIn("angle", m_angle),
00053     m_torqueOut("torque", m_torque),
00054     
00055     // </rtc-template>
00056     dummy(0),
00057     qold(DOF)
00058 {
00059   // Registration: InPort/OutPort/Service
00060   // <rtc-template block="registration">
00061   // Set service provider to Ports
00062   
00063   // Set service consumers to Ports
00064   
00065   // Set CORBA Service Ports
00066   
00067   // </rtc-template>
00068 }
00069 
00070 PA10Controller::~PA10Controller()
00071 {
00072   closeFiles();
00073   delete [] Pgain;
00074   delete [] Dgain;
00075 }
00076 
00077 
00078 RTC::ReturnCode_t PA10Controller::onInitialize()
00079 {
00080   // <rtc-template block="bind_config">
00081   // Bind variables and configuration variable
00082 
00083   // Set InPort buffers
00084   addInPort("angle", m_angleIn);
00085   
00086   // Set OutPort buffer
00087   addOutPort("torque", m_torqueOut);
00088   
00089   // </rtc-template>
00090 
00091   Pgain = new double[DOF];
00092   Dgain = new double[DOF];
00093 
00094   gain.open(GAIN_FILE);
00095   if (gain.is_open()){
00096     for (int i=0; i<DOF; i++){
00097       gain >> Pgain[i];
00098       gain >> Dgain[i];
00099     }
00100     gain.close();
00101   }else{
00102     std::cerr << GAIN_FILE << " not opened" << std::endl;
00103   }
00104   m_torque.data.length(DOF);
00105   m_angle.data.length(DOF);
00106 
00107   return RTC::RTC_OK;
00108 }
00109 
00110 
00111 
00112 /*
00113 RTC::ReturnCode_t PA10Controller::onFinalize()
00114 {
00115   return RTC::RTC_OK;
00116 }
00117 */
00118 
00119 /*
00120 RTC::ReturnCode_t PA10Controller::onStartup(RTC::UniqueId ec_id)
00121 {
00122   return RTC::RTC_OK;
00123 }
00124 */
00125 
00126 /*
00127 RTC::ReturnCode_t PA10Controller::onShutdown(RTC::UniqueId ec_id)
00128 {
00129   return RTC::RTC_OK;
00130 }
00131 */
00132 
00133 RTC::ReturnCode_t PA10Controller::onActivated(RTC::UniqueId ec_id)
00134 {
00135   std::cout << "on Activated" << std::endl;
00136   openFiles();
00137 
00138   if(m_angleIn.isNew()){
00139     m_angleIn.read();
00140   }
00141 
00142   for(int i=0; i < DOF; ++i){
00143     qold[i] = m_angle.data[i];
00144     q_ref[i] = dq_ref[i] = 0.0;
00145   }
00146 
00147   return RTC::RTC_OK;
00148 }
00149 
00150 RTC::ReturnCode_t PA10Controller::onDeactivated(RTC::UniqueId ec_id)
00151 {
00152   std::cout << "on Deactivated" << std::endl;
00153   closeFiles();
00154   return RTC::RTC_OK;
00155 }
00156 
00157 
00158 RTC::ReturnCode_t PA10Controller::onExecute(RTC::UniqueId ec_id)
00159 {
00160   if(m_angleIn.isNew()){
00161     m_angleIn.read();
00162   }
00163 
00164   if(!angle.eof()){
00165     angle >> q_ref[0]; vel >> dq_ref[0];// skip time
00166     for (int i=0; i<DOF; i++){
00167       angle >> q_ref[i];
00168       vel >> dq_ref[i];
00169     }
00170   }
00171 
00172   for(int i=0; i<DOF; i++){
00173     double q = m_angle.data[i];
00174     double dq = (q - qold[i]) / TIMESTEP;
00175     qold[i] = q;
00176     
00177     m_torque.data[i] = -(q - q_ref[i]) * Pgain[i] - (dq - dq_ref[i]) * Dgain[i];
00178   }
00179   
00180   m_torqueOut.write();
00181   
00182   return RTC::RTC_OK;
00183 }
00184 
00185 
00186 /*
00187   RTC::ReturnCode_t PA10Controller::onAborting(RTC::UniqueId ec_id)
00188   {
00189   return RTC::RTC_OK;
00190   }
00191 */
00192 
00193 /*
00194   RTC::ReturnCode_t PA10Controller::onError(RTC::UniqueId ec_id)
00195   {
00196   return RTC::RTC_OK;
00197   }
00198 */
00199 
00200 /*
00201   RTC::ReturnCode_t PA10Controller::onReset(RTC::UniqueId ec_id)
00202   {
00203   return RTC::RTC_OK;
00204   }
00205 */
00206 
00207 /*
00208   RTC::ReturnCode_t PA10Controller::onStateUpdate(RTC::UniqueId ec_id)
00209   {
00210   return RTC::RTC_OK;
00211   }
00212 */
00213 
00214 /*
00215   RTC::ReturnCode_t PA10Controller::onRateChanged(RTC::UniqueId ec_id)
00216   {
00217   return RTC::RTC_OK;
00218   }
00219 */
00220 
00221 void PA10Controller::openFiles()
00222 {
00223   angle.open(ANGLE_FILE);
00224   if(!angle.is_open()){
00225     std::cerr << ANGLE_FILE << " not opened" << std::endl;
00226   }
00227 
00228   vel.open(VEL_FILE);
00229   if (!vel.is_open()){
00230     std::cerr << VEL_FILE << " not opened" << std::endl;
00231   }
00232 }
00233 
00234 void PA10Controller::closeFiles()
00235 {
00236   if(angle.is_open()){
00237     angle.close();
00238     angle.clear();
00239   }
00240 
00241   if(vel.is_open()){
00242     vel.close();
00243     angle.clear();
00244   }
00245 }
00246 
00247 extern "C"
00248 {
00249 
00250   DLL_EXPORT void PA10ControllerInit(RTC::Manager* manager)
00251   {
00252     coil::Properties profile(PA10Controller_spec);
00253     manager->registerFactory(profile,
00254                              RTC::Create<PA10Controller>,
00255                              RTC::Delete<PA10Controller>);
00256   }
00257 
00258 };
00259 
00260 


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