MotorControl.h

Go to the documentation of this file.

// -*- C++ -*-
#ifndef MOTORCONTROL_H
#define MOTORCONTROL_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
//#include <rtm/idl/IIS.h>

//--- [add] ---------------
#include "IISStub.h"
#include "IISSkel.h"
#include "IIS.hh"

#define LOG 0
#define DEBUG 0
#define CALLBACK 0
#define CALLBACK_DEBUG 0
#include <iostream>
#include <fstream>
#include <math.h>

#if CALLBACK            
#include <rtm/RingBuffer.h>
//-CallBackObject--
class Debug
        : public RTC::OnWriteConvert<RTC::TimedDoubleSeq>
{
        //ofstream ofsCallBack;
        public:
                Debug(void) {   
                  //ofsCallBack.open("data/callback.dat");  
                };
                RTC::TimedDoubleSeq operator()(const RTC::TimedDoubleSeq& value)
                {
                        RTC::TimedDoubleSeq ret(value);
#if CALLBACK_DEBUG      
                        double dummy_sec = value.tm.sec + 1.0e-9*value.tm.nsec;
                        printf("--[CallBack_TIME(CURRENT)]: %lf\t[L]=%lf\t[R]=%lf\n",dummy_sec, value.data[0], value.data[1]);
#endif
                        return ret;
                };

};
//--
#endif
//--- [add] ---------------

// Service implementation headers
// <rtc-template block="service_impl_h">
#include "MotorControlProSVC_impl.h"

// </rtc-template>

// Service Consumer stub headers
// <rtc-template block="consumer_stub_h">

// </rtc-template>

using namespace RTC;
using namespace IIS;

class MotorControl
  : public RTC::DataFlowComponentBase
{
 public:
  MotorControl(RTC::Manager* manager);

  ~MotorControl();

  // <rtc-template block="public_attribute">
  
  // </rtc-template>

  // <rtc-template block="public_operation">
  
  // </rtc-template>

  /***
   * set Configuration parameters
   *
   * The initialize action (on CREATED->ALIVE transition)
   * formaer rtc_init_entry() 
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onInitialize();

  /***
   * 
   *
   * The finalize action (on ALIVE->END transition)
   * formaer rtc_exiting_entry()
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onFinalize();

  /***
   *
   * The startup action when ExecutionContext startup
   * former rtc_starting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);

  /***
   *
   * The shutdown action when ExecutionContext stop
   * former rtc_stopping_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);

  /***
   * (1) update INPORT's data
   * (2) open files for debugging
   * (3) initialize
   * (4) get robot's parameter
   *
   * The activated action (Active state entry action)
   * former rtc_active_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);

  /***
   * (1)close files for debugging
   *
   * The deactivated action (Active state exit action)
   * former rtc_active_exit()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  /***
   * (1)getting target trans/rot Velocity of robot's center data from DriveContr
   * olRTC
   * (2)calculating Torque values
   * (3)sending Torque data to Simulator or real Motor
   * (4)getting current Angle data (encoder dadta) from Simulator or Real Robot
   * (5)sending current Angle data  to DriveControlRTC or OdometryRTC
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

  /***
   *
   * The aborting action when main logic error occurred.
   * former rtc_aborting_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);

  /***
   *
   * The error action in ERROR state
   * former rtc_error_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);

  /***
   *
   * The reset action that is invoked resetting
   * This is same but different the former rtc_init_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
  
  /***
   *
   * The state update action that is invoked after onExecute() action
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);

  /***
   *
   * The action that is invoked when execution context's rate is changed
   * no corresponding operation exists in OpenRTm-aist-0.2.0
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);


 protected:
  // <rtc-template block="protected_attribute">
  
  // </rtc-template>

  // <rtc-template block="protected_operation">
  
  // </rtc-template>

  // Configuration variable declaration
  // <rtc-template block="config_declare">
  double m_PGainL;
  double m_DGainL;
  double m_PGainR;
  double m_DGainR;
  double m_SimulatedOffsetX;
  double m_SimulatedOffsetY;
  double m_SimulatedOffsetAngle;
  int m_leftWheelID;
  int m_rightWheelID;
  double m_radiusOfLeftWheel;
  double m_radiusOfRightWheel;
  double m_lengthOfAxle;
  double m_radiusOfBodyArea;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedVelocity m_TargetVelocity;
  InPort<TimedVelocity> m_TargetVelocityIn;
  TimedDoubleSeq m_InCurrentWheelAngle;
  InPort<TimedDoubleSeq> m_InCurrentWheelAngleIn;
  TimedDoubleSeq m_InSimulatedPosition;
  InPort<TimedDoubleSeq> m_InSimulatedPositionIn;
  
  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedDoubleSeq m_OutSimulatedPositionToInventGUI;
  OutPort<TimedDoubleSeq> m_OutSimulatedPositionToInventGUIOut;
  TimedDoubleSeq m_Torque;
  OutPort<TimedDoubleSeq> m_TorqueOut;
  TimedDoubleSeq m_OutCurrentWheelAngle;
  OutPort<TimedDoubleSeq> m_OutCurrentWheelAngleOut;
  TimedDoubleSeq m_OutSimulatedPositionToLocalization;
  OutPort<TimedDoubleSeq> m_OutSimulatedPositionToLocalizationOut;
  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  RTC::CorbaPort m_InventGUIProvPort;
  RTC::CorbaPort m_BumpProvPort;
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  MotorSVC_impl m_InventGUIMotor;
  MotorSVC_impl m_BumpMotor;
  
  // </rtc-template>

  // Consumer declaration
  // <rtc-template block="consumer_declare">
  
  // </rtc-template>

 private:
  // <rtc-template block="private_attribute">
  
  // </rtc-template>

  // <rtc-template block="private_operation">
  
  // </rtc-template>

//---[add]------------------------
        const static int Dof = 2;            
        //const static int WheelLeftId = 0;       //!< jointID of left wheel
        //const static int WheelRightId = 1;       //!< jointID of right wheel

        double timeStep;   
        double TimeStamp;  
        double TimeStampOld;  
        unsigned long sec;   
        unsigned long nsec; 
        int compSwitch;  
        double qOld[Dof]; 
        bool InitializeFlag;   

        struct BODY {
                int leftID;    
                int rightID;   
                double length;      
                double wheelRadiusLeft;   
                double wheelRadiusRight;   
                double bodyRadius;   
        }Body;

#if DEBUG       
        std::ofstream ofs;   // TODO:someday REMOVE ( for output the SimulatedPosition&Angle data to DEBUG file) 
#endif

#if CALLBACK    
        //-- callback object
        Debug m_debug;  
#endif
//---[add]------------------------

};


extern "C"
{
  DLL_EXPORT void MotorControlInit(RTC::Manager* manager);
};

#endif // MOTORCONTROL_H