PowerCubeCtrl.h

Go to the documentation of this file.

#ifndef __POWER_CUBE_CTRL_H_
#define __POWER_CUBE_CTRL_H_

// standard includes
#include <iostream>
#include <sstream>
#include <string>
#include <deque>
#include <pthread.h> 

// own includes
#include <schunk_libm5api/m5apiw32.h>
#include <schunk_powercube_chain/moveCommand.h>
#include <schunk_powercube_chain/PowerCubeCtrlParams.h>

class PowerCubeCtrl
{

public:

        PowerCubeCtrl(PowerCubeCtrlParams * params);

        ~PowerCubeCtrl();

        typedef enum
        {
                PC_CTRL_OK = 0, PC_CTRL_NOT_HOMED = -1, PC_CTRL_ERR = -2, PC_CTRL_POW_VOLT_ERR = -3
        } PC_CTRL_STATUS;

        // Functions for initialization and close: //

        bool Init(PowerCubeCtrlParams * params);

        bool isInitialized() const
        {
                return m_Initialized;
        }

        std::string getErrorMessage() const
        {
                return m_ErrorMessage;
        }
        
        PC_CTRL_STATUS getPC_Status() const
        {
                return m_pc_status;
        }

        bool Close();

        // Functions for control: //

        bool MoveJointSpaceSync(const std::vector<double>& angles);

        bool MoveVel(const std::vector<double>& velocities);

        bool Stop();

        bool Recover();

        // functions to set parameters: //

        bool setMaxVelocity(double velocity);
        bool setMaxVelocity(const std::vector<double>& velocities);

        bool setMaxAcceleration(double acceleration);
        bool setMaxAcceleration(const std::vector<double>& accelerations);

        bool setHorizon(double horizon);

        double getHorizon();

        bool setSyncMotion();

        bool setASyncMotion();

        // Functions for getting state and monitoring: //

        bool updateStates();

        bool getStatus(PC_CTRL_STATUS& status, std::vector<std::string>& errorMessages);
        
        std::vector<unsigned long> getVersion();

        bool statusMoving();

        std::vector<double> getPositions();

        std::vector<double> getVelocities();

        std::vector<double> getAccelerations();

        bool doHoming();

        void updateVelocities(std::vector<double> pos_temp, double delta_t);

protected:
        pthread_mutex_t m_mutex;

        int m_DeviceHandle;
        bool m_Initialized;
        bool m_CANDeviceOpened;

        PowerCubeCtrlParams* m_params;
        PC_CTRL_STATUS m_pc_status;

        std::vector<unsigned long> m_status;
        std::vector<std::string> m_ModuleTypes;
        std::vector<unsigned long> m_version;
        std::vector<unsigned char> m_dios;
        std::vector<double> m_positions;
        std::deque< std::vector<double> > m_cached_pos;
        std::vector<double> m_velocities;
        std::vector<double> m_accelerations;

        double m_horizon;

        ros::Time m_last_time_pub;

        std::string m_ErrorMessage;

};

#endif