JoystickController.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
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 "JoystickController.h"
00020 
00021 // Module specification
00022 // <rtc-template block="module_spec">
00023 static const char* joystickcontroller_spec[] =
00024   {
00025     "implementation_id", "JoystickController",
00026     "type_name",         "JoystickController",
00027     "description",       "Controller to drive a mobile robot",
00028     "version",           "1.0.0",
00029     "vendor",            "AIST HRG",
00030     "category",          "OpenHRP Controller",
00031     "activity_type",     "DataFlowComponent",
00032     "max_instance",      "1",
00033     "language",          "C++",
00034     "lang_type",         "compile",
00035     ""
00036   };
00037 // </rtc-template>
00038 
00039 JoystickController::JoystickController(RTC::Manager* manager)
00040   : RTC::DataFlowComponentBase(manager),
00041     // <rtc-template block="initializer">
00042     m_angleIn("angle", m_angle),
00043     m_velocityIn("velocity", m_velocity),
00044     m_commandIn("command", m_command),
00045     m_torqueOut("torque", m_torque),
00046     
00047     // </rtc-template>
00048         dummy(0)
00049 {
00050   // Registration: InPort/OutPort/Service
00051   // <rtc-template block="registration">
00052   
00053   // Set service provider to Ports
00054   
00055   // Set service consumers to Ports
00056   
00057   // Set CORBA Service Ports
00058   
00059   // </rtc-template>
00060 
00061 }
00062 
00063 JoystickController::~JoystickController()
00064 {
00065 }
00066 
00067 
00068 RTC::ReturnCode_t JoystickController::onInitialize()
00069 {
00070   // Set InPort buffers
00071   addInPort("angle", m_angleIn);
00072   addInPort("velocity", m_velocityIn);
00073   addInPort("command", m_commandIn);
00074   
00075   // Set OutPort buffer
00076   addOutPort("torque", m_torqueOut);
00077 
00078   // ポート初期化 //
00079   m_command.data.length(2);
00080   m_command.data[0] = m_command.data[1] = 0.0;
00081   m_angle.data.length(1);
00082   m_angle.data[0] = 0.0;
00083   m_velocity.data.length(2);
00084   m_velocity.data[0] = m_velocity.data[1] = 0.0;
00085   m_torque.data.length(4);
00086   return RTC::RTC_OK;
00087 }
00088 
00089 /*
00090 RTC::ReturnCode_t JoystickController::onFinalize()
00091 {
00092   return RTC::RTC_OK;
00093 }
00094 */
00095 
00096 /*
00097 RTC::ReturnCode_t JoystickController::onStartup(RTC::UniqueId ec_id)
00098 {
00099   return RTC::RTC_OK;
00100 }
00101 */
00102 
00103 /*
00104 RTC::ReturnCode_t JoystickController::onShutdown(RTC::UniqueId ec_id)
00105 {
00106   return RTC::RTC_OK;
00107 }
00108 */
00109 
00110 /*
00111 RTC::ReturnCode_t JoystickController::onActivated(RTC::UniqueId ec_id)
00112 {
00113   return RTC::RTC_OK;
00114 }
00115 */
00116 
00117 /*
00118 RTC::ReturnCode_t JoystickController::onDeactivated(RTC::UniqueId ec_id)
00119 {
00120   return RTC::RTC_OK;
00121 }
00122 */
00123 
00124 RTC::ReturnCode_t JoystickController::onExecute(RTC::UniqueId ec_id)
00125 {
00126   // ロボットからのデータ入力 //
00127   m_angleIn.read();
00128   m_velocityIn.read();
00129   double steerAngle = m_angle.data[0];
00130   double steerVel = m_velocity.data[0];
00131   double tireVel = m_velocity.data[1];
00132 
00133   // ジョイスティック(ユーザ)からのデータ入力 //
00134   m_commandIn.read();
00135   double steerCommandAngle = 3.14159 * -0.5 * m_command.data[0] / 180.0;
00136   double tireCommandVel = m_command.data[1] / 10;
00137 
00138   // ステアリングトルク計算 //
00139   double steerCommandTorque = 20.0 * (steerCommandAngle - steerAngle) - 2.0 * steerVel;
00140 
00141   // 駆動トルク計算 //
00142   double tireCommandTorque = 1.0 * (tireCommandVel - tireVel);
00143 
00144   // ロボットへのトルク出力 //
00145   m_torque.data[0] = steerCommandTorque;
00146   m_torque.data[1] = tireCommandTorque;
00147   m_torque.data[2] = tireCommandTorque; 
00148   m_torque.data[3] = tireCommandTorque; 
00149   m_torqueOut.write(); 
00150 
00151   return RTC::RTC_OK;
00152 }
00153 
00154 /*
00155 RTC::ReturnCode_t JoystickController::onAborting(RTC::UniqueId ec_id)
00156 {
00157   return RTC::RTC_OK;
00158 }
00159 */
00160 
00161 /*
00162 RTC::ReturnCode_t JoystickController::onError(RTC::UniqueId ec_id)
00163 {
00164   return RTC::RTC_OK;
00165 }
00166 */
00167 
00168 /*
00169 RTC::ReturnCode_t JoystickController::onReset(RTC::UniqueId ec_id)
00170 {
00171   return RTC::RTC_OK;
00172 }
00173 */
00174 
00175 /*
00176 RTC::ReturnCode_t JoystickController::onStateUpdate(RTC::UniqueId ec_id)
00177 {
00178   return RTC::RTC_OK;
00179 }
00180 */
00181 
00182 /*
00183 RTC::ReturnCode_t JoystickController::onRateChanged(RTC::UniqueId ec_id)
00184 {
00185   return RTC::RTC_OK;
00186 }
00187 */
00188 
00189 
00190 
00191 extern "C"
00192 {
00193  
00194   void JoystickControllerInit(RTC::Manager* manager)
00195   {
00196     RTC::Properties profile(joystickcontroller_spec);
00197     manager->registerFactory(profile,
00198                              RTC::Create<JoystickController>,
00199                              RTC::Delete<JoystickController>);
00200   }
00201   
00202 };
00203 
00204 


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:55