///get a ethercat message form the buffer
///@param msgBuffer ethercat message
///@param jointNumber joint number of the receiver joint
YouBotSlaveMsg EthercatMaster::getMsgBuffer(const unsigned int jointNumber) {
  // Bouml preserved body begin 00041571

    static bool lastValueof_newDataFlagOne;
    static bool lastValueof_newDataFlagTwo;
    YouBotSlaveMsg returnMsg;

    
    if (this->automaticReceiveOn == true) {
      if (newDataFlagOne == true && lastValueof_newDataFlagOne == false) {
        {
          boost::mutex::scoped_lock dataMutex1(mutexDataOne);
          BufferForGetMsgBuffer = firstBufferVector;
        }
      } else if (newDataFlagTwo == true && lastValueof_newDataFlagTwo == false) {
        {
          boost::mutex::scoped_lock dataMutex2(mutexDataTwo);
          BufferForGetMsgBuffer = secondBufferVector;
        }
      } 
      
      returnMsg = BufferForGetMsgBuffer[jointNumber - 1];
      lastValueof_newDataFlagOne = newDataFlagOne;
      lastValueof_newDataFlagTwo = newDataFlagTwo;
    } else {
      returnMsg = this->automaticReceiveOffBufferVector[jointNumber - 1];
    }

    return returnMsg;
  // Bouml preserved body end 00041571
}
Beispiel #2
0
bool SickS300::getData(LaserScannerData& data, Errors& error) {
    // Bouml preserved body begin 000211E7
    if (!this->open(error)) {
        return false;
    }
    try {

        if (newDataFlagOne == true) {
            {
                boost::mutex::scoped_lock dataMutex1(mutexData1);
                data.setMeasurements(distanceBufferOne, angleBufferOne, si::meter, radian); //TODO dictance in centimeter
            }
            newDataFlagOne = false;

        } else if (newDataFlagTwo == true) {
            {
                boost::mutex::scoped_lock dataMutex2(mutexData2);
                data.setMeasurements(distanceBufferTwo, angleBufferTwo, meter, radian); //TODO dictance in centimeter
            }
            newDataFlagTwo = false;
        } else {
            //   error.addError("unable_to_get_data", "could not get data from the Sick S300");
            return false;
        }

        //  LOG(trace) << "receiving range scan from Sick S300";
    } catch (...) {
        error.addError("unable_to_get_data", "could not get data from the Sick S300");
        return false;
    }

    return true;
    // Bouml preserved body end 000211E7
}
///stores a ethercat message to the buffer
///@param msgBuffer ethercat message
///@param jointNumber joint number of the sender joint
void EthercatMaster::setMsgBuffer(const YouBotSlaveMsg& msgBuffer, const unsigned int jointNumber) {
  // Bouml preserved body begin 000414F1

    if (this->automaticSendOn == true) {
      if (newDataFlagOne == true) {
        {
          boost::mutex::scoped_lock dataMutex1(mutexDataOne);
          firstBufferVector[jointNumber - 1].stctOutput = msgBuffer.stctOutput;
          newOutputDataFlagOne[jointNumber - 1] = true;
          newOutputDataFlagTwo[jointNumber - 1] = false;
        }
      } else if (newDataFlagTwo == true) {
        {
          boost::mutex::scoped_lock dataMutex2(mutexDataTwo);
          secondBufferVector[jointNumber - 1].stctOutput = msgBuffer.stctOutput;
          newOutputDataFlagOne[jointNumber - 1] = false;
          newOutputDataFlagTwo[jointNumber - 1] = true;
        }

      } else {
        return;
      }
    } else {
      YouBotSlaveMsg localMsg;
      localMsg.stctOutput = msgBuffer.stctOutput;
      localMsg.jointNumber = jointNumber;
      automaticSendOffBufferVector.push_back(localMsg);
    }

  // Bouml preserved body end 000414F1
}
///stores a mailbox message in a buffer which will be sent to the motor controllers
///@param msgBuffer ethercat mailbox message
///@param jointNumber joint number of the sender joint
void EthercatMaster::setMailboxMsgBuffer(const YouBotSlaveMailboxMsg& msgBuffer, const unsigned int jointNumber) {
  // Bouml preserved body begin 00049D71

    if (newDataFlagOne == true) {
      {
        boost::mutex::scoped_lock dataMutex1(mutexDataOne);
        firstMailboxBufferVector[jointNumber - 1].stctOutput = msgBuffer.stctOutput;
        newMailboxDataFlagOne[jointNumber - 1] = true;
        newMailboxDataFlagTwo[jointNumber - 1] = false;
      }
    } else if (newDataFlagTwo == true) {
      {
        boost::mutex::scoped_lock dataMutex2(mutexDataTwo);
        secondMailboxBufferVector[jointNumber - 1].stctOutput = msgBuffer.stctOutput;
        newMailboxDataFlagOne[jointNumber - 1] = false;
        newMailboxDataFlagTwo[jointNumber - 1] = true;
      }

    }
    return;
  // Bouml preserved body end 00049D71
}
void EthercatMaster::AutomaticReceiveOn(const bool enableAutomaticReceive) {
  // Bouml preserved body begin 0008FB71
    this->automaticReceiveOn = enableAutomaticReceive;


    if (this->automaticReceiveOn == false) {
      if (newDataFlagOne == true) {
        {
          boost::mutex::scoped_lock dataMutex1(mutexDataOne);
          this->automaticReceiveOffBufferVector = firstBufferVector;
        }
      } else if (newDataFlagTwo == true) {
        {
          boost::mutex::scoped_lock dataMutex2(mutexDataTwo);
          this->automaticReceiveOffBufferVector = firstBufferVector;
        }
      }
    }

    return;
  // Bouml preserved body end 0008FB71
}
Beispiel #6
0
void SickS300::receiveScan() {
    // Bouml preserved body begin 000371F1

    bool returnValue = false;

    {
        boost::mutex::scoped_lock lock_it(mutexSickS300);


        while (!stopThread) {

            if (newDataFlagOne == false) {
                {
                    boost::mutex::scoped_lock dataMutex1(mutexData1);
                    returnValue = sickS300->getScan(distanceBufferOne, angleBufferOne, intensityBufferOne);
                }
                if (returnValue) {
                    newDataFlagOne = true;
                    newDataFlagTwo = false;
                }

            } else if (newDataFlagTwo == false) {
                {
                    boost::mutex::scoped_lock dataMutex2(mutexData2);
                    returnValue = sickS300->getScan(distanceBufferTwo, angleBufferTwo, intensityBufferTwo);
                }
                if (returnValue) {
                    newDataFlagTwo = true;
                    newDataFlagOne = false;
                }
            }
            boost::this_thread::sleep(boost::posix_time::milliseconds(timeTillNextPollForData));
        }
    }
    // Bouml preserved body end 000371F1
}
///gets a mailbox message form the buffer which came form the motor controllers
///@param msgBuffer ethercat mailbox message
///@param jointNumber joint number of the receiver joint
bool EthercatMaster::getMailboxMsgBuffer(YouBotSlaveMailboxMsg& mailboxMsg, const unsigned int jointNumber) {
  // Bouml preserved body begin 00049DF1


    if (newMailboxInputDataFlagOne[jointNumber - 1] == true) {
      {
        boost::mutex::scoped_lock dataMutex1(mutexDataOne);
        mailboxMsg.stctInput = firstMailboxBufferVector[jointNumber - 1].stctInput;
     //   mailboxMsg.stctOutput = firstMailboxBufferVector[jointNumber - 1].stctOutput;
        newMailboxInputDataFlagOne[jointNumber - 1] = false;
      }
      return true;
    } else if (newMailboxInputDataFlagTwo[jointNumber - 1] == true) {
      {
        boost::mutex::scoped_lock dataMutex2(mutexDataTwo);
        mailboxMsg.stctInput = secondMailboxBufferVector[jointNumber - 1].stctInput;
    //    mailboxMsg.stctOutput = secondMailboxBufferVector[jointNumber - 1].stctOutput;
        newMailboxInputDataFlagTwo[jointNumber - 1] = false;
      }
      return true;
    }
    return false;
  // Bouml preserved body end 00049DF1
}
///sends and receives ethercat messages and mailbox messages to and from the motor controllers
///this method is executed in a separate thread
void EthercatMaster::updateSensorActorValues() {
  // Bouml preserved body begin 0003F771

    long timeToWait = 0;
    ptime startTime = microsec_clock::local_time();
    time_duration pastTime;
    int counter = 0;
    time_duration realperiode;
    time_duration timeSum = startTime - startTime;


    while (!stopThread) {

      pastTime = microsec_clock::local_time() - startTime;
      timeToWait = timeTillNextEthercatUpdate - pastTime.total_microseconds() - 100;
      
      if(timeToWait < 0 || timeToWait > timeTillNextEthercatUpdate){
    //    printf("Missed communication period of %d  microseconds it have been %d microseconds \n",timeTillNextEthercatUpdate, (int)pastTime.total_microseconds()+ 100);
      }else{
        boost::this_thread::sleep(boost::posix_time::microseconds(timeToWait));
      }

 //   realperiode = microsec_clock::local_time() - startTime;
      startTime = microsec_clock::local_time();

/*
      counter++;
      timeSum  = timeSum + realperiode;

      if(counter == 100){

        double dtotaltime = (double)timeSum.total_microseconds()/counter;
        printf("TotalTime %7.0lf us\n", dtotaltime);
        counter = 0;
        timeSum = startTime - startTime;
      }
*/

      //check if for joint limits
    //  this->checkJointLimits();  //TODO test joint limit check
      
      //send and receive data from ethercat
      if (ec_send_processdata() == 0) {
        LOG(warning) << "Sending process data failed";
      }

      if (ec_receive_processdata(this->ethercatTimeout) == 0) {
        if(communicationErrors == 0){
          LOG(warning) << "Receiving data failed";
        }
        communicationErrors++;
      }else{
        communicationErrors = 0;
      }
      
      if(communicationErrors > maxCommunicationErrors){
        LOG(error) << "Lost EtherCAT connection";
        this->closeEthercat();
        stopThread = true;
        break;
      }
      
      if (ec_iserror())
        LOG(warning) << "there is an error in the soem driver";
      

      if (newDataFlagOne == false) {
        {
          boost::mutex::scoped_lock dataMutex1(mutexDataOne);
          for (unsigned int i = 0; i < firstBufferVector.size(); i++) {
            
            //fill first output buffer (send data)
            if (newOutputDataFlagOne[i]) {
              *(ethercatOutputBufferVector[i]) = (firstBufferVector[i]).stctOutput;
            }
           
            //fill first input buffer (receive data)
            (firstBufferVector[i]).stctInput = *(ethercatInputBufferVector[i]);
            
              
           // this->parseYouBotErrorFlags(secondBufferVector[i]);

            //send mailbox messages from first buffer
            if (newMailboxDataFlagOne[i]) {
              sendMailboxMessage(firstMailboxBufferVector[i]);
              newMailboxDataFlagOne[i] = false;
              pendingMailboxMsgsReply[i] = true;
            }
            
            //receive mailbox messages to first buffer
            if(pendingMailboxMsgsReply[i]){
              if (receiveMailboxMessage(firstMailboxBufferVector[i])) {
                newMailboxInputDataFlagOne[i] = true;
                pendingMailboxMsgsReply[i] = false;
              }
            }
          }
        }
        newDataFlagOne = true;
        newDataFlagTwo = false;

      } else if (newDataFlagTwo == false) {
        {
          boost::mutex::scoped_lock dataMutex2(mutexDataTwo);
          for (unsigned int i = 0; i < secondBufferVector.size(); i++) {
            
            //fill second output buffer (send data)
            if (newOutputDataFlagTwo[i]) {
              *(ethercatOutputBufferVector[i]) = (secondBufferVector[i]).stctOutput;
            }
            //fill second input buffer (receive data)
            (secondBufferVector[i]).stctInput = *(ethercatInputBufferVector[i]);
            
           // this->parseYouBotErrorFlags(secondBufferVector[i]);

            //send mailbox messages from second buffer
            if (newMailboxDataFlagTwo[i]) {
              sendMailboxMessage(secondMailboxBufferVector[i]);
              newMailboxDataFlagTwo[i] = false;
              pendingMailboxMsgsReply[i] = true;
            }
             
            //receive mailbox messages to second buffer
            if(pendingMailboxMsgsReply[i]){
              if (receiveMailboxMessage(secondMailboxBufferVector[i])) {
                newMailboxInputDataFlagTwo[i] = true;
                pendingMailboxMsgsReply[i] = false;
              }
            }
          }
        }
        newDataFlagTwo = true;
        newDataFlagOne = false;
      }

      
     // if(ethercatInputBufferVector[3]->actualCurrent >= 900 ){
     //   printf("joint 3 encoder: %d current %d \n", ethercatInputBufferVector[3]->actualPosition, ethercatInputBufferVector[3]->actualCurrent);
     // }
      // int cnt = 7;
      //  printf("activeports:%i DCrtA:%i DCrtB:%d DCrtC:%d DCrtD:%d\n", (int)ec_slave[cnt].activeports, ec_slave[cnt].DCrtA, ec_slave[cnt].DCrtB, ec_slave[cnt].DCrtC, ec_slave[cnt].DCrtD);
      //  printf("next DC slave:%i previous DC slave:%i DC cyle time in ns:%d DC shift:%d DC sync activation:%d\n", ec_slave[cnt].DCnext, ec_slave[cnt].DCprevious, ec_slave[cnt].DCcycle, ec_slave[cnt].DCshift, ec_slave[cnt].DCactive);

//      for (unsigned int i = 0; i < motorProtections.size(); i++) {
//        if (motorProtections[i].createSafeMotorCommands(stopMotorCommand)) {
//          *(ethercatOutputBufferVector[i]) = stopMotorCommand.stctOutput;
//        }
//      }
    }
  // Bouml preserved body end 0003F771
}