======================================================================
  RTComponent: MotorControl specificatioin

  OpenRTM-aist-1.0.0
  Date: $Date$

  @author Yusuke Nakajima (y.nakajima@aist.go.jp)

  Copyright (c) 2008, National Institute of Advanced Industrial Science and Tec
  hnology (AIST). All rights reserved. This program is made available under the te
  rms of the Eclipse Public License v1.0 which accompanies this distribution, and 
  is available at http://www.eclipse.org/legal/epl-v10.html

  This file is generated by rtc-template with the following argments.

#   rtc-template -bcxx --module-name=MotorControl 
#   --module-desc='MotorControl' --module-version=4.0.0 --module-vendor=AIST INVENT 
#   --module-category=Controller --module-comp-type=STATIC 
#   --module-act-type=PERIODIC --module-max-inst=10 
#   --config=PGainL:double:5.0 --config=DGainL:double:2.0 --config=PGainR:double:5.0 --config=DGainR:double:2.0 --config=SimulatedOffsetX:double:0.0 --config=SimulatedOffsetY:double:0.0 --config=SimulatedOffsetAngle:double:0.0 --config=leftWheelID:int:0 --config=rightWheelID:int:1 --config=radiusOfLeftWheel:double:0.1 --config=radiusOfRightWheel:double:0.1 --config=lengthOfAxle:double:0.441 --config=radiusOfBodyArea:double:0.45 
#   --inport=TargetVelocity:IIS::TimedVelocity --inport=InCurrentWheelAngle:TimedDoubleSeq --inport=InSimulatedPosition:TimedDoubleSeq --outport=OutSimulatedPositionToInventGUI:TimedDoubleSeq --outport=Torque:TimedDoubleSeq --outport=OutCurrentWheelAngle:TimedDoubleSeq --outport=OutSimulatedPositionToLocalization:TimedDoubleSeq 
#   --service-idl=/tmp/OpenINVENT-4.0.0/MotorControl_ver4.0.0/MotorControlPro.idl 
#   --service=InventGUIProv:InventGUIMotor:Motor 
#   --service=BumpProv:BumpMotor:Motor 


======================================================================
    Basic Information
======================================================================
# <rtc-template block="module">
Module Name: MotorControl
Description: MotorControl
Version:     4.0.0
Vendor:      AIST INVENT
Category:    Controller
Kind:        DataFlowComponent
Comp. Type:  STATIC
Act. Type:   PERIODIC
MAX Inst.:   10
Lang:        C++
Lang Type:   

Overview:    (1)getting target trans/rot Velocity of robot's center data from Dr
             iveControlRTC
             (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 and OdometryRTC
In/Out:      [INPUT]
               (1) target Trans/Rot Velocity data
               (2) current Angle data (encoder data)
             [OUTPUT]
               (1) Torque data
               (2) current Angle data
Algorithm:   [Calculating Torque values using current/target angle/AngVel values
             ]
               [Torque] = Pgain * ([TargetAngle] - [CurrentAngle]) * Dgain * ([TargetAngVel] 
             - [CurrentAngVel])
# </rtc-template>


======================================================================
    Activity definition
======================================================================

[on_initialize]    implemented
	Description:   set Configuration parameters
	PreCondition:  none
	PostCondition: none

[on_finalize]

[on_startup]

[on_shutdown]

[on_activated]     implemented
	Description:   (1) update INPORT's data
	               (2) open files for debugging
	               (3) initialize
	               (4) get robot's parameter
	PreCondition:  none
	PostCondition: none

[on_deactivated]   implemented
	Description:   (1)close files for debugging
	PreCondition:  none
	PostCondition: none

[on_execute]       implemented
	Description:   (1)getting target trans/rot Velocity of robot's center data fro
	               m DriveControlRTC
	               (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
	PreCondition:  none
	PostCondition: none

[on_aborting]

[on_error]

[on_reset]

[on_state_update]

[on_rate_changed]

======================================================================
    InPorts definition
======================================================================
# <rtc-template block="inport">

	Name:        TargetVelocity
	PortNumber:  0
	Description: target Trans/Rot Velocity of robot's center data for PD control
	PortType: 
	DataType:    IIS::TimedVelocity
	MaxOut: 
	[Data Elements]
		Name:
		Type:            Trans/Rot velocity data: double,    timeStamp:tm
		Number:          Trans velocity data:2, Rot velocity data:1,  timeStamp:1
		Semantics:       target Trans/Rot Velocity of robot's center data for PD con
		                 trol
		                   [Trans Velocity]: translational velocity  of robot's center [m/sec]
		                   [Rot Velocity]: rotation velocity (swing velocity) of robot's center [rad/sec]
		Unit:            Trans velocity [m/s],  Rot velocity [rad/s],  timeStamp: [s
		                 ec],[nanosec]
		Frequency:       
		Operation Cycle: 
		RangeLow:
		RangeHigh:
		DefaultValue:

	Name:        InCurrentWheelAngle
	PortNumber:  1
	Description: current Wheel Angle data for PD control
	PortType: 
	DataType:    TimedDoubleSeq
	MaxOut: 
	[Data Elements]
		Name:
		Type:            angle data: double,    timeStamp:tm
		Number:          angle data:2,  timeStamp:1
		Semantics:       current Wheel Angle data for PD control
		                  [0]: left wheel's current data
		                  [1]: right wheel's current data
		Unit:            angle data : [rad],  timeStamp: [sec],[nanosec]
		Frequency:       
		Operation Cycle: 
		RangeLow:
		RangeHigh:
		DefaultValue:

	Name:        InSimulatedPosition
	PortNumber:  2
	Description: current Simulated Position data
	PortType: 
	DataType:    TimedDoubleSeq
	MaxOut: 
	[Data Elements]
		Name:
		Type:            double
		Number:          12 (x,y,z, posture angle matrix:9)
		Semantics:       current Simulated Position data (the position of center of 
		                 robots model)
		                  [0]: X data
		                  [1]: Y data
		                  [2]: Z data
		                  [3]-[11]: 3x3 Matrix data (posture angle)
		Unit:            X,Y,Z data : [m]
		Frequency:       
		Operation Cycle: 
		RangeLow:
		RangeHigh:
		DefaultValue:


# </rtc-template>

======================================================================
    OutPorts definition
======================================================================
# <rtc-template block="outport">

	Name:        OutSimulatedPositionToInventGUI
	PortNumber:  0
	Description: current Simulated Position data
	PortType: 
	DataType:    TimedDoubleSeq
	MaxOut: 
	[Data Elements]
		Name:
		Type:            simulated position data: double,    timeStamp:tm
		Number:          simulated position data:4 (x,y,z, posture angle ) ,  timeSt
		                 amp:1
		Semantics:       current Simulated Position data (the position of center of 
		                 robots model)
		                  [0]: X data
		                  [1]: Y data
		                  [2]: Z data
		                  [3]: THETA (posture angle)
		Unit:            X,Y,Z data : [m], THETA[rad] ,  timeStamp: [sec],[nanosec]
		Frequency:       
		Operation Cycle: 
		RangeLow:
		RangeHigh:
		DefaultValue:

	Name:        Torque
	PortNumber:  1
	Description: Torque data
	PortType: 
	DataType:    TimedDoubleSeq
	MaxOut: 
	[Data Elements]
		Name:
		Type:            Torque data: double,    timeStamp:tm
		Number:          Torque data:2,  timeStamp:1
		Semantics:       Torque data
		                  [0]: left wheel's torque data
		                  [1]: right wheel's torque data
		Unit:            torque data : [],  timeStamp: [sec],[nanosec]
		Frequency:       
		Operation Cycle: 
		RangeLow:
		RangeHigh:
		DefaultValue:

	Name:        OutCurrentWheelAngle
	PortNumber:  2
	Description: current Wheel Angle data for sending to [OdometryRTC]
	PortType: 
	DataType:    TimedDoubleSeq
	MaxOut: 
	[Data Elements]
		Name:
		Type:            angle data: double,    timeStamp:tm
		Number:          angle data:2,  timeStamp:1
		Semantics:       current Wheel Angle data for PD control
		                  [0]: left wheel's current data
		                  [1]: right wheel's current data
		Unit:            angle data : [rad],  timeStamp: [sec],[nanosec]
		Frequency:       
		Operation Cycle: 
		RangeLow:
		RangeHigh:
		DefaultValue:

	Name:        OutSimulatedPositionToLocalization
	PortNumber:  3
	Description: current Simulated Position data
	PortType: 
	DataType:    TimedDoubleSeq
	MaxOut: 
	[Data Elements]
		Name:
		Type:            simulated position data: double,    timeStamp:tm
		Number:          simulated position data:4 (x,y,z, posture angle ) ,  timeSt
		                 amp:1
		Semantics:       current Simulated Position data (the position of center of 
		                 robots model)
		                  [0]: X data
		                  [1]: Y data
		                  [2]: Z data
		                  [3]: THETA (posture angle)
		Unit:            X,Y,Z data : [m], THETA[rad] ,  timeStamp: [sec],[nanosec]
		Frequency:       
		Operation Cycle: 
		RangeLow:
		RangeHigh:
		DefaultValue:


# </rtc-template>


======================================================================
    Service Port definition
======================================================================
# <rtc-template block="serviceport">
	PortName:    InventGUIProv
	Description: for InventGUIRTC
	Interface:   [Provider]
	              (1) for changing GAIN parameters
	              (2) for receiving start/finish etc.. commands
		[Service Provider Interfaces]
		Name:          InventGUIMotor
		Type:          Motor
		Description:   [Provider]
		                (1) for changing GAIN parameters
		                (2) for receiving start/finish etc.. commands
		Argument:      setPDGain()
		                 PGainL : P gain (LeftWheel) of PD control
		                 PGainR : P gain (RightWheel) of PD control
		                 DGainL : D gain (LeftWheel) of PD control
		                 DGainR : D gain (RightWheel) of PD control
		Return Value:  void
		Exception:     
		PreCondition:  
		PostCondition: 
	PortName:    BumpProv
	Description: for BumpDetectionRTC
	Interface:   (1) for receiving bump command
		[Service Provider Interfaces]
		Name:          BumpMotor
		Type:          Motor
		Description:   If get BumpDetected information,
		               set 0.0 values to Torque data 
		               for stopping MOTOR. 
		Argument:      void
		Return Value:  void
		Exception:     
		PreCondition:  
		PostCondition: 
# </rtc-template> 

======================================================================
    Configuration definition
======================================================================
# <rtc-template block="configuration">
	Configuration:
		Name:            PGainL PGainL
		Description:     Left Wheel's P gain value of PD control
		Type:            double
		DefaultValue:    5.0 5.0
		Unit:            
		Range:           
		Constraint:      

		Name:            DGainL DGainL
		Description:     Left Wheel's D gain value of PD control
		Type:            double
		DefaultValue:    2.0 2.0
		Unit:            
		Range:           
		Constraint:      

		Name:            PGainR PGainR
		Description:     Right Wheel's P gain value of PD control
		Type:            double
		DefaultValue:    5.0 5.0
		Unit:            
		Range:           
		Constraint:      

		Name:            DGainR DGainR
		Description:     Right Wheel's D gain value of PD control
		Type:            double
		DefaultValue:    2.0 2.0
		Unit:            
		Range:           
		Constraint:      

		Name:            SimulatedOffsetX SimulatedOffsetX
		Description:     distance from CenterOfBody to CenterOfAxle of Model
		Type:            double
		DefaultValue:    0.0 0.0
		Unit:            
		Range:           
		Constraint:      

		Name:            SimulatedOffsetY SimulatedOffsetY
		Description:     distance from CenterOfBody to CenterOfAxle of Model
		Type:            double
		DefaultValue:    0.0 0.0
		Unit:            
		Range:           
		Constraint:      

		Name:            SimulatedOffsetAngle SimulatedOffsetAngle
		Description:     offset for adjustment of initial posture angle
		Type:            double
		DefaultValue:    0.0 0.0
		Unit:            
		Range:           
		Constraint:      

		Name:            leftWheelID leftWheelID
		Description:     ID number of left wheel of mobile robot.
		Type:            int
		DefaultValue:    0 0
		Unit:            none
		Range:           
		Constraint:      none

		Name:            rightWheelID rightWheelID
		Description:     ID number of right wheel of mobile robot.
		Type:            int
		DefaultValue:    1 1
		Unit:            none
		Range:           
		Constraint:      none

		Name:            radiusOfLeftWheel radiusOfLeftWheel
		Description:     radius of left wheel of mobile robot
		Type:            double
		DefaultValue:    0.1(RefHard2 model) 0.1
		Unit:            [m]
		Range:           
		Constraint:      none

		Name:            radiusOfRightWheel radiusOfRightWheel
		Description:     radius of right wheel of mobile robot
		Type:            double
		DefaultValue:    0.1(RefHard2 model) 0.1
		Unit:            [m]
		Range:           
		Constraint:      none

		Name:            lengthOfAxle lengthOfAxle
		Description:     length of axle (from left wheel to right wheel)
		Type:            double
		DefaultValue:    0.441(RefHard2 model) 0.441
		Unit:            [m]
		Range:           
		Constraint:      none

		Name:            radiusOfBodyArea radiusOfBodyArea
		Description:     length of body radius (from center of Axle to corner of Bod
		                 y)
		Type:            double
		DefaultValue:    0.45(RefHard2 model) 0.45
		Unit:            [m]
		Range:           
		Constraint:      

# </rtc-template> 
This area is reserved for future OpenRTM.

