PowerCubeSim.h

Go to the documentation of this file.

#ifndef __POWER_CUBE_SIM_H_
#define __POWER_CUBE_SIM_H_

#include <schunk_powercube_chain/PowerCubeCtrl.h>
#include <schunk_powercube_chain/Joint.h>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <vector>

using namespace std;
#include <pthread.h>


//-------------------------------------------------------------------------
//                              Defines
// -------------------------------------------------------------------------

#define MAX_VEL 0.5
#define MAX_ACC 0.5
#define K13 0.5
#define K14 0.5
#define K15 3
#define DAEMPFUNG 0.5

//#define __LINUX__

#ifdef SWIG
%module PowerCubeSim
%include "Source/Manipulation/Interfaces/armInterface.h"
%{
        #include "PowerCubeSim.h"
%}
#endif 

class PowerCubeSim;

/* Thread arguments for simulation threads*/
typedef struct
{
        PowerCubeSim * cubeSimPtr;
        int cubeID;
        double targetAngle;
} SimThreadArgs;

class PowerCubeSim
{
        public:
                
                PowerCubeSim();
                ~PowerCubeSim();
                

                bool Init(PowerCubeCtrlParams * params);

                bool isInitialized() const { return m_Initialized; }

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

                int Close(){return true;}
                // Arm-Ansteuerung:
                

                bool MoveJointSpaceSync(const std::vector<double>& Angle);
                                
                bool MoveVel(const std::vector<double>& vel);
                
                bool Stop();
                
                // Funktionen zum setzen von Parametern: //
                
                bool setMaxVelocity(double radpersec);
                
                bool setMaxVelocity(const std::vector<double>& radpersec);

                bool setMaxAcceleration(double radPerSecSquared);
                bool setMaxAcceleration(const std::vector<double>& radPerSecSquared);
                
                // hier die Funktionen zur Statusabfrage: //
        

                bool getConfig(std::vector<double>& result);
                
                bool getJointVelocities(std::vector<double>& result);

                void setCurrentAngles(std::vector<double> Angles);

                void setCurrentJointVelocities(std::vector<double> Angles);
                
                bool statusMoving();
                bool statusMoving(int cubeNo);
                
                bool statusDec();
                
                bool statusAcc();

                //vector<int> getModuleMap(int dev);
                
                //void HomingDone();
                
                typedef enum
                {
                        PC_CTRL_OK = 0,
                        PC_CTRL_NOT_REFERENCED = -1,
                        PC_CTRL_ERR = -2,
                        PC_CTRL_POW_VOLT_ERR = -3
                } PC_CTRL_STATE;
                //int getStatus(){return PC_CTRL_OK;}
                double maxVel;

                void setStatusMoving (int cubeNo, bool moving);
                bool getStatusMoving (int cubeNo) const { return m_MovementInProgress[cubeNo]; }

                vector<int>  getModuleMap() const {return m_IdModules;}
                std::vector<double>  getCurrentAngularMaxVel() {return m_CurrentAngularMaxVel;}
                std::vector<double>  getCurrentAngularMaxAccel() {return m_CurrentAngularMaxAccel;}
            void millisleep(unsigned int milliseconds) const;
        protected:
                
                //void waitForSync();
                //void dontWaitForSync();
                double timeRampMove(double dtheta, double vnow, double v, double a);

                int startSimulatedMovement(std::vector<double> target);


                //void* SimThreadRoutine (void*);       
        
        
                #ifdef COB3
                Manipulator * m_Obj_Manipulator;
#endif
                
                int m_DOF;
                int m_Initialized;
                int m_NumOfModules;
                int m_Dev;
                vector<int> m_IdModules;
                
                std::vector<double> m_maxVel;
                std::vector<double> m_maxAcc;

                Jointd m_AngleOffsets;

                std::vector<bool> m_MovementInProgress;

                std::vector<double> m_CurrentAngles;
                std::vector<double> m_CurrentAngularVel;
                std::vector<double> m_CurrentAngularMaxVel;
                std::vector<double> m_CurrentAngularMaxAccel;



                //vector<unsigned long> startConf;
                
                std::string m_ErrorMessage;

                float maxAcc;

                pthread_mutex_t  m_Angles_Mutex;
                pthread_mutex_t  m_AngularVel_Mutex;
                pthread_mutex_t  m_Movement_Mutex;
                pthread_t  * m_SimThreadID;
                SimThreadArgs ** m_SimThreadArgs;
                
};


#endif