///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
}
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
}
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
}
