RS003: MotorControl.cpp Source File

Go to the documentation of this file.
00001 // -*- C++ -*-
00017 #include "MotorControl.h"
00018 
00019 //--- add---------------------------------------
00020 #include <stdio.h>
00021 #include <iostream>
00022 //----------------------------------------------
00023 
00024 // Module specification
00025 // <rtc-template block="module_spec">
00026 static const char* motorcontrol_spec[] =
00027   {
00028     "implementation_id", "MotorControl",
00029     "type_name",         "MotorControl",
00030     "description",       "MotorControl",
00031     "version",           "4.0.0",
00032     "vendor",            "AIST INVENT",
00033     "category",          "Controller",
00034     "activity_type",     "PERIODIC",
00035     "kind",              "DataFlowComponent",
00036     "max_instance",      "10",
00037     "language",          "C++",
00038     "lang_type",         "compile",
00039     "exec_cxt.periodic.rate", "1000.0",
00040     // Configuration variables
00041     "conf.default.PGainL", "5.0",
00042     "conf.default.DGainL", "2.0",
00043     "conf.default.PGainR", "5.0",
00044     "conf.default.DGainR", "2.0",
00045     "conf.default.SimulatedOffsetX", "0.0",
00046     "conf.default.SimulatedOffsetY", "0.0",
00047     "conf.default.SimulatedOffsetAngle", "0.0",
00048     "conf.default.leftWheelID", "0",
00049     "conf.default.rightWheelID", "1",
00050     "conf.default.radiusOfLeftWheel", "0.1",
00051     "conf.default.radiusOfRightWheel", "0.1",
00052     "conf.default.lengthOfAxle", "0.441",
00053     "conf.default.radiusOfBodyArea", "0.45",
00054     ""
00055   };
00056 // </rtc-template>
00057 
00062 MotorControl::MotorControl(RTC::Manager* manager)
00063     // <rtc-template block="initializer">
00064   : RTC::DataFlowComponentBase(manager),
00065     m_TargetVelocityIn("TargetVelocity", m_TargetVelocity),
00066     m_InCurrentWheelAngleIn("InCurrentWheelAngle", m_InCurrentWheelAngle),
00067     m_InSimulatedPositionIn("InSimulatedPosition", m_InSimulatedPosition),
00068     m_OutSimulatedPositionToInventGUIOut("OutSimulatedPositionToInventGUI", m_OutSimulatedPositionToInventGUI),
00069     m_TorqueOut("Torque", m_Torque),
00070     m_OutCurrentWheelAngleOut("OutCurrentWheelAngle", m_OutCurrentWheelAngle),
00071     m_OutSimulatedPositionToLocalizationOut("OutSimulatedPositionToLocalization", m_OutSimulatedPositionToLocalization),
00072     m_InventGUIProvPort("InventGUIProv"),
00073     m_BumpProvPort("BumpProv")
00074 
00075     // </rtc-template>
00076 {
00077 }
00078 
00082 MotorControl::~MotorControl()
00083 {
00084         RTC_INFO(("on Destructer"));
00085         //std::cout << "on Destructer" << std::endl;
00086 }
00087 
00088 
00092 RTC::ReturnCode_t MotorControl::onInitialize()
00093 {
00094   // Registration: InPort/OutPort/Service
00095   // <rtc-template block="registration">
00096   // Set InPort buffers
00097   registerInPort("TargetVelocity", m_TargetVelocityIn);
00098   registerInPort("InCurrentWheelAngle", m_InCurrentWheelAngleIn);
00099   registerInPort("InSimulatedPosition", m_InSimulatedPositionIn);
00100   
00101   // Set OutPort buffer
00102   registerOutPort("OutSimulatedPositionToInventGUI", m_OutSimulatedPositionToInventGUIOut);
00103   registerOutPort("Torque", m_TorqueOut);
00104   registerOutPort("OutCurrentWheelAngle", m_OutCurrentWheelAngleOut);
00105   registerOutPort("OutSimulatedPositionToLocalization", m_OutSimulatedPositionToLocalizationOut);
00106   
00107   // Set service provider to Ports
00108   m_InventGUIProvPort.registerProvider("InventGUIMotor", "Motor", m_InventGUIMotor);
00109   m_BumpProvPort.registerProvider("BumpMotor", "Motor", m_BumpMotor);
00110   
00111   // Set service consumers to Ports
00112   
00113   // Set CORBA Service Ports
00114   registerPort(m_InventGUIProvPort);
00115   registerPort(m_BumpProvPort);
00116   
00117   // </rtc-template>
00118 
00119   // <rtc-template block="bind_config">
00120   // Bind variables and configuration variable
00121   bindParameter("PGainL", m_PGainL, "5.0");
00122   bindParameter("DGainL", m_DGainL, "2.0");
00123   bindParameter("PGainR", m_PGainR, "5.0");
00124   bindParameter("DGainR", m_DGainR, "2.0");
00125   bindParameter("SimulatedOffsetX", m_SimulatedOffsetX, "0.0");
00126   bindParameter("SimulatedOffsetY", m_SimulatedOffsetY, "0.0");
00127   bindParameter("SimulatedOffsetAngle", m_SimulatedOffsetAngle, "0.0");
00128   bindParameter("leftWheelID", m_leftWheelID, "0");
00129   bindParameter("rightWheelID", m_rightWheelID, "1");
00130   bindParameter("radiusOfLeftWheel", m_radiusOfLeftWheel, "0.1");
00131   bindParameter("radiusOfRightWheel", m_radiusOfRightWheel, "0.1");
00132   bindParameter("lengthOfAxle", m_lengthOfAxle, "0.441");
00133   bindParameter("radiusOfBodyArea", m_radiusOfBodyArea, "0.45");
00134   
00135   // </rtc-template>
00136 
00137 //---[add]---------------------------------
00138   RTC_INFO(("on Constructer"));
00139   //std::cout << "on Constructer" << std::endl;
00140 
00141   // set Dimension(PORT length) 
00142   m_Torque.data.length(Dof);  // left and right wheel (no casters)     
00143   m_OutCurrentWheelAngle.data.length(Dof);
00144 //  m_OutSimulatedPositionToInventGUI.data.length(12);  //for output SimulatedPosition&angle data to InventGUI
00145 //  m_OutSimulatedPositionToLocalization.data.length(12);  //for output SimulatedPosition&angle data to Localization  
00146   m_OutSimulatedPositionToInventGUI.data.length(4);  //for output SimulatedPosition&angle data to InventGUI
00147   m_OutSimulatedPositionToLocalization.data.length(4);  //for output SimulatedPosition&angle data to Localization  
00148  
00149 #if CALLBACK
00150         //-- callback object
00151 //      m_InCurrentWheelAngleIn.setOnWriteConvert(&m_debug);
00152 #endif  
00153 
00154 //---[add]---------------------------------
00155 
00156   return RTC::RTC_OK;
00157 }
00158 
00159 /*
00160 RTC::ReturnCode_t MotorControl::onFinalize()
00161 {
00162   return RTC::RTC_OK;
00163 }
00164 */
00165 
00166 /*
00167 RTC::ReturnCode_t MotorControl::onStartup(RTC::UniqueId ec_id)
00168 {
00169   return RTC::RTC_OK;
00170 }
00171 */
00172 
00173 /*
00174 RTC::ReturnCode_t MotorControl::onShutdown(RTC::UniqueId ec_id)
00175 {
00176   return RTC::RTC_OK;
00177 }
00178 */
00179 
00187 RTC::ReturnCode_t MotorControl::onActivated(RTC::UniqueId ec_id)
00188 {
00189 //---[add]---------------------------------
00190         RTC_INFO(("on Activated"));
00191         //std::cout << "on Activated" << std::endl;
00192 
00193         // Update INPORT 
00194         m_InCurrentWheelAngleIn.update();
00195         m_TargetVelocityIn.update();
00196         m_InSimulatedPositionIn.update();
00197 
00198 //--get Model Parameter from Configulation
00199         Body.leftID = m_leftWheelID; // jointID of left wheel
00200         Body.rightID = m_rightWheelID; // jointID of right wheel
00201         Body.wheelRadiusLeft = m_radiusOfLeftWheel; // Wheel Radius(Left)
00202         Body.wheelRadiusRight = m_radiusOfRightWheel; // Wheel Radius(Right)
00203         Body.bodyRadius = 0.5 * m_lengthOfAxle; // Body Radius (half of length of Wheel Axle)
00204         Body.length = m_radiusOfBodyArea; // length from center of Body to corner of Body
00205 
00206         RTC_DEBUG(("Body.leftID: %d   Body.rightID: %d", Body.leftID, Body.rightID));   
00207         RTC_DEBUG(("Body.wheelRadius[L]: %f Body.wheelRadius[R]: %f   Body.bodyRadius: %f",Body.wheelRadiusLeft, Body.wheelRadiusRight, Body.bodyRadius));      
00208 #if DEBUG       
00209         //std::cout << "Body.leftID: " << Body.leftID << "   Body.rightID: " << Body.rightID << std::endl;      
00210         //std::cout << "Body.wheelRadius[L]: " << Body.wheelRadiusLeft << "Body.wheelRadius[R]: " << Body.wheelRadiusRight << "   Body.bodyRadius: " << Body.bodyRadius << std::endl;   
00211 
00212         ofs.open("SimulatePosition.dat");
00213 #endif
00214 
00215 
00216   // set previous wheel angular velocity's data 
00217         for(int i=0; i < Dof; i++)      qOld[i] = 0.0;  // initialize to current wheel's angle value
00218 
00219   // initialize SWITCH
00220         compSwitch = -1;
00221         InitializeFlag = false;
00222 
00223         //-- initialize Time data
00224         sec = nsec = 0; 
00225         TimeStampOld = TimeStamp = 0.0;
00226 
00227 //---[add]---------------------------------
00228 
00229   return RTC::RTC_OK;
00230 }
00231 
00236 RTC::ReturnCode_t MotorControl::onDeactivated(RTC::UniqueId ec_id)
00237 {
00238 //---[add]---------------------------------
00239         // close file
00240 #if DEBUG       
00241         ofs.close();
00242 #endif
00243 //---[add]---------------------------------
00244   return RTC::RTC_OK;
00245 }
00246 
00256 RTC::ReturnCode_t MotorControl::onExecute(RTC::UniqueId ec_id)
00257 {
00258 //---[add]---------------------------------
00259 //  std::cout << " ==================================== [onExecute] ====================================" << std::endl;
00260 
00261 /*********************************************************************************************************/
00262 //  [START][FINISH][CLEAR] 
00263 /*********************************************************************************************************/
00264 
00265         //--- set startFlag if [start()] was called
00266         if(m_InventGUIMotor.m_StartFlag){  
00267                 m_InventGUIMotor.m_StartFlag = false;
00268                 compSwitch = 1;
00269                 RTC_INFO(("====== [start() was called from InventGUIComp] ======================"));
00270                 //std::cout << "====== [start() was called from InventGUIComp] ======================" << std::endl;
00271         //--- set finishFlag if [finish()] was called
00272         }else if(m_InventGUIMotor.m_FinishFlag){  
00273                 m_InventGUIMotor.m_FinishFlag = false;
00274                 compSwitch = 2;
00275                 RTC_INFO(("====== [finish() was called from InventGUIComp] ======================"));
00276                 //std::cout << "====== [finish() was called from InventGUIComp] ======================" << std::endl;
00277         //--- set clearFlag if [clear()] was called
00278         }else if(m_InventGUIMotor.m_ClearFlag){  
00279                 m_InventGUIMotor.m_ClearFlag = false;
00280                 compSwitch = 3;
00281                 //TODO: add CLEAR_ACTION 
00282                 RTC_INFO(("====== [clear() was called from InventGUIComp] ======================"));
00283                 //std::cout << "====== [clear() was called from InventGUIComp] ======================" << std::endl;
00284         //--- set bumpDetectFlag if [Stop()] was called
00285         }else if(m_BumpMotor.m_StopFlag){  
00286                 m_BumpMotor.m_StopFlag = false;
00287                 compSwitch = 4;
00288                 //TODO: add Bump Detect_ACTION 
00289                 RTC_INFO(("====== [Stop() was called from BumpDetectionComp] ======================"));
00290         }
00291 
00292         
00293 
00294 /*********************************************************************************************************/
00295 //  if [New GainDatas] are given -> update ConfigValue
00296 /*********************************************************************************************************/
00297         if(m_InventGUIMotor.m_GainFlag){  // if [New GainDatas] are given
00298                 m_PGainL = m_InventGUIMotor.m_PGainL;
00299                 m_PGainR = m_InventGUIMotor.m_PGainR;
00300                 m_DGainL = m_InventGUIMotor.m_DGainL;
00301                 m_DGainR = m_InventGUIMotor.m_DGainR;
00302                 m_InventGUIMotor.m_GainFlag = false;
00303                 RTC_INFO(("== [GAIN] change GAIN value"));
00304                 //std::cout << "== [GAIN] change GAIN value" << std::endl;
00305         }
00306 
00307 
00308 /*********************************************************************************************************/
00309 //  main action
00310 /*********************************************************************************************************/
00311 
00312         if (m_InCurrentWheelAngleIn.isNew()) {
00313 
00314                 //-- get Previous Wheel Angle Data 
00315                 m_InCurrentWheelAngleIn.read();
00316 
00317                 double dummy_sec =  m_InCurrentWheelAngle.tm.sec + 1.0e-9*m_InCurrentWheelAngle.tm.nsec;
00318                 RTC_DEBUG(("--[TIME]: %f nsec = %ld", dummy_sec, m_InCurrentWheelAngle.tm.nsec));
00319 #if DEBUG       
00320                 //double dummy_sec =  m_InCurrentWheelAngle.tm.sec + 1.0e-9*m_InCurrentWheelAngle.tm.nsec;
00321                 //std::cout << "--[TIME]: " << dummy_sec << " nsec =  "  << m_InCurrentWheelAngle.tm.nsec << "-------------------------------------------" << std::endl;
00322 #endif
00323          
00324                 //-- set Previous Wheel Angle Data
00325                 double q[Dof];
00326                 q[Body.leftID] = m_InCurrentWheelAngle.data[Body.leftID];
00327                 q[Body.rightID] = m_InCurrentWheelAngle.data[Body.rightID];
00328 
00329                 //-- get time stamp data 
00330                 sec = m_InCurrentWheelAngle.tm.sec;
00331                 nsec = m_InCurrentWheelAngle.tm.nsec;
00332                 TimeStamp = sec + 1.0e-9*nsec;
00333 
00334                 //-- initialize 
00335                 if (!InitializeFlag) {
00336                         InitializeFlag = true;
00337                 } else {
00338                         //-- calc timestep value
00339                         timeStep = TimeStamp - TimeStampOld;  
00340 
00341                         //-- calc Wheel Angular Velocity's Datas by previous angle data
00342                         double dq[Dof];
00343                         dq[Body.leftID] = (q[Body.leftID] - qOld[Body.leftID]) / timeStep;
00344                         dq[Body.rightID] = (q[Body.rightID] - qOld[Body.rightID]) / timeStep;
00345 
00346                         //-- calc TORQUE  & set data to OUTPORT
00347                         double q_ref[Dof], dq_ref[Dof];
00348 
00349                         //-- action if Target Velocity datas are given 
00350                         if (m_TargetVelocityIn.isNew()) {
00351 
00352                                 //*** read Target Velocity data
00353                                 m_TargetVelocityIn.read();
00354 
00355                                 double TransVel,RotVel,Rho;
00356                                 TransVel = m_TargetVelocity.vx;   // [m/s]
00357                                 RotVel = m_TargetVelocity.w;    //  [rad/s]
00358 
00359                                 if (RotVel == 0.0) { 
00360                                         //-- calc [Target] Angular Velocity Dat
00361                                         dq_ref[Body.leftID] = TransVel / Body.wheelRadiusLeft;
00362                                         dq_ref[Body.rightID] = TransVel / Body.wheelRadiusRight;
00363                                 } else {
00364                                         Rho = TransVel / RotVel;        // curvature radius
00365                                         //-- calc [Target] Angular Velocity Dat
00366                                         dq_ref[Body.leftID] = (Rho - Body.bodyRadius) * RotVel / Body.wheelRadiusLeft;
00367                                         dq_ref[Body.rightID] = (Rho + Body.bodyRadius) * RotVel / Body.wheelRadiusRight;
00368                                 }
00369 
00370                                 //-- calc [Target] Angle Data
00371                                 q_ref[Body.leftID] = q[Body.leftID] + 0.5 * (dq_ref[Body.leftID] + dq[Body.leftID]) * timeStep;
00372                                 q_ref[Body.rightID] = q[Body.rightID] + 0.5 * (dq_ref[Body.rightID] + dq[Body.rightID]) * timeStep;
00373 
00374                                 //-- calc TORQUE value    
00375                                 m_Torque.data[Body.leftID] = (q_ref[Body.leftID] - q[Body.leftID]) * m_PGainL + (dq_ref[Body.leftID] - dq[Body.leftID]) * m_DGainL;
00376                                 m_Torque.data[Body.rightID] = (q_ref[Body.rightID] - q[Body.rightID]) * m_PGainR + (dq_ref[Body.rightID] - dq[Body.rightID]) * m_DGainR;
00377 
00378                                 RTC_DEBUG((" Pgain(L): %f Pgain(R): %f Dgain(L): %f Dgain(R): %f", m_PGainL, m_PGainR, m_DGainL, m_DGainR));;
00379                                 RTC_DEBUG((" TransVel: %f RotVel: %f", TransVel, RotVel));
00380                                 RTC_DEBUG((" q_ref(L): %f q(L): %f [q_ref-q](L): %f dq_ref(L): %f dq(L): %f [dq_ref-dq](L): %f", q_ref[Body.leftID], q[Body.leftID], q_ref[Body.leftID] - q[Body.leftID], dq_ref[Body.leftID], dq[Body.leftID], dq_ref[Body.leftID] - dq[Body.leftID]));        
00381                                 RTC_DEBUG((" q_ref(R): %f q(R): %f [q_ref-q](R): %f dq_ref(R): %f dq(R): %f [dq_ref-dq](R): %f", q_ref[Body.rightID], q[Body.rightID], q_ref[Body.rightID] - q[Body.rightID], dq_ref[Body.rightID], dq[Body.rightID], dq_ref[Body.rightID] - dq[Body.rightID]));;       
00382                                 RTC_DEBUG((" Ptorque(L): %f Dtorque(L): %f", (q_ref[Body.leftID] - q[Body.leftID]) * m_PGainL, (dq_ref[Body.leftID] - dq[Body.leftID]) * m_DGainL));
00383                                 RTC_DEBUG((" Ptorque(R): %f Dtorque(R): %f", (q_ref[Body.rightID] - q[Body.rightID]) * m_PGainR, (dq_ref[Body.rightID] - dq[Body.rightID]) * m_DGainR));
00384 #if DEBUG
00385                                 //std::cout << " Pgain(L): " << m_PGainL << " Pgain(R): " << m_PGainR << " Dgain(L): " << m_DGainL << " Dgain(R): " << m_DGainR << std::endl;
00386                                 //std::cout << " TransVel: " << TransVel << " RotVel: " << RotVel  << std::endl;
00387                                 //std::cout << " q_ref(L): " << q_ref[Body.leftID] << " q(L): " << q[Body.leftID] << " [q_ref-q](L): " << q_ref[Body.leftID] - q[Body.leftID] << " dq_ref(L): " << dq_ref[Body.leftID] << " dq(L): " << dq[Body.leftID] << " [dq_ref-dq](L): " << dq_ref[Body.leftID] - dq[Body.leftID] << std::endl;   
00388                                 //std::cout << " q_ref(R): " << q_ref[Body.rightID] << " q(R): " << q[Body.rightID] << " [q_ref-q](R): " << q_ref[Body.rightID] - q[Body.rightID] << " dq_ref(R): " << dq_ref[Body.rightID] << " dq(R): " << dq[Body.rightID] << " [dq_ref-dq](R): " << dq_ref[Body.rightID] - dq[Body.rightID] << std::endl;   
00389                                 //std::cout << " Ptorque(L): " << (q_ref[Body.leftID] - q[Body.leftID]) * m_PGainL << " Dtorque(L): " << (dq_ref[Body.leftID] - dq[Body.leftID]) * m_DGainL << std::endl;
00390                                 //std::cout << " Ptorque(R): " << (q_ref[Body.rightID] - q[Body.rightID]) * m_PGainR << " Dtorque(R): " << (dq_ref[Body.rightID] - dq[Body.rightID]) * m_DGainR << std::endl;
00391 #endif
00392 
00393                         } else {  
00394 #if DEBUG       
00395 //                              std::cout << "   ***[Skip][NoTarget] can't get new TARGET Velocity data from [DriveControl]***" << std::endl;   
00396 #endif
00397                                 m_Torque.data[Body.leftID] = 0.0;
00398                                 m_Torque.data[Body.rightID] = 0.0;      
00399                         }
00400 
00401         RTC_DEBUG(("[MotorControl Torque] Time = %f[s]  Torque(L,R) = (%f, %f)", TimeStamp, m_Torque.data[Body.leftID], m_Torque.data[Body.rightID]));
00402 #if LOG
00403         //std::cout <<  "[MotorControl Torque] Time = " << TimeStamp << "[s]  Torque(L,R) =  (" << m_Torque.data[Body.leftID]  << " , " << m_Torque.data[Body.rightID] <<  ") "  << std::endl;
00404 #endif
00405 
00406                         // set TIME
00407                         m_Torque.tm.sec = sec;
00408                         m_Torque.tm.nsec = nsec;
00409                         //*** set Torque data to OutPort
00410                         m_TorqueOut.write();                      
00411 
00412                 } // enod of if (initialize)
00413 
00414                 //set Current Wheel Angle Data for output to DriveControl and Odometry
00415                 m_OutCurrentWheelAngle.data[Body.leftID] = m_InCurrentWheelAngle.data[Body.leftID];
00416                 m_OutCurrentWheelAngle.data[Body.rightID] = m_InCurrentWheelAngle.data[Body.rightID];
00417                 // set TIME
00418                 m_OutCurrentWheelAngle.tm.sec = sec;
00419                 m_OutCurrentWheelAngle.tm.nsec = nsec;
00420                 //*** set Current Wheel angle's data to OUTPORT
00421                 m_OutCurrentWheelAngleOut.write();
00422 
00423 
00424         RTC_DEBUG(("  wheel[L] = %f ,  wheel[R] = %f TIME1= %ld TIME2= %ld", m_OutCurrentWheelAngle.data[Body.leftID], m_OutCurrentWheelAngle.data[Body.rightID], m_OutCurrentWheelAngle.tm.sec, m_OutCurrentWheelAngle.tm.nsec));
00425 #if DEBUG
00426         //std::cout << "  wheel[L] = " << m_OutCurrentWheelAngle.data[Body.leftID]  << " ,  wheel[R] = " << m_OutCurrentWheelAngle.data[Body.rightID]   << " TIME1= " << m_OutCurrentWheelAngle.tm.sec << " TIME2=" << m_OutCurrentWheelAngle.tm.nsec << std::endl;
00427 #endif
00428 
00429         //*** set Current data -> Previous data
00430                 qOld[Body.leftID] = q[Body.leftID];
00431                 qOld[Body.rightID] = q[Body.rightID];
00432                 TimeStampOld = TimeStamp;
00433 
00434         //*** get Simulated Position data and send them to InventGUIRTC
00435                 if (m_InSimulatedPositionIn.isNew()) {
00436                         m_InSimulatedPositionIn.read();
00437 
00438                         //calc collect simulated position values
00439                         double simu_x,simu_y,simu_theta,simu_fai;
00440                         double length = sqrt(m_SimulatedOffsetX * m_SimulatedOffsetX  +  m_SimulatedOffsetY * m_SimulatedOffsetY);
00441 //                      simu_theta = atan2(m_InSimulatedPosition.data[3], m_InSimulatedPosition.data[4]);  // (0,0) -> (0,1) : theta = 90[deg]
00442                         simu_theta = -1.0*atan2(m_InSimulatedPosition.data[4], m_InSimulatedPosition.data[3]);  // (0,0) -> (0,1) : theta = 90[deg]
00443                         simu_fai = atan2(m_SimulatedOffsetY, m_SimulatedOffsetX);    // CenterOfBody(Xb,Yb) -> CenterOfAxle(Xa,Ya) : offsetX = Xa-Xb, offsetY = Ya-Yb
00444 //                      simu_x = m_InSimulatedPosition.data[0] + length*cos(simu_theta + simu_fai - 0.5*M_PI);
00445 //                      simu_y = m_InSimulatedPosition.data[1] + length*sin(simu_theta + simu_fai - 0.5*M_PI);
00446                         simu_x = m_InSimulatedPosition.data[0] + length*cos(simu_theta + simu_fai);
00447                         simu_y = m_InSimulatedPosition.data[1] + length*sin(simu_theta + simu_fai);
00448 
00449                         // Simulated X value 
00450                         m_OutSimulatedPositionToInventGUI.data[0] = simu_x;                     m_OutSimulatedPositionToLocalization.data[0] = simu_x;
00451 
00452                         // Simulated Y value 
00453                         m_OutSimulatedPositionToInventGUI.data[1] = simu_y;                     m_OutSimulatedPositionToLocalization.data[1] = simu_y;
00454 
00455 /*
00456                         for(int i=2;i<12;i++){
00457                                 m_OutSimulatedPositionToInventGUI.data[i] = m_InSimulatedPosition.data[i];                              m_OutSimulatedPositionToLocalization.data[i] = m_InSimulatedPosition.data[i];
00458                         }
00459 */
00460                         // Simulated Z value
00461                         m_OutSimulatedPositionToInventGUI.data[2] = m_InSimulatedPosition.data[2];                      m_OutSimulatedPositionToLocalization.data[2] = m_InSimulatedPosition.data[2];
00462 
00463                         // Simulated PostureAngle value
00464                         simu_theta += m_SimulatedOffsetAngle;
00465 
00466                         //-- [offset] -180 < dummySimuTheta < 180  
00467                         while (simu_theta <= -1.0*M_PI || simu_theta >= 1.0*M_PI) {
00468                                 if( simu_theta <= -1.0*M_PI)                    simu_theta += 2.0*M_PI;
00469                                 else if ( simu_theta >= 1.0*M_PI)               simu_theta -= 2.0*M_PI;
00470                         }
00471 
00472                         m_OutSimulatedPositionToInventGUI.data[3] = simu_theta;                 m_OutSimulatedPositionToLocalization.data[3] = simu_theta;
00473 
00474                 RTC_DEBUG(("[BEFORE SIMULATED]  [X] = %f ,  [Y] = %f TH[rad]= %f TH[deg]= %f TIME1= %ld TIME2= %ld", m_InSimulatedPosition.data[0], m_InSimulatedPosition.data[1], -1.0*atan2(m_InSimulatedPosition.data[4], m_InSimulatedPosition.data[3]), -180*atan2(m_InSimulatedPosition.data[4], m_InSimulatedPosition.data[3])/M_PI, m_InSimulatedPosition.tm.sec ,m_InSimulatedPosition.tm.nsec));
00475                 RTC_DEBUG(("[AFTER SIMULATED]  [X] = %f ,  [Y] = %f TH[rad]= %f TH[deg]= %f"));
00476 
00477 #if DEBUG       
00478                 //for(int i=0;i<12;i++) ofs << m_OutSimulatedPositionToInventGUI.data[i]  << "    "     ;
00479                 //ofs << std::endl;
00480                 for(int i=0;i<4;i++) ofs << m_OutSimulatedPositionToInventGUI.data[i]  << "       "     ;
00481                 ofs << std::endl;
00482                 //std::cout << "[BEFORE SIMULATED]  [X] = " << m_InSimulatedPosition.data[0]  << " ,  [Y] = " << m_InSimulatedPosition.data[1]   << " TH[rad]= " << -1.0*atan2(m_InSimulatedPosition.data[4], m_InSimulatedPosition.data[3]) << " TH[deg]=" << -180*atan2(m_InSimulatedPosition.data[4], m_InSimulatedPosition.data[3])/M_PI << " TIME1= " << m_InSimulatedPosition.tm.sec << " TIME2=" << m_InSimulatedPosition.tm.nsec << std::endl;
00483                 //std::cout << "[AFTER SIMULATED]  [X] = " << m_OutSimulatedPositionToInventGUI.data[0]  << " ,  [Y] = " << m_OutSimulatedPositionToInventGUI.data[1]   << " TH[rad]= " << simu_theta << " TH[deg]=" << 180*simu_theta/M_PI  << std::endl << std::endl;
00484 #endif
00485 
00486                 // set TIME
00487                         m_OutSimulatedPositionToInventGUI.tm.sec = m_InSimulatedPosition.tm.sec;
00488                         m_OutSimulatedPositionToInventGUI.tm.nsec = m_InSimulatedPosition.tm.nsec;      
00489                         m_OutSimulatedPositionToLocalization.tm.sec = m_InSimulatedPosition.tm.sec;
00490                         m_OutSimulatedPositionToLocalization.tm.nsec = m_InSimulatedPosition.tm.nsec;   
00491                 //*** set Simulated Data to OUTPORT
00492                         m_OutSimulatedPositionToInventGUIOut.write();   
00493                         m_OutSimulatedPositionToLocalizationOut.write();        
00494                 }
00495 
00496         } else {
00497 
00498 #if DEBUG       
00499 //      std::cout << "      ---[SKIP][NoCurrentData] can't get new CURRENT angle data ---" << std::endl;                 
00500 #endif
00501 
00502         }
00503 
00504 //---[add]---------------------------------
00505 
00506   return RTC::RTC_OK;
00507 }
00508 
00509 /*
00510 RTC::ReturnCode_t MotorControl::onAborting(RTC::UniqueId ec_id)
00511 {
00512   return RTC::RTC_OK;
00513 }
00514 */
00515 
00516 /*
00517 RTC::ReturnCode_t MotorControl::onError(RTC::UniqueId ec_id)
00518 {
00519   return RTC::RTC_OK;
00520 }
00521 */
00522 
00523 /*
00524 RTC::ReturnCode_t MotorControl::onReset(RTC::UniqueId ec_id)
00525 {
00526   return RTC::RTC_OK;
00527 }
00528 */
00529 
00530 /*
00531 RTC::ReturnCode_t MotorControl::onStateUpdate(RTC::UniqueId ec_id)
00532 {
00533   return RTC::RTC_OK;
00534 }
00535 */
00536 
00537 /*
00538 RTC::ReturnCode_t MotorControl::onRateChanged(RTC::UniqueId ec_id)
00539 {
00540   return RTC::RTC_OK;
00541 }
00542 */
00543 
00544 
00545 
00546 extern "C"
00547 {
00548  
00549   void MotorControlInit(RTC::Manager* manager)
00550   {
00551     coil::Properties profile(motorcontrol_spec);
00552     manager->registerFactory(profile,
00553                              RTC::Create<MotorControl>,
00554                              RTC::Delete<MotorControl>);
00555   }
00556   
00557 };
00558 
00559