PerformernAxesVelocityController.hpp

#ifndef PERFORMERMK2_NAXES_VELOCITY_CONTROLLER_HPP
#define PERFORMERMK2_NAXES_VELOCITY_CONTROLLER_HPP

#include <vector>
#include <deque>
#include <rtt/RTT.hpp>

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

#if defined (OROPKG_OS_LXRT)
#include <rtt/dev/AnalogOutput.hpp>
#include <rtt/dev/DigitalOutput.hpp>
#include <rtt/dev/DigitalInput.hpp>

#include "dev/ComediDevice.hpp"
#include "dev/ComediSubDeviceAOut.hpp"
#include "dev/ComediSubDeviceDOut.hpp"
#include "dev/ComediSubDeviceDIn.hpp"
#include "dev/ComediEncoder.hpp"

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

#include <termios.h>

#endif
#include "dev/SimulationAxis.hpp"
#include <rtt/dev/AxisInterface.hpp>

#include <ocl/OCL.hpp>

#include <kdl/chain.hpp>

namespace OCL
{
    class PerformerMK2nAxesVelocityController : public TaskContext
    {
    public:
        PerformerMK2nAxesVelocityController(std::string name,std::string propertyfilename="cpf/PerformerMK2nAxesVelocityController.cpf");
        virtual ~PerformerMK2nAxesVelocityController();

    protected:


        Method<bool(int)> resetController_mtd;
        Method<bool(void)> startAllAxes_mtd;

        Method<bool(void)> stopAllAxes_mtd;

        Method<bool(void)> unlockAllAxes_mtd;

        Method<bool(void)> lockAllAxes_mtd;

        Command<bool(void)> prepareForUse_cmd;

        Command<bool(void)> prepareForShutdown_cmd;

        Method<bool(std::vector<double>)> addDriveOffset_mtd;

    Method<bool(int)>  _initPosition;

        DataPort<std::vector<double> > driveValues_port;
        DataPort<std::vector<double> > servoValues_port;

        DataPort<std::vector<double> >  positionValues_port;
        DataPort<std::vector<double> >  velocityValues_port;
        DataPort<std::vector<double> >  jValues_port;
        DataPort<std::vector<double> >  homingSwitchValues_port;
        DataPort< double >              deltaTime_port;

        Property<std::vector <double> > driveLimits_prop;

        Property<std::vector <double> > lowerPositionLimits_prop;

        Property<std::vector <double> > upperPositionLimits_prop;

        Property<std::vector <double> > velocityLimits_prop;

        Property<std::vector <double> > initialPosition_prop;

        Property<std::vector <double> > driveOffset_prop;

        Property<bool > simulation_prop;
        bool            simulation;

        Property<std::vector<double> > servoIntegrationFactor_prop;
        Property<std::vector<double> > servoGain_prop;
        Property<std::vector<double> > servoFFScale_prop;
        Property<std::vector<double> > PIDkp_prop;
        Property<std::vector<double> > PIDTi_prop;
    Property<std::vector<double> > PIDTd_prop;

        Constant<unsigned int> num_axes_attr;

        Attribute<KDL::Chain> chain_attr;
        KDL::Chain kinematics;

        Event< void(std::string) > driveOutOfRange_evt;

        Event< void(std::string) > positionOutOfRange_evt;
        Event< void(std::string) > velocityOutOfRange_evt;

    private:

        virtual bool resetController(const int& axis_id);
        virtual bool startAllAxes();
        virtual bool stopAllAxes();
        virtual bool lockAllAxes();
        virtual bool unlockAllAxes();
        virtual bool addDriveOffset(const std::vector<double>& offset);
    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;

        const std::string propertyfile;

        bool activated;

        std::vector<double> positionConvertFactor;


        std::vector<double> driveConvertFactor;


        std::vector<double> positionValues, driveValues, velocityValues, jValues;
    std::deque<double> positionDeque_axis1, positionDeque_axis2, positionDeque_axis3, positionDeque_axis4, positionDeque_axis5, timeDeque;
        std::vector<double>   homingSwitchValues;
        std::vector<double> servoIntError,outputvel;
        std::vector<double> servoIntegrationFactor,servoGain,servoFFScale, PIDkp, PIDTi, PIDTd;
        std::vector<double> outputvel_kmin1,outputvel_kmin2,velocity_error_k,velocity_error_kmin1,velocity_error_kmin2,position_error_k,position_desired_k;
    bool servoInitialized;
        RTT::TimeService::ticks    previousTime;

    public:

        virtual bool configureHook();
        virtual bool startHook();
        virtual void updateHook();
        virtual void stopHook();
        virtual void cleanupHook();

        KDL::Chain getKinematics(){
            return kinematics;
        };

    private:

        //
        // Members implementing the interface to the hardware
        //
#if  (defined (OROPKG_OS_LXRT))
        std::vector<Axis*>  axes_hardware;

        ComediDevice*                    NI6713;
        ComediDevice*                    NI6602;

        ComediSubDeviceAOut*             SubAOut_NI6713;
        ComediSubDeviceDIn*              SubDIn_NI6713;
        ComediSubDeviceDOut*             SubDOut_NI6602;

        std::vector<EncoderInterface*>           encoderInterface;
        std::vector<IncrementalEncoderSensor*>   encoder;
        std::vector<AnalogOutput*> vref;
        std::vector<DigitalOutput*>              enable;
        std::vector<AnalogDrive*>                drive;
        DigitalOutput*                           brakeAxis2;
        DigitalOutput*                           brakeAxis3;
        DigitalInput*                            armPowerOn;
        DigitalOutput*                           armPowerEnable;
        std::vector<DigitalInput*>               homingSwitch;

#endif
        std::vector<AxisInterface*>              axes;
    TimeService::ticks                       previous_time, initial_time;
    TimeService::Seconds                     delta_time, total_time;

    };//class PerformerMK2nAxesVelocityController
}//namespace Orocos
#endif // PERFORMERNAXESVELOCITYCONTROLLER

---