Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
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
00030
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
00044
00045 ""
00046 };
00047
00048
00049 PA10Controller::PA10Controller(RTC::Manager* manager)
00050 : RTC::DataFlowComponentBase(manager),
00051
00052 m_angleIn("angle", m_angle),
00053 m_torqueOut("torque", m_torque),
00054
00055
00056 dummy(0),
00057 qold(DOF)
00058 {
00059
00060
00061
00062
00063
00064
00065
00066
00067
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
00081
00082
00083
00084 addInPort("angle", m_angleIn);
00085
00086
00087 addOutPort("torque", m_torqueOut);
00088
00089
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
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
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];
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
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
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