CRSnAxesVelocityController.hpp

#ifndef LIAS_NAXES_VELOCITY_CONTROLLER_HPP
#define LIAS_NAXES_VELOCITY_CONTROLLER_HPP
#include <vector>
#include <rtt/RTT.hpp>

#include <rtt/TaskContext.hpp>
#include <rtt/Ports.hpp>
#include <rtt/Event.hpp>
#include <rtt/Properties.hpp>


#include <rtt/RTT.hpp>

#include "IP_Encoder_6_EncInterface.hpp"
#include "dev/SimulationAxis.hpp"


#if defined (OROPKG_OS_LXRT)

    #include "CombinedDigitalOutInterface.hpp"

    #include <rtt/dev/AnalogOutput.hpp>
    #include <rtt/dev/DigitalOutput.hpp>
    #include <rtt/dev/DigitalInput.hpp>
    #include <rtt/dev/AxisInterface.hpp>

    #include "dev/AnalogDrive.hpp"
    #include "dev/Axis.hpp"
    #include "dev/IncrementalEncoderSensor.hpp"
#endif

#include "LiASConstants.hpp"

#include <ocl/OCL.hpp>

namespace OCL {

class CRSnAxesVelocityController : public RTT::TaskContext
{
public:
   CRSnAxesVelocityController(const std::string& name,const std::string& propertyfilename="cpf/crs.cpf");
  virtual ~CRSnAxesVelocityController();

protected:
 //
 // Members implementing the component interface
 //

 //
 // COMMANDS :
 //

  virtual bool startAxis(int axis);
  virtual bool startAxisCompleted(int axis) const;

  virtual bool startAllAxes();
  virtual bool startAllAxesCompleted() const;

  virtual bool stopAxis(int axis);
  virtual bool stopAxisCompleted(int axis) const;

  virtual bool stopAllAxes();
  virtual bool stopAllAxesCompleted() const;

  virtual bool lockAxis(int axis);
  virtual bool lockAxisCompleted(int axis) const;

  virtual bool lockAllAxes();
  virtual bool lockAllAxesCompleted() const;

  virtual bool unlockAxis(int axis);
  virtual bool unlockAxisCompleted(int axis) const;

  virtual bool unlockAllAxes();
  virtual bool unlockAllAxesCompleted() const;

  virtual bool addDriveOffset(int axis,double offset);
  virtual bool addDriveOffsetCompleted(int axis,double offset) const;

  // initPosition : argument q gegeven via initialPosition.
  virtual bool initPosition(int axis);
  virtual bool initPositionCompleted(int axis) const;

  virtual bool prepareForUse();
  virtual bool prepareForUseCompleted() const;
  virtual bool prepareForShutdown();
  virtual bool prepareForShutdownCompleted() const;

  // METHOD returning the status of each axis
  virtual bool isDriven(int axis);

private:
  std::vector<RTT::ReadDataPort<double>*>   driveValue;

  std::vector<RTT::WriteDataPort<bool>*>    reference;
  std::vector<RTT::WriteDataPort<double>*>  positionValue;
  std::vector<RTT::WriteDataPort<double>*>  output;

private:
 const std::string _propertyfile;

  RTT::Property<std::vector <double> >     driveLimits;

  RTT::Property<std::vector <double> >     lowerPositionLimits;

  RTT::Property<std::vector <double> >     upperPositionLimits;

  RTT::Property<std::vector <double> >     initialPosition;

  RTT::Property<std::vector<double> >      signAxes;

  RTT::Property<std::vector<double> >      offset;


  RTT::Event< void(int,double) > driveOutOfRange;

  RTT::Event< void(int,double) > positionOutOfRange;

   virtual bool startup();

   virtual void update();

   virtual void shutdown();

private:
  //
  // Private properties of this component.
  //
  //

   RTT::Constant<unsigned int> _num_axes;

private:
  // to keep track of which axes are homed :
  std::vector<bool>  _homed;
  //
  //   Servo-loop gain :
  //   property to indicate initial value and variable to store actual value

  RTT::Property<std::vector <double> >     servoGain;
  std::vector<double>                              _servoGain;
  //
  //   Servo-loop integration time
  //   property to indicate initial value and variable to store actual value
  RTT::Property<std::vector <double> >     servoIntegrationFactor;
  std::vector<double>                              _servoIntegrationFactor;

  //
  //   Feedforward scale factor
  //   (mainly to turn feedforward on and off).
  //   property to indicate initial value and variable to store actual value
  RTT::Property<std::vector <double> >     servoFFScale;
  std::vector<double>                              _servoFFScale;

  RTT::Property<std::vector<double> >     servoDerivTime;

  //
  // Continuous state for the servo-loop
  //
  std::vector<double>  servoIntVel;     // integrated velocity
  std::vector<double>  servoIntError;   // integrated error
  bool                               servoInitialized;
  RTT::TimeService::ticks    previousTime;
  std::vector<double>        previousPos;  // used by friction comp.

  //
  // Command to read and apply the properties to the controller
  //
  virtual bool changeServo();
  virtual bool changeServoCompleted() const;

private:
  //
  // Members implementing the interface to the hardware
  //
  #if !defined (OROPKG_OS_LXRT)
     std::vector<OCL::SimulationAxis*>      _axes;
     std::vector<RTT::AxisInterface*>    _axesInterface;
  #else
    std::vector<OCL::Axis*>                _axes;

    std::vector<RTT::AxisInterface*>    _axesInterface;

    RTT::DigitalOutInterface*               _IP_Digital_24_DOut;
    IP_Encoder_6_Task*                                      _IP_Encoder_6_task;
    RTT::AnalogOutInterface*  _IP_FastDac_AOut;
    RTT::DigitalInInterface*                _IP_OptoInput_DIn;

    RTT::DigitalOutput*                    _enable;
    RTT::DigitalOutput*                    _combined_enable[LiAS_NUM_AXIS];
    RTT::CombinedDigitalOutInterface*   _combined_enable_DOutInterface;
    RTT::DigitalOutput*                    _brake;
    RTT::DigitalOutput*                    _combined_brake[2];
    RTT::CombinedDigitalOutInterface*   _combined_brake_DOutInterface;

    RTT::EncoderInterface*              _encoderInterface[LiAS_NUM_AXIS];
    RTT::IncrementalEncoderSensor*         _encoder[LiAS_NUM_AXIS];
    RTT::AnalogOutput*       _vref[LiAS_NUM_AXIS];
    RTT::AnalogDrive*                      _drive[LiAS_NUM_AXIS];
    RTT::DigitalInput*                     _reference[LiAS_NUM_AXIS];
  #endif

  bool _activate_axis2, _activate_axis3, _deactivate_axis2, _deactivate_axis3;

};

} // namespace Orocos

#endif // LIAS_HARDWARE_HPP

---