PowerCubeSim.cpp

Go to the documentation of this file.

#include <schunk_powercube_chain/PowerCubeSim.h>
#include <string>
#include <sstream>
#ifdef PYTHON_THREAD
#include <Python.h>
#endif
//#define __LINUX__

#define DEG 57.295779524
#define MANUAL_AXES0_OFFSET  1.8
#define MANUAL_AXES6_OFFSET 1.5


#define SIM_CLOCK_FREQUENCY 10.0 //ms
using namespace std;

#ifndef PCTRL_CHECK_INITIALIZED()
#define PCTRL_CHECK_INITIALIZED() \
if ( isInitialized()==false )                                                                                                   \
{                                                                                                                                               \
    m_ErrorMessage.assign("Manipulator not initialized.");              \
        return false;                                                                                                           \
}
#endif


PowerCubeSim::PowerCubeSim()
{
        m_Dev=0;
        m_NumOfModules = 0;
        m_SimThreadArgs = NULL;
        m_SimThreadID = NULL;

        //pthreads variables
        
        
}


bool PowerCubeSim::Init(PowerCubeCtrlParams * params)
{
        //std::cout << "---------- PowerCubes-Simulation Init -------------" << std::endl;
        m_DOF=params->GetNumberOfDOF();

        m_Dev=0;
        m_NumOfModules = m_DOF;
        
        m_maxVel = params->GetMaxVel();
        m_maxAcc = params->GetMaxAcc();

        // Make sure m_IdModules is clear of Elements:
        m_IdModules.clear();
                        

        m_CurrentAngles.resize(m_DOF);
        m_CurrentAngularMaxAccel.resize(m_DOF);
        m_CurrentAngularMaxVel.resize(m_DOF);
        m_CurrentAngularVel.resize(m_DOF);

        //m_AngleOffsets = m_Obj_Manipulator->GetThetaOffsets();

        for(int i=0; i<m_DOF; i++)
        {
                std::ostringstream os;
                os << "ID_Module_Number" << i+1;
                
                // Get the Module Id from the config file
                
                //set initial angles to zero
                m_CurrentAngles[i] = 0.0;
                m_CurrentAngularVel[i] =0.0;
                m_CurrentAngularMaxVel[i] = m_maxVel[i];
                m_CurrentAngularMaxAccel[i] =m_maxAcc[i];
                
                //printf("Offset Angle %d: %f\n",i,m_AngleOffsets[i]);
        }
        
        
        // Jetzt Winkelgrenzen setzen:
        //m_AngleLimits = m_Obj_Manipulator->GetLimitsTheta();

        // pthreads initialisation
        
        pthread_mutex_init(&m_Movement_Mutex,NULL);
        pthread_mutex_init(&m_Angles_Mutex,NULL);
        pthread_mutex_init(&m_AngularVel_Mutex,NULL);

        m_SimThreadID = (pthread_t*)malloc(m_DOF * sizeof(pthread_t));
        m_MovementInProgress.resize(m_DOF);
        m_SimThreadArgs = (SimThreadArgs**)malloc(m_DOF * sizeof(SimThreadArgs*));
        for (int i = 0; i < m_DOF; i++)
        {
                m_MovementInProgress[i] = false;
                m_SimThreadArgs[i] = new SimThreadArgs();
                m_SimThreadArgs[i]->cubeSimPtr = this;
                m_SimThreadArgs[i]->cubeID = i;
        }
        setMaxVelocity(MAX_VEL);
        setMaxAcceleration(MAX_ACC);


        //std::cout << "---------- PowerCubes Init fertig ----------" << std::endl;
        m_Initialized = true;
        return true;
}


PowerCubeSim::~PowerCubeSim()
{ 
        free(m_SimThreadID);
        for (int i = 0; i < m_DOF; i++)
        {
                delete m_SimThreadArgs[i];
        }
        free(m_SimThreadArgs);

}

bool PowerCubeSim::getConfig(std::vector<double>& result)
{
        PCTRL_CHECK_INITIALIZED();
        //lock mutex
        //
        //std::cout << "getConfig: Waiting for Current_Angles_Mutex ... ";
        pthread_mutex_lock(&m_Angles_Mutex);
        //m_Out << "locked"<<endl;
        result.resize(m_DOF);
        for(int i; i < m_DOF; i++)
                result[i] = m_CurrentAngles[i]*DEG;
        //unlock mutex
        //
        //m_Out <<"getConfig: Unlocking Angles_Mutex ... ";
        pthread_mutex_unlock(&m_Angles_Mutex);
        //m_Out <<"unlocked "<<endl;
        
        return true;
}



bool PowerCubeSim::getJointVelocities(std::vector<double>& result)
{
        PCTRL_CHECK_INITIALIZED();
        //lock mutex
        //
        //std::cout << "getConfig: Waiting for Current_Angles_Mutex ... ";
        pthread_mutex_lock(&m_AngularVel_Mutex);
        //m_Out << "locked"<<endl;
        result.resize(m_DOF);
        result = m_CurrentAngularVel;
        //unlock mutex
        //
        //m_Out <<"getConfig: Unlocking Angles_Mutex ... ";
        pthread_mutex_unlock(&m_AngularVel_Mutex);
        //m_Out <<"unlocked "<<endl;

        return true;
}
void PowerCubeSim::setCurrentAngles(std::vector<double> Angles)
{

        //std::cout << "setCurrentAngles: " << Angles[0] << " " << Angles[1] << " " << Angles[2] << " " << Angles[3] << " " << Angles[4] << " " << Angles[5] << " \n";
        pthread_mutex_lock(&m_Angles_Mutex);
        //m_Out << "locked"<<endl;
        
        m_CurrentAngles = Angles;
        //unlock mutex
        //
        //m_Out <<"setCurrentAngles: Unlocking Angles_Mutex ... ";
        pthread_mutex_unlock(&m_Angles_Mutex);
        //m_Out <<"unlocked "<<endl;
        

}

void PowerCubeSim::setCurrentJointVelocities( std::vector<double> AngularVel)
{

        //lock mutex
        //m_Out << "setCurrentJointVelocities: Waiting for AngularVel_Mutex ... ";
        pthread_mutex_lock(&m_AngularVel_Mutex);
        //m_Out << "locked"<<endl;
        
        m_CurrentAngularVel = AngularVel;
        //unlock mutex
        //
        //m_Out <<"setCurrentJointVelocities: Unlocking AngularVel_Mutex ... ";
        pthread_mutex_unlock(&m_AngularVel_Mutex);
        //m_Out <<"unlocked "<<endl;

}
        


bool PowerCubeSim::MoveJointSpaceSync(const std::vector<double>& target)
{
        PCTRL_CHECK_INITIALIZED();
        std::cout << "Starting MoveJointSpaceSync(Jointd Angle) ... "<<endl;
        std::vector<double> targetRAD;
        targetRAD.resize(m_DOF);
        for(int i = 0; i<m_DOF; i++)
                targetRAD[i] = target[i]/DEG;

        // Evtl. Fragen zur Rechnung / zum Verfahren an: Felix.Geibel@gmx.de
        std::vector<double> acc(m_DOF);
        std::vector<double> vel(m_DOF);
        
        double TG = 0;
        
        try
        {
                
                // Ermittle Joint, der bei max Geschw. und Beschl. am längsten braucht:
                int DOF = m_DOF;

                std::vector<double> posnow;
                if ( getConfig(posnow) == false )
                    return false;


                std::vector<double> velnow;
                if ( getJointVelocities(velnow) == false )
                    return false;

                std::vector<double> times(DOF);

                for (int i=0; i < DOF; i++)
                {
                        RampCommand rm(posnow[i], velnow[i], targetRAD[i], m_maxAcc[i], m_maxVel[i]);
                        times[i] = rm.getTotalTime();
                }
                
                // determine the joint index that has the greates value for time
                int furthest = 0;

                double max = times[0];

            for (int i=1; i<m_DOF; i++)
            {
                    if (times[i] > max)
                    {
                            max = times[i];
                            furthest = i;
                    }
            }

                RampCommand rm_furthest(posnow[furthest], velnow[furthest], targetRAD[furthest], m_maxAcc[furthest], m_maxVel[furthest]);

                double T1 = rm_furthest.T1();
                double T2 = rm_furthest.T2();
                double T3 = rm_furthest.T3();

                // Gesamtzeit:
                TG = T1 + T2 + T3;
                
                // Jetzt Geschwindigkeiten und Beschl. für alle:
                acc[furthest] = m_maxAcc[furthest];
                vel[furthest] = m_maxVel[furthest];
                
                for (int i = 0; i < DOF; i++)
                {
                        if (i != furthest)
                        {
                                double a; double v;
                                // a und v berechnen:
                                RampCommand::calculateAV(
                                        posnow[i],
                                        velnow[i],
                                        targetRAD[i],
                                        TG, T3,
                                        m_maxAcc[i],
                                        m_maxVel[i],
                                        a,
                                        v);

                                acc[i] = a;
                                vel[i] = v;
                        }
                }
        }
        catch(...)
        {
                return false;
        }

        startSimulatedMovement(targetRAD);
        
        // Errechnete Gesamtzeit zurückgeben (könnte ja nützlich sein)
        return true;
                
}


                

double PowerCubeSim::timeRampMove(double dtheta, double vnow, double v, double a)
{
        // die Zeiten T1, T2 und T3 errechnen
        // ACHTUNG: Hier wird vorläufig angenommen, dass die Bewegung groß ist, d.h. es eine Phase der Bewegung
        // mit konst. Geschw. gibt. Der andere Fall wird erst später hinzugefügt.
        //m_Out << "DEBUG: timeRampMove" << endl;
        //m_Out << "-------------------" << endl;
        //m_Out << "dtheta: " << dtheta << ", vnow: " << vnow << ", v: " << v << ", a: " << a << endl;
        //m_Out << endl;
        
        // Wird Joint mit +vmax oder -vmax drehen?
        double vm = (dtheta<0)?-v:v;
        double am = (dtheta<0)?-a:a;
        //m_Out << "vm: " << vm << endl;
        //m_Out << "am: " << am << endl;
        
        // Zeit bis vm erreicht:
        double T1 = (vm - vnow) / am;
        // Winkel, der hierbei zurückgelegt wird:
        double dtheta1 = vnow * T1 + 0.5 * am * T1 * T1;
        //m_Out << "T1: " << T1 << endl;
        //m_Out << "dtheta1: " << dtheta1 << endl;
        
        // Zeit zum Bremsen:
        double T3 = vm / am;
        // Winkel hierbei:
        double dtheta3 = 0.5 * vm * T3;
        
        // Verbleibender Winkel:
        double dtheta2 = dtheta - dtheta1 - dtheta3;
        // Also Restzeit (Bew. mit vm):
        double T2 = dtheta2 / vm;
        //m_Out << "T2: " << T2 << endl;
        //m_Out << "dtheta2: " << dtheta2 << endl;
        //m_Out << "T3: " << T3 << endl;
        //m_Out << "dtheta3: " << dtheta3 << endl;

        
        // Gesamtzeit zurückgeben:
        return T1 + T2 + T3;
}

                
bool PowerCubeSim::MoveVel(const std::vector<double>& vel)
{
        PCTRL_CHECK_INITIALIZED();
        /* TODO
        if (getStatus() != PC_CTRL_OK)
        {
                printf("PowerCubeSim::MoveVel: canceled!\n");
                return;
        }
        for(int i=0;i<m_NumOfModules;i++)
        {
                PCube_moveVel(m_Dev,m_IdModules[i],AngularVelocity[i]);
        }
        PCube_startMotionAll(m_Dev);
        */
}
                
                                
bool PowerCubeSim::Stop()
{
        for (int i=0; i < m_DOF; i++)
                setStatusMoving(i, false);
        return true;
}

bool PowerCubeSim::setMaxVelocity(double radpersec)
{ 
        for (int i=0; i<m_DOF; i++)
        {
                m_CurrentAngularMaxVel[i] = radpersec;
        }
        return true;
}

bool PowerCubeSim::setMaxVelocity(const std::vector<double>& radpersec)
{
        for (int i=0; i<m_DOF; i++)
        {
                m_CurrentAngularMaxVel[i] = radpersec[i];
        }
        return true;
}


bool PowerCubeSim::setMaxAcceleration(double radPerSecSquared)
{
    PCTRL_CHECK_INITIALIZED();

        for (int i=0; i<m_DOF; i++)
        {
                m_CurrentAngularMaxAccel[i] = radPerSecSquared;
        }

    return true;
}
bool PowerCubeSim::setMaxAcceleration(const std::vector<double>& radPerSecSquared)
{
    PCTRL_CHECK_INITIALIZED();

        for (int i=0; i<m_DOF; i++)
        {
                m_CurrentAngularMaxAccel[i] = radPerSecSquared[i];
        }

    return true;
}



bool PowerCubeSim::statusMoving()
{
        PCTRL_CHECK_INITIALIZED();
        bool isMoving = false;

        //m_Out << "statusMoving: Waiting for Movement_Mutex ... ";
        pthread_mutex_lock(&m_Movement_Mutex);
        //m_Out << "locked"<<endl;
        for (int i = 0; i < m_DOF; i++)
        {
                if (m_MovementInProgress[i])
                {
                        isMoving = true;
                        break;
                }
        }

        //unlock mutex
        //m_Out <<"statusMoving: Unlocking Movement_Mutex ... ";
        pthread_mutex_unlock(&m_Movement_Mutex);
        //m_Out <<"unlocked "<<endl;

        return isMoving;
}

bool PowerCubeSim::statusMoving(int cubeNo)
{
        PCTRL_CHECK_INITIALIZED();
        bool isMoving = false;

        //m_Out << "statusMoving: Waiting for Movement_Mutex ... ";
        pthread_mutex_lock(&m_Movement_Mutex);
        //m_Out << "locked"<<endl;
        if (m_MovementInProgress[cubeNo])
        {
                isMoving = true;
        }

        //unlock mutex
        //m_Out <<"statusMoving: Unlocking Movement_Mutex ... ";
        pthread_mutex_unlock(&m_Movement_Mutex);
        //m_Out <<"unlocked "<<endl;

        return isMoving;
}
                
void PowerCubeSim::setStatusMoving (int cubeNo, bool moving)
{
        //m_Out << "setStatusMoving: Waiting for Movement_Mutex ... ";
        pthread_mutex_lock(&m_Movement_Mutex);
        //m_Out << "locked"<<endl;

        m_MovementInProgress[cubeNo] = moving;

        //m_Out <<"setStatusMoving: Unlocking Movement_Mutex ... ";
        pthread_mutex_unlock(&m_Movement_Mutex);
        //m_Out <<"unlocked "<<endl;

}

bool PowerCubeSim::statusDec()
{
        PCTRL_CHECK_INITIALIZED();
        /* TODO
        for (int i=0; i<m_NumOfModules; i++)
        {
                unsigned long status;
                PCube_getModuleState(m_Dev,m_IdModules[i], &status);
                if (status & STATEID_MOD_RAMP_DEC)
                        return true;
        }
        */
        return false;
}


bool PowerCubeSim::statusAcc()
{
        PCTRL_CHECK_INITIALIZED();
        /* TODO
        for (int i=0; i<m_NumOfModules; i++)
        {
                unsigned long status;
                PCube_getModuleState(m_Dev,m_IdModules[i], &status);
                if (status & STATEID_MOD_RAMP_ACC)
                        return true;
        }

        */
        return false;
}

void  * SimThreadRoutine(void* threadArgs)
{
        //get argument
        SimThreadArgs * args =  (SimThreadArgs*)threadArgs;
        PowerCubeSim* cubeSimPtr = args->cubeSimPtr;
        int cubeNo = args->cubeID;
        double targetAngle = args->targetAngle;

        //std::cout << "Thread started for cube no "<< cubeNo<<"["<<(cubeSimPtr->getModuleMap())[cubeNo]<<"]"<<endl;
        
        //calculate phases of movement
        float t1,t2,t,tges; //acceleration time t1/t , duration of maximum vel t2, total time tges
        double deltaT = SIM_CLOCK_FREQUENCY/1000; //clock period
        t1=t2=t=tges=0;
        float maxVel = (cubeSimPtr->getCurrentAngularMaxVel())[cubeNo];
        float maxAccel = (cubeSimPtr->getCurrentAngularMaxAccel())[cubeNo];
        std::vector<double> currAngles;
        cubeSimPtr->getConfig(currAngles);
        std::vector<double> currVels;
        cubeSimPtr->getJointVelocities(currVels);

        double deltaAngle = targetAngle - currAngles[cubeNo];
        
        //acceleration phase
        t1 = maxVel/maxAccel;

        //constant velocity phase
        t2 = abs(deltaAngle)/maxVel - t1;

        //cubeSimPtr->getOutputStream()<<" (abs(deltaAngle) >, maxVel*maxVel/maxAccel)?"<< abs(deltaAngle) <<"; "<< maxVel*maxVel/maxAccel<<endl;
        
        //is maxVel reached?
        if (abs(deltaAngle) > maxVel*maxVel/maxAccel)
        {
                tges=2*t1+t2;
        }
        else
        {

                //how long will we accelerate?
                t = sqrt(abs(deltaAngle)/(maxAccel));
                //what velocity will be reached?
                //maxVel = maxAccel*t;
                tges = 2*t;
        }

        //determine direction of movement
        if (deltaAngle<0)
        {
                maxAccel = -maxAccel;
                maxVel = -maxVel;
        }
        std::cout << "maxVel: "<<maxVel<<"; maxAccel: "<<maxAccel<< "; t1 [ms]: "<<t1<<"; t2 [ms]: "<<t2 <<"; deltaAngle[rad]" <<deltaAngle<<endl;

        //control loop
        
        double simulatedTime = 0.0; 
        int n = 0;  
        float currDeltaAngle = deltaAngle;

        if (abs(deltaAngle) > 0.001)
        {
                while ((simulatedTime <= tges) && cubeSimPtr->getStatusMoving(cubeNo) /*||(abs(currDeltaAngle) > 0.001)*/)
                //while (abs(deltaAngle)>0.005)
                {
                        //advavnce in simulated time
                        cubeSimPtr->millisleep((int)SIM_CLOCK_FREQUENCY);
                        simulatedTime += SIM_CLOCK_FREQUENCY/1000; //=n*deltaT
                        n++;
                        cubeSimPtr->getJointVelocities(currVels);
                        //calculate delta phi
                        double deltaPhi = 0.0;

                        //is max Vel reached?
                        if (abs(deltaAngle) > maxVel*maxVel/abs(maxAccel))
                        {
                                if (simulatedTime < t1)
                                {
                                        deltaPhi = 0.5*maxAccel*(n*deltaT*n*deltaT - (n-1)*deltaT*(n-1)*deltaT);
                                        currVels[cubeNo] = maxAccel * n*deltaT;
                                        std::cout << "Phase 1, maxVel ->";

                                }
                                else if ((t1 < simulatedTime) && (simulatedTime < t1+t2))
                                {
                                        deltaPhi = maxVel * deltaT;
                                        currVels[cubeNo] = maxVel;
                                        std::cout << "Phase 2, maxVel ->";
                                }
                                else if ((simulatedTime > t1+t2) && (simulatedTime < 2*t1+t2))
                                {
                                        //deltaPhi = maxVel*simulatedTime - 0.5*maxAccel*(deltaT - (t1+t2))*(simulatedTime - (t1+t2));
                                        deltaPhi = maxVel*deltaT - 0.5*maxAccel*((n*deltaT-(t1+t2))*(n*deltaT-(t1+t2))-((n-1)*deltaT-(t1+t2))*((n-1)*deltaT-(t1+t2)));
                                        currVels[cubeNo] = maxVel - maxAccel * (simulatedTime -(t1+t2));
                                        std::cout << "Phase 3, maxVel ->";
                                }
                                else
                                {
                                        deltaPhi = 0.0;
                                        currVels[cubeNo] = 0.0;
                                        std::cout << "Phase 4, maxVel ->";
                                }
                        }
                        //no
                        else
                        {
                                if (simulatedTime < t)
                                {
                                        //deltaPhi = 0.5*maxAccel*simulatedTime*simulatedTime;
                                        deltaPhi = 0.5*maxAccel*(n*deltaT*n*deltaT - (n-1)*deltaT*(n-1)*deltaT);
                                        currVels[cubeNo] = maxAccel * simulatedTime;
                                        std::cout  << "Phase 1 ->";
                                }
                                else if ((simulatedTime > t) && (simulatedTime <= 2*t))
                                {
                                        //deltaPhi = maxVel *simulatedTime - 0.5*maxAccel*(simulatedTime -t)*(simulatedTime-t); 
                                        deltaPhi = maxAccel*t*deltaT - 0.5*maxAccel*((n*deltaT-t)*(n*deltaT-t)-((n-1)*deltaT-t)*((n-1)*deltaT-t));
                                        currVels[cubeNo] = maxAccel*t - maxAccel * (simulatedTime -t);
                                        std::cout  << "Phase 2 ->";
                                }
                                else
                                {
                                        deltaPhi = 0.0;
                                        currVels[cubeNo] = 0.0;
                                        std::cout  << "Phase 3 ->" << t << "\n";
                                }

                        }
                        cubeSimPtr->getConfig(currAngles);

                        currAngles[cubeNo] = currAngles[cubeNo]+deltaPhi;
                        currDeltaAngle = targetAngle - currAngles[cubeNo];
                        //std::cout << "cube "<<cubeNo<<"["<<simulatedTime<<"]: deltaPhi: "<<deltaPhi<<"; deltaAngle:"<<currDeltaAngle<<endl;

                        //write new angle and velocity
                        cubeSimPtr->setCurrentAngles(currAngles);
                        cubeSimPtr->setCurrentJointVelocities(currVels);
                }
        }

        //we have finished our move
        cubeSimPtr->setStatusMoving(cubeNo,false);
        currVels[cubeNo] = 0.0;
        cubeSimPtr->setCurrentJointVelocities(currVels);

        //cubeSimPtr->getOutputStream() << "Thread finished for cube ID "<< (cubeSimPtr->getModuleMap())[cubeNo]<<endl;
        pthread_exit(NULL);
}


int PowerCubeSim::startSimulatedMovement(std::vector<double> target)
{

        if (statusMoving())
        {
                std::cout << "startSimulatedMovement: Movement already in progress, preemption not implemented yet! Aborting .."<<endl;
        }
        else
        {
                //create thread

                for (int i = 0; i < m_DOF; i++)
                {
                        setStatusMoving(i,true);
                        m_SimThreadArgs[i]->targetAngle = target[i];
                        pthread_create(&m_SimThreadID[i],NULL,SimThreadRoutine,(void*)m_SimThreadArgs[i]);
                }
        }
        return 0;

}
void PowerCubeSim::millisleep(unsigned int milliseconds) const
{
        timespec warten;
        // Millisekunden in Sekunden und Nanosekunden aufteilen
        warten.tv_sec = milliseconds / 1000;
        warten.tv_nsec = (milliseconds % 1000) * 1000000;
        timespec gewartet;
        nanosleep(&warten, &gewartet);
}


/* Not necessary
   void PowerCubeSim::HomingDone()
   {
   for(int i=0; i<m_NumOfModules; i++)
   {
   unsigned long int help;
   std::cout << "Warte auf Modul " << m_IdModules[i] << endl;
   do
   {
   PCube_getModuleState(m_Dev,m_IdModules[i],&help);
//printf("Status: 0x%lx\n",help);
millisleep(100);
} while ( (help & STATEID_MOD_HOME) == 0 );
}

}

*/

/*
   vector<int> PowerCubeSim::getModuleMap(int dev)
   {
   vector<int> mod_map;
   for( int i = 1; i < MAX_MODULES; i++ )
   {
   unsigned long serNo;
   int ret = PCube_getModuleSerialNo( dev, i, &serNo );
   if( ret == 0 )
   {
   std::cout << "Found module " << i << " with SerialNo " << serNo << "\n";
   mod_map.push_back(i);
   }
//              else
//              printf( "Module %d not found(%d)\n", i, ret);

}
return mod_map;
}
*/

/* TODO: necessary?
   void PowerCubeSim::waitForSync()
   {
   for (int i=0; i < m_DOF; i++)
   {
   unsigned long confword;
   millisleep(4);
   PCube_getConfig(m_Dev, m_IdModules[i], &confword );
   millisleep(4);
   PCube_setConfig(m_Dev, m_IdModules[i], confword | CONFIGID_MOD_SYNC_MOTION);
   }
   }

*/

/* TODO: necessary?
   void PowerCubeSim::dontWaitForSync()
   {
   for (int i=0; i < m_DOF; i++)
   {
   unsigned long confword;
   millisleep(4);
   PCube_getConfig(m_Dev, m_IdModules[i], &confword );
   millisleep(4);
   PCube_setConfig(m_Dev, m_IdModules[i], confword & (~CONFIGID_MOD_SYNC_MOTION));
   }
   }
   */

/*
   int PowerCubeSim::getStatus()
   {

   PC_CTRL_STATE error = PC_CTRL_OK;

   for(int i=0; i<m_NumOfModules; i++)
   {
   unsigned long int state;
   PCube_getModuleState(m_Dev,m_IdModules[i],&state);

   if (state & STATEID_MOD_POW_VOLT_ERR)
   {
   printf("Error in Module %d: Motor voltage below minimum value!\n",m_IdModules[i]);
   error = PC_CTRL_POW_VOLT_ERR;
   }
   else if (!(state & STATEID_MOD_HOME))
   {
   printf("Warning: Module %d is not referenced!\n",m_IdModules[i]);
   error = PC_CTRL_NOT_REFERENCED;
   }
   else if (state & STATEID_MOD_ERROR)
   {
   printf("Error in  Module %d: 0x%lx\n",m_IdModules[i],state);
   error = PC_CTRL_ERR;
   }
   }
        return error;

}*/