EthercatMasterWithoutThread.cpp

Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2011
00004  * All rights reserved.
00005  *
00006  * Hochschule Bonn-Rhein-Sieg
00007  * University of Applied Sciences
00008  * Computer Science Department
00009  *
00010  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00011  *
00012  * Author:
00013  * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
00014  * Supervised by:
00015  * Gerhard K. Kraetzschmar
00016  *
00017  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00018  *
00019  * This sofware is published under a dual-license: GNU Lesser General Public 
00020  * License LGPL 2.1 and BSD license. The dual-license implies that users of this
00021  * code may choose which terms they prefer.
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
00034  *       contributors may be used to endorse or promote products derived from
00035  *       this software without specific prior written permission.
00036  *
00037  * This program is free software: you can redistribute it and/or modify
00038  * it under the terms of the GNU Lesser General Public License LGPL as
00039  * published by the Free Software Foundation, either version 2.1 of the
00040  * License, or (at your option) any later version or the BSD license.
00041  *
00042  * This program is distributed in the hope that it will be useful,
00043  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00044  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00045  * GNU Lesser General Public License LGPL and the BSD license for more details.
00046  *
00047  * You should have received a copy of the GNU Lesser General Public
00048  * License LGPL and BSD license along with this program.
00049  *
00050  ****************************************************************/
00051 extern "C" {
00052 #include "ethercattype.h"
00053 #include "nicdrv.h"
00054 #include "ethercatbase.h"
00055 #include "ethercatmain.h"
00056 #include "ethercatconfig.h"
00057 #include "ethercatcoe.h"
00058 #include "ethercatdc.h"
00059 #include "ethercatprint.h"
00060 }
00061 #include "youbot/EthercatMasterWithoutThread.hpp"
00062 
00063 namespace youbot {
00064 
00065 EthercatMasterWithoutThread::EthercatMasterWithoutThread(const std::string& configFile, const std::string& configFilePath) {
00066   // Bouml preserved body begin 000D1AF1
00067 
00068     this->ethercatConnectionEstablished = false;
00069     ethernetDevice = "eth0";
00070     mailboxTimeout = 4000; //micro sec
00071     ethercatTimeout = 500; //micro sec
00072     configfile = NULL;
00073     this->configFileName = configFile;
00074     this->configFilepath = configFilePath;
00075 
00076     //initialize to zero
00077     for (unsigned int i = 0; i < 4096; i++) {
00078       IOmap_[i] = 0;
00079     }
00080     //read ethercat parameters form config file
00081     configfile = new ConfigFile(this->configFileName, this->configFilepath);
00082 
00083     // configfile.setSection("EtherCAT");
00084     configfile->readInto(ethernetDevice, "EtherCAT", "EthernetDevice");
00085     configfile->readInto(ethercatTimeout, "EtherCAT", "EtherCATTimeout_[usec]");
00086     configfile->readInto(mailboxTimeout, "EtherCAT", "MailboxTimeout_[usec]");
00087 
00088     this->initializeEthercat();
00089 
00090   // Bouml preserved body end 000D1AF1
00091 }
00092 
00093 EthercatMasterWithoutThread::~EthercatMasterWithoutThread() {
00094   // Bouml preserved body begin 000D1BF1
00095     this->closeEthercat();
00096     if (configfile != NULL)
00097       delete configfile;
00098   // Bouml preserved body end 000D1BF1
00099 }
00100 
00101 bool EthercatMasterWithoutThread::isThreadActive() {
00102   // Bouml preserved body begin 000E6B71
00103   return false;
00104   // Bouml preserved body end 000E6B71
00105 }
00106 
00107 ///return the quantity of ethercat slave which have an input/output buffer
00108 unsigned int EthercatMasterWithoutThread::getNumberOfSlaves() const {
00109   // Bouml preserved body begin 000D1D71
00110     return this->nrOfSlaves;
00111   // Bouml preserved body end 000D1D71
00112 }
00113 
00114 void EthercatMasterWithoutThread::AutomaticSendOn(const bool enableAutomaticSend) {
00115   // Bouml preserved body begin 000D1DF1
00116     LOG(trace) << "automatic send is not possible if the EtherCAT master has no thread";
00117 
00118     return;
00119   // Bouml preserved body end 000D1DF1
00120 }
00121 
00122 void EthercatMasterWithoutThread::AutomaticReceiveOn(const bool enableAutomaticReceive) {
00123   // Bouml preserved body begin 000D1E71
00124     LOG(trace) << "automatic receive is not possible if the EtherCAT master has no thread";
00125     return;
00126   // Bouml preserved body end 000D1E71
00127 }
00128 
00129 ///provides all ethercat slave informations from the SOEM driver
00130 ///@param ethercatSlaveInfos ethercat slave informations
00131 void EthercatMasterWithoutThread::getEthercatDiagnosticInformation(std::vector<ec_slavet>& ethercatSlaveInfos) {
00132   // Bouml preserved body begin 000D1EF1
00133     ethercatSlaveInfos = this->ethercatSlaveInfo;
00134     for (unsigned int i = 0; i < ethercatSlaveInfos.size(); i++) {
00135       ethercatSlaveInfos[i].inputs = NULL;
00136       ethercatSlaveInfos[i].outputs = NULL;
00137     }
00138   // Bouml preserved body end 000D1EF1
00139 }
00140 
00141 ///sends ethercat messages to the motor controllers
00142 /// returns a true if everything it OK and returns false if something fail
00143 bool EthercatMasterWithoutThread::sendProcessData() {
00144   // Bouml preserved body begin 000D2471
00145 
00146     for (unsigned int i = 0; i < processDataBuffer.size(); i++) {
00147       //fill output buffer (send data)
00148       *(ethercatOutputBufferVector[i]) = (processDataBuffer[i]).stctOutput;
00149     }
00150 
00151     //send data to ethercat
00152     if (ec_send_processdata() == 0) {
00153       return false;
00154     }
00155     
00156     return true;
00157 
00158   // Bouml preserved body end 000D2471
00159 }
00160 
00161 /// receives ethercat messages from the motor controllers
00162 /// returns a true if everything it OK and returns false if something fail
00163 bool EthercatMasterWithoutThread::receiveProcessData() {
00164   // Bouml preserved body begin 000D5D71
00165 
00166     //receive data from ethercat
00167     if (ec_receive_processdata(this->ethercatTimeout) == 0) {
00168       return false;
00169     }
00170 
00171     for (unsigned int i = 0; i < processDataBuffer.size(); i++) {
00172       //fill input buffer (receive data)
00173       (processDataBuffer[i]).stctInput = *(ethercatInputBufferVector[i]);
00174     }
00175 
00176     return true;
00177 
00178   // Bouml preserved body end 000D5D71
00179 }
00180 
00181 /// checks if an error has occurred in the soem driver
00182 /// returns a true if an error has occurred
00183 bool EthercatMasterWithoutThread::isErrorInSoemDriver() {
00184   // Bouml preserved body begin 000D5DF1
00185    
00186     return ec_iserror();
00187 
00188   // Bouml preserved body end 000D5DF1
00189 }
00190 
00191 bool EthercatMasterWithoutThread::isEtherCATConnectionEstablished() {
00192   // Bouml preserved body begin 000F77F1
00193   return this->ethercatConnectionEstablished;
00194   // Bouml preserved body end 000F77F1
00195 }
00196 
00197 void EthercatMasterWithoutThread::registerJointLimitMonitor(JointLimitMonitor* object, const unsigned int JointNumber) {
00198   // Bouml preserved body begin 000FB0F1
00199 
00200   // Bouml preserved body end 000FB0F1
00201 }
00202 
00203 ///establishes the ethercat connection
00204 void EthercatMasterWithoutThread::initializeEthercat() {
00205   // Bouml preserved body begin 000D1F71
00206 
00207     /* initialise SOEM, bind socket to ifname */
00208     if (ec_init(ethernetDevice.c_str())) {
00209       LOG(info) << "Initializing EtherCAT on " << ethernetDevice << " without communication thread";
00210       /* find and auto-config slaves */
00211       if (ec_config(TRUE, &IOmap_) > 0) {
00212 
00213         LOG(trace) << ec_slavecount << " EtherCAT slaves found and configured.";
00214 
00215         /* wait for all slaves to reach Pre OP state */
00216         /*ec_statecheck(0, EC_STATE_PRE_OP,  EC_TIMEOUTSTATE);
00217         if (ec_slave[0].state != EC_STATE_PRE_OP ){
00218         LOG(debug) << "Not all slaves reached pre operational state.";
00219         ec_readstate();
00220         //If not all slaves operational find out which one
00221           for(int i = 1; i<=ec_slavecount ; i++){
00222             if(ec_slave[i].state != EC_STATE_PRE_OP){
00223               printf("Slave %d State=%2x StatusCode=%4x : %s\n",
00224               i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
00225             }
00226           }
00227         }
00228          */
00229 
00230         /* distributed clock is not working
00231         //Configure distributed clock
00232         if(!ec_configdc()){
00233           LOG(warning) << "no distributed clock is available";
00234         }else{
00235 
00236           uint32 CyclTime = 4000000;
00237           uint32 CyclShift = 0;
00238           for (int i = 1; i <= ec_slavecount; i++) {
00239             ec_dcsync0(i, true, CyclTime, CyclShift);
00240           }
00241 
00242         }
00243          */
00244 
00245         /* wait for all slaves to reach SAFE_OP state */
00246         ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
00247         if (ec_slave[0].state != EC_STATE_SAFE_OP) {
00248           LOG(warning) << "Not all EtherCAT slaves reached safe operational state.";
00249           ec_readstate();
00250           //If not all slaves operational find out which one
00251           for (int i = 1; i <= ec_slavecount; i++) {
00252             if (ec_slave[i].state != EC_STATE_SAFE_OP) {
00253               LOG(info) << "Slave " << i << " State=" << ec_slave[i].state << " StatusCode=" << ec_slave[i].ALstatuscode << " : " << ec_ALstatuscode2string(ec_slave[i].ALstatuscode);
00254 
00255             }
00256           }
00257         }
00258 
00259 
00260         //Read the state of all slaves
00261         //ec_readstate();
00262 
00263         LOG(trace) << "Request operational state for all EtherCAT slaves";
00264 
00265         ec_slave[0].state = EC_STATE_OPERATIONAL;
00266         // request OP state for all slaves
00267         /* send one valid process data to make outputs in slaves happy*/
00268         ec_send_processdata();
00269         ec_receive_processdata(EC_TIMEOUTRET);
00270         /* request OP state for all slaves */
00271         ec_writestate(0);
00272         // wait for all slaves to reach OP state
00273 
00274         ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
00275         if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
00276           LOG(trace) << "Operational state reached for all EtherCAT slaves.";
00277         } else {
00278           throw std::runtime_error("Not all EtherCAT slaves reached operational state.");
00279 
00280         }
00281 
00282       } else {
00283         throw std::runtime_error("No EtherCAT slaves found!");
00284       }
00285 
00286     } else {
00287       throw std::runtime_error("No socket connection on " + ethernetDevice + "\nExcecute as root");
00288     }
00289 
00290 
00291 
00292     std::string baseJointControllerName = "TMCM-174";
00293     std::string baseJointControllerNameAlternative = "TMCM-1632";
00294     std::string manipulatorJointControllerName = "TMCM-174";
00295     std::string ManipulatorJointControllerNameAlternative = "TMCM-1610";
00296     std::string actualSlaveName;
00297     nrOfSlaves = 0;
00298     YouBotSlaveMsg emptySlaveMsg;
00299 
00300 
00301     configfile->readInto(baseJointControllerName, "BaseJointControllerName");
00302     configfile->readInto(baseJointControllerNameAlternative, "BaseJointControllerNameAlternative");
00303     configfile->readInto(manipulatorJointControllerName, "ManipulatorJointControllerName");
00304     configfile->readInto(ManipulatorJointControllerNameAlternative, "ManipulatorJointControllerNameAlternative");
00305 
00306     //reserve memory for all slave with a input/output buffer
00307     for (int cnt = 1; cnt <= ec_slavecount; cnt++) {
00308            LOG(trace) << "Slave: " << cnt  << " Name: " << ec_slave[cnt].name  << " Output size: " << ec_slave[cnt].Obits
00309                    << "bits Input size: " << ec_slave[cnt].Ibits << "bits State: " << ec_slave[cnt].state  
00310                    << " delay: " << ec_slave[cnt].pdelay; //<< " has dclock: " << (bool)ec_slave[cnt].hasdc;
00311 
00312       ethercatSlaveInfo.push_back(ec_slave[cnt]);
00313 
00314       actualSlaveName = ec_slave[cnt].name;
00315       if ((actualSlaveName == baseJointControllerName || actualSlaveName == baseJointControllerNameAlternative || 
00316               actualSlaveName == manipulatorJointControllerName || actualSlaveName == ManipulatorJointControllerNameAlternative
00317               ) && ec_slave[cnt].Obits > 0 && ec_slave[cnt].Ibits > 0) {
00318         nrOfSlaves++;
00319         processDataBuffer.push_back(emptySlaveMsg);
00320         ethercatOutputBufferVector.push_back((SlaveMessageOutput*) (ec_slave[cnt].outputs));
00321         ethercatInputBufferVector.push_back((SlaveMessageInput*) (ec_slave[cnt].inputs));
00322         YouBotSlaveMailboxMsg emptyMailboxSlaveMsg(cnt);
00323         firstMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
00324       }
00325     }
00326 
00327 
00328 
00329     if (nrOfSlaves > 0) {
00330       LOG(info) << nrOfSlaves << " EtherCAT slaves found";
00331       this->ethercatConnectionEstablished = true;
00332     } else {
00333       throw std::runtime_error("No EtherCAT slave could be found");
00334       return;
00335     }
00336 
00337 
00338     return;
00339   // Bouml preserved body end 000D1F71
00340 }
00341 
00342 ///closes the ethercat connection
00343 bool EthercatMasterWithoutThread::closeEthercat() {
00344   // Bouml preserved body begin 000D2071
00345 
00346     this->ethercatConnectionEstablished = false;
00347     // Request safe operational state for all slaves
00348     ec_slave[0].state = EC_STATE_SAFE_OP;
00349 
00350     /* request SAFE_OP state for all slaves */
00351     ec_writestate(0);
00352 
00353     //stop SOEM, close socket
00354     ec_close();
00355 
00356     return true;
00357   // Bouml preserved body end 000D2071
00358 }
00359 
00360 ///stores a ethercat message to the buffer
00361 ///@param msgBuffer ethercat message
00362 ///@param jointNumber joint number of the sender joint
00363 void EthercatMasterWithoutThread::setMsgBuffer(const YouBotSlaveMsg& msgBuffer, const unsigned int jointNumber) {
00364   // Bouml preserved body begin 000D20F1
00365 
00366     processDataBuffer[jointNumber - 1].stctOutput = msgBuffer.stctOutput;
00367 
00368   // Bouml preserved body end 000D20F1
00369 }
00370 
00371 ///get a ethercat message form the buffer
00372 ///@param msgBuffer ethercat message
00373 ///@param jointNumber joint number of the receiver joint
00374 void EthercatMasterWithoutThread::getMsgBuffer(const unsigned int jointNumber, YouBotSlaveMsg& returnMsg) {
00375   // Bouml preserved body begin 000D2171
00376   
00377     returnMsg = processDataBuffer[jointNumber - 1];
00378     
00379   // Bouml preserved body end 000D2171
00380 }
00381 
00382 ///stores a mailbox message in a buffer which will be sent to the motor controllers
00383 ///@param msgBuffer ethercat mailbox message
00384 ///@param jointNumber joint number of the sender joint
00385 void EthercatMasterWithoutThread::setMailboxMsgBuffer(const YouBotSlaveMailboxMsg& msgBuffer, const unsigned int jointNumber) {
00386   // Bouml preserved body begin 000D21F1
00387   
00388     firstMailboxBufferVector[jointNumber - 1].stctOutput = msgBuffer.stctOutput;
00389     sendMailboxMessage(firstMailboxBufferVector[jointNumber - 1]);
00390     return;
00391   // Bouml preserved body end 000D21F1
00392 }
00393 
00394 ///gets a mailbox message form the buffer which came form the motor controllers
00395 ///@param msgBuffer ethercat mailbox message
00396 ///@param jointNumber joint number of the receiver joint
00397 bool EthercatMasterWithoutThread::getMailboxMsgBuffer(YouBotSlaveMailboxMsg& mailboxMsg, const unsigned int jointNumber) {
00398   // Bouml preserved body begin 000D2271
00399     bool returnvalue = receiveMailboxMessage(firstMailboxBufferVector[jointNumber - 1]);
00400     mailboxMsg.stctInput = firstMailboxBufferVector[jointNumber - 1].stctInput;
00401     return returnvalue;
00402   // Bouml preserved body end 000D2271
00403 }
00404 
00405 ///sends the mailbox Messages which have been stored in the buffer
00406 ///@param mailboxMsg ethercat mailbox message
00407 bool EthercatMasterWithoutThread::sendMailboxMessage(const YouBotSlaveMailboxMsg& mailboxMsg) {
00408   // Bouml preserved body begin 000D22F1
00409     mailboxBufferSend[0] = mailboxMsg.stctOutput.moduleAddress;
00410     mailboxBufferSend[1] = mailboxMsg.stctOutput.commandNumber;
00411     mailboxBufferSend[2] = mailboxMsg.stctOutput.typeNumber;
00412     mailboxBufferSend[3] = mailboxMsg.stctOutput.motorNumber;
00413     mailboxBufferSend[4] = mailboxMsg.stctOutput.value >> 24;
00414     mailboxBufferSend[5] = mailboxMsg.stctOutput.value >> 16;
00415     mailboxBufferSend[6] = mailboxMsg.stctOutput.value >> 8;
00416     mailboxBufferSend[7] = mailboxMsg.stctOutput.value & 0xff;
00417     if (ec_mbxsend(mailboxMsg.slaveNumber, &mailboxBufferSend, mailboxTimeout)) {
00418       return true;
00419     } else {
00420       return false;
00421     }
00422   // Bouml preserved body end 000D22F1
00423 }
00424 
00425 ///receives mailbox messages and stores them in the buffer
00426 ///@param mailboxMsg ethercat mailbox message
00427 bool EthercatMasterWithoutThread::receiveMailboxMessage(YouBotSlaveMailboxMsg& mailboxMsg) {
00428   // Bouml preserved body begin 000D2371
00429     if (ec_mbxreceive(mailboxMsg.slaveNumber, &mailboxBufferReceive, mailboxTimeout)) {
00430       mailboxMsg.stctInput.replyAddress = (int) mailboxBufferReceive[0];
00431       mailboxMsg.stctInput.moduleAddress = (int) mailboxBufferReceive[1];
00432       mailboxMsg.stctInput.status = (int) mailboxBufferReceive[2];
00433       mailboxMsg.stctInput.commandNumber = (int) mailboxBufferReceive[3];
00434       mailboxMsg.stctInput.value = (mailboxBufferReceive[4] << 24 | mailboxBufferReceive[5] << 16 | mailboxBufferReceive[6] << 8 | mailboxBufferReceive[7]);
00435       return true;
00436     }
00437     return false;
00438   // Bouml preserved body end 000D2371
00439 }
00440 
00441 void EthercatMasterWithoutThread::parseYouBotErrorFlags(const YouBotSlaveMsg& messageBuffer) {
00442   // Bouml preserved body begin 000D24F1
00443     std::stringstream errorMessageStream;
00444     errorMessageStream << " ";
00445     std::string errorMessage;
00446     errorMessage = errorMessageStream.str();
00447 
00448 
00449     if (messageBuffer.stctInput.errorFlags & OVER_CURRENT) {
00450       LOG(error) << errorMessage << "got over current";
00451       //    throw JointErrorException(errorMessage + "got over current");
00452     }
00453 
00454     if (messageBuffer.stctInput.errorFlags & UNDER_VOLTAGE) {
00455       LOG(error) << errorMessage << "got under voltage";
00456       //    throw JointErrorException(errorMessage + "got under voltage");
00457     }
00458 
00459     if (messageBuffer.stctInput.errorFlags & OVER_VOLTAGE) {
00460       LOG(error) << errorMessage << "got over voltage";
00461       //   throw JointErrorException(errorMessage + "got over voltage");
00462     }
00463 
00464     if (messageBuffer.stctInput.errorFlags & OVER_TEMPERATURE) {
00465       LOG(error) << errorMessage << "got over temperature";
00466       //   throw JointErrorException(errorMessage + "got over temperature");
00467     }
00468 
00469     if (messageBuffer.stctInput.errorFlags & MOTOR_HALTED) {
00470       //   LOG(info) << errorMessage << "is halted";
00471       //   throw JointErrorException(errorMessage + "is halted");
00472     }
00473 
00474     if (messageBuffer.stctInput.errorFlags & HALL_SENSOR_ERROR) {
00475       LOG(error) << errorMessage << "got hall sensor problem";
00476       //   throw JointErrorException(errorMessage + "got hall sensor problem");
00477     }
00478 
00479     //    if (messageBuffer.stctInput.errorFlags & ENCODER_ERROR) {
00480     //      LOG(error) << errorMessage << "got encoder problem";
00481     //      //   throw JointErrorException(errorMessage + "got encoder problem");
00482     //    }
00483     //
00484     //     if (messageBuffer.stctInput.errorFlags & INITIALIZATION_ERROR) {
00485     //      LOG(error) << errorMessage << "got inizialization problem";
00486     //      //   throw JointErrorException(errorMessage + "got motor winding problem");
00487     //    }
00488 
00489 //    if (messageBuffer.stctInput.errorFlags & PWM_MODE_ACTIVE) {
00490       //  LOG(error) << errorMessage << "has PWM mode active";
00491       //   throw JointErrorException(errorMessage + "the cycle time is violated");
00492 //    }
00493 
00494     if (messageBuffer.stctInput.errorFlags & VELOCITY_MODE) {
00495       //   LOG(info) << errorMessage << "has velocity mode active";
00496       //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00497     }
00498 
00499     if (messageBuffer.stctInput.errorFlags & POSITION_MODE) {
00500       //   LOG(info) << errorMessage << "has position mode active";
00501       //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00502     }
00503 
00504     if (messageBuffer.stctInput.errorFlags & TORQUE_MODE) {
00505       //   LOG(info) << errorMessage << "has torque mode active";
00506       //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00507     }
00508 
00509     //    if (messageBuffer.stctInput.errorFlags & EMERGENCY_STOP) {
00510     //      LOG(info) << errorMessage << "has emergency stop active";
00511     //      //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00512     //    }
00513     //
00514     //    if (messageBuffer.stctInput.errorFlags & FREERUNNING) {
00515     //   //   LOG(info) << errorMessage << "has freerunning active";
00516     //      //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00517     //    }
00518 
00519     if (messageBuffer.stctInput.errorFlags & POSITION_REACHED) {
00520       //    LOG(info) << errorMessage << "has position reached";
00521       //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00522     }
00523 
00524     if (messageBuffer.stctInput.errorFlags & INITIALIZED) {
00525       //  LOG(info) << errorMessage << "is initialized";
00526       //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00527     }
00528 
00529     if (messageBuffer.stctInput.errorFlags & TIMEOUT) {
00530       LOG(error) << errorMessage << "has a timeout";
00531       //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00532     }
00533 
00534     if (messageBuffer.stctInput.errorFlags & I2T_EXCEEDED) {
00535       LOG(error) << errorMessage << "exceeded I2t";
00536       //   throw JointErrorException(errorMessage + "need to initialize the sinus commutation");
00537     }
00538 
00539   // Bouml preserved body end 000D24F1
00540 }
00541 
00542 std::string EthercatMasterWithoutThread::configFileName;
00543 
00544 std::string EthercatMasterWithoutThread::configFilepath;
00545 
00546 
00547 } // namespace youbot
Generated by  doxygen 1.6.3