Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00019 #include "SamplePD.h"
00020
00021 #include <iostream>
00022
00023 #define TIMESTEP 0.002
00024
00025 #define ANGLE_FILE "etc/angle.dat"
00026 #define VEL_FILE "etc/vel.dat"
00027 #define ACC_FILE "etc/acc.dat"
00028
00029 #define GAIN_FILE "etc/PDgain.dat"
00030
00031 namespace {
00032 const bool CONTROLLER_BRIDGE_DEBUG = false;
00033 }
00034
00035
00036
00037
00038 static const char* samplepd_spec[] =
00039 {
00040 "implementation_id", "SamplePD",
00041 "type_name", "SamplePD",
00042 "description", "Sample PD component",
00043 "version", "0.1",
00044 "vendor", "AIST",
00045 "category", "Generic",
00046 "activity_type", "DataFlowComponent",
00047 "max_instance", "10",
00048 "language", "C++",
00049 "lang_type", "compile",
00050
00051
00052 ""
00053 };
00054
00055
00056 SamplePD::SamplePD(RTC::Manager* manager)
00057 : RTC::DataFlowComponentBase(manager),
00058
00059 m_angleIn("angle", m_angle),
00060 m_torqueOut("torque", m_torque),
00061
00062
00063 dummy(0),
00064 qold(DOF)
00065 {
00066 if( CONTROLLER_BRIDGE_DEBUG )
00067 {
00068 std::cout << "SamplePD::SamplePD" << std::endl;
00069 }
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 }
00082
00083 SamplePD::~SamplePD()
00084 {
00085 closeFiles();
00086 delete [] Pgain;
00087 delete [] Dgain;
00088 }
00089
00090
00091 RTC::ReturnCode_t SamplePD::onInitialize()
00092 {
00093
00094
00095 if( CONTROLLER_BRIDGE_DEBUG )
00096 {
00097 std::cout << "onInitialize" << std::endl;
00098 }
00099
00100
00101 addInPort("angle", m_angleIn);
00102
00103
00104 addOutPort("torque", m_torqueOut);
00105
00106
00107 Pgain = new double[DOF];
00108 Dgain = new double[DOF];
00109
00110 gain.open(GAIN_FILE);
00111 if (gain.is_open()){
00112 for (int i=0; i<DOF; i++){
00113 gain >> Pgain[i];
00114 gain >> Dgain[i];
00115 }
00116 gain.close();
00117 }else{
00118 std::cerr << GAIN_FILE << " not found" << std::endl;
00119 }
00120 m_torque.data.length(DOF);
00121 m_angle.data.length(DOF);
00122
00123 return RTC::RTC_OK;
00124 }
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149 RTC::ReturnCode_t SamplePD::onActivated(RTC::UniqueId ec_id)
00150 {
00151 std::cout << "on Activated" << std::endl;
00152 openFiles();
00153
00154 if(m_angleIn.isNew()){
00155 m_angleIn.read();
00156 }
00157
00158 for(int i=0; i < DOF; ++i){
00159 qold[i] = m_angle.data[i];
00160 q_ref[i] = dq_ref[i] = 0.0;
00161 }
00162
00163 return RTC::RTC_OK;
00164 }
00165
00166
00167 RTC::ReturnCode_t SamplePD::onDeactivated(RTC::UniqueId ec_id)
00168 {
00169 std::cout << "on Deactivated" << std::endl;
00170 closeFiles();
00171 return RTC::RTC_OK;
00172 }
00173
00174
00175
00176 RTC::ReturnCode_t SamplePD::onExecute(RTC::UniqueId ec_id)
00177 {
00178 if( CONTROLLER_BRIDGE_DEBUG )
00179 {
00180 std::cout << "onExecute" << std::endl;
00181 std::string localStr;
00182 std::cin >> localStr;
00183 }
00184
00185 if(m_angleIn.isNew()){
00186 m_angleIn.read();
00187 }
00188
00189 if(!angle.eof()){
00190 angle >> q_ref[0]; vel >> dq_ref[0];
00191 for (int i=0; i<DOF; i++){
00192 angle >> q_ref[i];
00193 vel >> dq_ref[i];
00194 }
00195 }
00196 for(int i=0; i<DOF; i++){
00197 double q = m_angle.data[i];
00198 double dq = (q - qold[i]) / TIMESTEP;
00199 qold[i] = q;
00200
00201 m_torque.data[i] = -(q - q_ref[i]) * Pgain[i] - (dq - dq_ref[i]) * Dgain[i];
00202 }
00203
00204 m_torqueOut.write();
00205
00206 return RTC::RTC_OK;
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 SamplePD::openFiles()
00245 {
00246 angle.open(ANGLE_FILE);
00247 if(!angle.is_open()){
00248 std::cerr << ANGLE_FILE << " not opened" << std::endl;
00249 }
00250
00251 vel.open(VEL_FILE);
00252 if (!vel.is_open()){
00253 std::cerr << VEL_FILE << " not opened" << std::endl;
00254 }
00255 }
00256
00257 void SamplePD::closeFiles()
00258 {
00259 if( angle.is_open() ){
00260 angle.close();
00261 angle.clear();
00262 }
00263 if( vel.is_open() ){
00264 vel.close();
00265 vel.clear();
00266 }
00267 }
00268
00269 extern "C"
00270 {
00271
00272 DLL_EXPORT void SamplePDInit(RTC::Manager* manager)
00273 {
00274 coil::Properties profile(samplepd_spec);
00275 manager->registerFactory(profile,
00276 RTC::Create<SamplePD>,
00277 RTC::Delete<SamplePD>);
00278 }
00279
00280 };
00281