StaubliRX130nAxesVelocityController.hpp

#ifndef STAUBLIRX130_NAXES_VELOCITY_CONTROLLER_HPP
#define STAUBLIRX130_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/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 <ocl/VectorTemplateComposition.hpp>

#include <kdl/chain.hpp>

namespace OCL
{
    class StaubliRX130nAxesVelocityController : public TaskContext
    {
    public:
        StaubliRX130nAxesVelocityController(std::string name);
        virtual ~StaubliRX130nAxesVelocityController();

    protected:
        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;

        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< 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;


        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;

        Property<std::vector<double> > servoIntegrationFactor_prop;
        Property<std::vector<double> > servoGain_prop;
        Property<std::vector<double> > servoFFScale_prop;

        Property<std::vector<std::string> > EmergencyEvents_prop;



    private:
        bool readAbsolutePosition(std::vector<double>& initPos);

        virtual bool startAllAxes();
        virtual bool stopAllAxes();
        virtual bool lockAllAxes();
        virtual bool unlockAllAxes();
        virtual bool addDriveOffset(const std::vector<double>& offset);
        virtual bool prepareForUse();
        virtual bool prepareForUseCompleted() const;
        virtual bool prepareForShutdown();
        virtual bool prepareForShutdownCompleted() const;

        void EmergencyStop(std::string message)
        {
            log(Error) << "---------------------------------------------" << endlog();
            log(Error) << "--------- EMERGENCY STOP --------------------" << endlog();
            log(Error) << "---------------------------------------------" << endlog();
            log(Error) << message << endlog();
            this->fatal();
        };
        std::vector<RTT::Handle> EmergencyEventHandlers;

        bool activated;

        std::vector<double> positionConvertFactor;


        std::vector<double> driveConvertFactor;


        std::vector<double> positionValues, driveValues, velocityValues;
        std::vector<double> servoIntError,outputvel;
        std::vector<double> servoIntegrationFactor,servoGain,servoFFScale;

        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*                    AOut;
        ComediDevice*                    DInOut;
        ComediDevice*                    Encoder;

        AnalogOutInterface*             SubAOut;
        DigitalInInterface*              SubDIn;
        DigitalOutInterface*             SubDOut;

        std::vector<EncoderInterface*>           encoderInterface;
        std::vector<IncrementalEncoderSensor*>   encoder;
        std::vector<AnalogOutput*> vref;
        std::vector<DigitalOutput*>              enable;
        std::vector<AnalogDrive*>                drive;
        DigitalOutput*                           brakeOff;
        DigitalOutput*                           highPowerEnable;
        DigitalInput*                            eStop;
        std::vector<DigitalInput*>               driveFailure;
        DigitalInput*                            armPowerOn;

      const char* port;
      struct termios tio;
      int fd;

#endif
        std::vector<AxisInterface*>             axes;
        std::vector<TimeService::ticks>         _previous_time;
        TimeService::Seconds                    _delta_time;
        std::vector<double>                     init_pos;
    };//class StaubliRX130nAxesVelocityController
}//namespace Orocos
#endif // STAUBLIRX130NAXESVELOCITYCONTROLLER

---