Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
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
00043
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
00057
00058 ""
00059 };
00060
00061
00062 SampleSV::SampleSV(RTC::Manager* manager)
00063 : RTC::DataFlowComponentBase(manager),
00064
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
00073 wheel_ref(0.0)
00074 {
00075 if( CONTROLLER_BRIDGE_DEBUG )
00076 {
00077 std::cout << "SampleSV::SampleSV" << std::endl;
00078 }
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088 }
00089
00090 SampleSV::~SampleSV()
00091 {
00092 closeFiles();
00093 }
00094
00095
00096 RTC::ReturnCode_t SampleSV::onInitialize()
00097 {
00098
00099
00100 if( CONTROLLER_BRIDGE_DEBUG )
00101 {
00102 std::cout << "onInitialize" << std::endl;
00103 }
00104
00105
00106 addInPort("steer", m_steerIn);
00107 addInPort("vel", m_velIn);
00108 #if 1
00109 addInPort("range", m_rangeIn);
00110 #endif
00111
00112
00113 addOutPort("torque", m_torqueOut);
00114
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
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
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
00192 double dummy;
00193 steer >> dummy;
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
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
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