JoystickController.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  * General Robotix Inc.
10  */
19 #include "JoystickController.h"
20 
21 // Module specification
22 // <rtc-template block="module_spec">
23 static const char* joystickcontroller_spec[] =
24  {
25  "implementation_id", "JoystickController",
26  "type_name", "JoystickController",
27  "description", "Controller to drive a mobile robot",
28  "version", "1.0.0",
29  "vendor", "AIST HRG",
30  "category", "OpenHRP Controller",
31  "activity_type", "DataFlowComponent",
32  "max_instance", "1",
33  "language", "C++",
34  "lang_type", "compile",
35  ""
36  };
37 // </rtc-template>
38 
40  : RTC::DataFlowComponentBase(manager),
41  // <rtc-template block="initializer">
42  m_angleIn("angle", m_angle),
43  m_velocityIn("velocity", m_velocity),
44  m_commandIn("command", m_command),
45  m_torqueOut("torque", m_torque),
46 
47  // </rtc-template>
48  dummy(0)
49 {
50  // Registration: InPort/OutPort/Service
51  // <rtc-template block="registration">
52 
53  // Set service provider to Ports
54 
55  // Set service consumers to Ports
56 
57  // Set CORBA Service Ports
58 
59  // </rtc-template>
60 
61 }
62 
64 {
65 }
66 
67 
69 {
70  // Set InPort buffers
71  addInPort("angle", m_angleIn);
72  addInPort("velocity", m_velocityIn);
73  addInPort("command", m_commandIn);
74 
75  // Set OutPort buffer
76  addOutPort("torque", m_torqueOut);
77 
78  // ポート初期化 //
79  m_command.data.length(2);
80  m_command.data[0] = m_command.data[1] = 0.0;
81  m_angle.data.length(1);
82  m_angle.data[0] = 0.0;
83  m_velocity.data.length(2);
84  m_velocity.data[0] = m_velocity.data[1] = 0.0;
85  m_torque.data.length(4);
86  return RTC::RTC_OK;
87 }
88 
89 /*
90 RTC::ReturnCode_t JoystickController::onFinalize()
91 {
92  return RTC::RTC_OK;
93 }
94 */
95 
96 /*
97 RTC::ReturnCode_t JoystickController::onStartup(RTC::UniqueId ec_id)
98 {
99  return RTC::RTC_OK;
100 }
101 */
102 
103 /*
104 RTC::ReturnCode_t JoystickController::onShutdown(RTC::UniqueId ec_id)
105 {
106  return RTC::RTC_OK;
107 }
108 */
109 
110 /*
111 RTC::ReturnCode_t JoystickController::onActivated(RTC::UniqueId ec_id)
112 {
113  return RTC::RTC_OK;
114 }
115 */
116 
117 /*
118 RTC::ReturnCode_t JoystickController::onDeactivated(RTC::UniqueId ec_id)
119 {
120  return RTC::RTC_OK;
121 }
122 */
123 
125 {
126  // ロボットからのデータ入力 //
127  m_angleIn.read();
128  m_velocityIn.read();
129  double steerAngle = m_angle.data[0];
130  double steerVel = m_velocity.data[0];
131  double tireVel = m_velocity.data[1];
132 
133  // ジョイスティック(ユーザ)からのデータ入力 //
134  m_commandIn.read();
135  double steerCommandAngle = 3.14159 * -0.5 * m_command.data[0] / 180.0;
136  double tireCommandVel = m_command.data[1] / 10;
137 
138  // ステアリングトルク計算 //
139  double steerCommandTorque = 20.0 * (steerCommandAngle - steerAngle) - 2.0 * steerVel;
140 
141  // 駆動トルク計算 //
142  double tireCommandTorque = 1.0 * (tireCommandVel - tireVel);
143 
144  // ロボットへのトルク出力 //
145  m_torque.data[0] = steerCommandTorque;
146  m_torque.data[1] = tireCommandTorque;
147  m_torque.data[2] = tireCommandTorque;
148  m_torque.data[3] = tireCommandTorque;
149  m_torqueOut.write();
150 
151  return RTC::RTC_OK;
152 }
153 
154 /*
155 RTC::ReturnCode_t JoystickController::onAborting(RTC::UniqueId ec_id)
156 {
157  return RTC::RTC_OK;
158 }
159 */
160 
161 /*
162 RTC::ReturnCode_t JoystickController::onError(RTC::UniqueId ec_id)
163 {
164  return RTC::RTC_OK;
165 }
166 */
167 
168 /*
169 RTC::ReturnCode_t JoystickController::onReset(RTC::UniqueId ec_id)
170 {
171  return RTC::RTC_OK;
172 }
173 */
174 
175 /*
176 RTC::ReturnCode_t JoystickController::onStateUpdate(RTC::UniqueId ec_id)
177 {
178  return RTC::RTC_OK;
179 }
180 */
181 
182 /*
183 RTC::ReturnCode_t JoystickController::onRateChanged(RTC::UniqueId ec_id)
184 {
185  return RTC::RTC_OK;
186 }
187 */
188 
189 
190 
191 extern "C"
192 {
193 
195  {
197  manager->registerFactory(profile,
198  RTC::Create<JoystickController>,
199  RTC::Delete<JoystickController>);
200  }
201 
202 };
203 
204 
png_infop png_charpp int png_charpp profile
Definition: png.h:2382
JoystickController(RTC::Manager *manager)
virtual RTC::ReturnCode_t onInitialize()
TimedDoubleSeq m_angle
TimedDoubleSeq m_torque
TimedDoubleSeq m_velocity
bool addOutPort(const char *name, OutPortBase &outport)
static const char * joystickcontroller_spec[]
Controller to drive a mobile robot.
InPort< TimedFloatSeq > m_commandIn
ExecutionContextHandle_t UniqueId
InPort< TimedDoubleSeq > m_angleIn
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
void JoystickControllerInit(RTC::Manager *manager)
OutPort< TimedDoubleSeq > m_torqueOut
virtual bool write(DataType &value)
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
InPort< TimedDoubleSeq > m_velocityIn


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:23