src/FastResearchInterfaceLibrary/StartRobot.cpp

Go to the documentation of this file.

//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
//   For a convenient reading of this file's source code,
//   please use a tab width of four characters.
//  ----------------------------------------------------------


#include <FastResearchInterface.h>
#include <Console.h>
#include <friComm.h>
#include <OSAbstraction.h>
#include <errno.h>
#include <string.h>



// ****************************************************************
// StartRobot()
//
int FastResearchInterface::StartRobot(      const unsigned int &ControlMode
                                        ,   const float &TimeOutValueInSeconds)
{
    int                 ResultValue             =   0;

    float               CommandValues[2 * LBR_MNJ]
                    ,   InitialSystemTimeValue  =   GetSystemTimeInSeconds(true);

    memset(CommandValues, 0x0, 2 * LBR_MNJ * sizeof(float));

    if (this->GetFRIMode() == FRI_STATE_OFF)
    {
        // wait until the FRI is in monitor mode
        while (     ((GetSystemTimeInSeconds() - InitialSystemTimeValue)    <   TimeOutValueInSeconds   )
                &&  (this->GetFRIMode()                                     !=  FRI_STATE_MON           ))
        {
            delay(1);
        }

        if (this->GetFRIMode() == FRI_STATE_OFF)
        {
            this->OutputConsole->printf("FastResearchInterface::StartRobot(): ERROR, the KRC has not opened the FRI yet or no UDP connection can be established.\n");
            return(ENOTCONN);
        }
    }

    if (this->GetFRIMode() == FRI_STATE_CMD)
    {
        if (this->CurrentControlScheme  == ControlMode)
        {
            this->OutputConsole->printf("FastResearchInterface::StartRobot(): The robot has already been started.\n");
            return(EALREADY);
        }
        else
        {
            this->StopRobot();
        }
    }

    if (this->GetKRLIntValue(15) == 20)
    {
        this->StopRobot();
    }


    this->CurrentControlScheme  =   ControlMode;

    this->SetControlScheme(this->CurrentControlScheme);

    // The KRL program running on the robot controller waits for a
    // integer value change at position 15 (i.e., 16 in KRL).
    // A value of 10 lets the KRL program call friStart()
    this->SetKRLIntValue(15, 10);

    // wait for the next data telegram of the KRC unit
    pthread_mutex_lock(&(this->MutexForControlData));
    this->NewDataFromKRCReceived    =   false;
    pthread_mutex_unlock(&(this->MutexForControlData));
    ResultValue =   this->WaitForKRCTick(((unsigned int)(this->CycleTime * 3000000.0)));

    while (     ((GetSystemTimeInSeconds() - InitialSystemTimeValue)    < TimeOutValueInSeconds )
            &&  (this->GetFRIMode()                                     != FRI_STATE_CMD        )   )
    {
        ResultValue =   this->WaitForKRCTick((unsigned int)(this->CycleTime * 3000000.0));

        if (ResultValue != EOK)
        {
            this->OutputConsole->printf("FastResearchInterface::StartRobot(): ERROR, the KRC unit does not respond anymore. Probably, the FRI was closed or there is no UDP connection anymore.");
            return(ENOTCONN);
        }
    }

    if (this->GetFRIMode() == FRI_STATE_CMD)
    {
        while (     ((GetSystemTimeInSeconds() - InitialSystemTimeValue) < TimeOutValueInSeconds)
                &&  (!(this->IsMachineOK()))
                &&  (this->GetKRLIntValue(15) != 20)    )
        {
            ResultValue =   this->WaitForKRCTick(((unsigned int)(this->CycleTime * 3000000.0)));

            if (ResultValue != EOK)
            {
                this->OutputConsole->printf("FastResearchInterface::StartRobot(): ERROR, the KRC unit does not respond anymore. Probably, the FRI was closed or there is no UDP connection anymore.");
                return(ENOTCONN);
            }
        }

        if (this->IsMachineOK())
        {
            delay(12);  // KUKA interpolation cycle time
            return(EOK);
        }
        else
        {
            this->OutputConsole->printf("FastResearchInterface::StartRobot(): ERROR, command mode reached, but the robot could be turned on correctly. Check KRC state.");
            return(ETIME);
        }
    }
    else
    {
        this->OutputConsole->printf("FastResearchInterface::StartRobot(): ERROR, could not switch to command mode after timeout.\n");
        return(ETIME);
    }
}