Platform/Aperios1.3.2/MotorCommandsSender.cpp

Go to the documentation of this file.

/**
 * @file
 * This file contains the implementation of a sender for motor commands.
 */

#include "ProcessFramework.h"
#include "Tools/Debugging/Debugging.h"

MotorCommandsSender::MotorCommandsSender(PlatformProcess* process,bool blocking)
: SenderBase<MotorCommands>(process,"Sender.OCommandVectorData.S",blocking),
  jointGainsSet(false)
{
  package = 0;
  numOfLED = SystemCall::getRobotDesign() == RobotDesign::ERS210 ? LEDValue::numOfLED_ERS210 : LEDValue::numOfLED_ERS7;
  VERIFY(OPENR::NewCommandVectorData(JointData::numOfJoint + numOfLED,&memID,&cmdVec) == oSUCCESS);
  cmdVec->SetNumData(JointData::numOfJoint + numOfLED);

  OPENR::SetMotorPower(opowerON);

  for(int i = 0; i < JointData::numOfJoint; i++) 
  {
    OPENR::OpenPrimitive((const char*) getPrimitiveJointName(i),&jointId[i]);
    OPENR::EnableJointGain(jointId[i]);
    OCommandInfo* info = cmdVec->GetInfo(i);
    info->Set(odataJOINT_COMMAND2,jointId[i],jointDataBufferNumOfFrames);
  }

  OPrimitiveID id;
  
  if(SystemCall::getRobotDesign() == RobotDesign::ERS7)
  {
    for(int i = 0; i < numOfLED; i++) 
    {
    OPENR::OpenPrimitive((const char*) getPrimitiveLEDName(i),&id);
    OCommandInfo* info = cmdVec->GetInfo(JointData::numOfJoint + i);
    if (i < (int)LEDValue::face1)
    {
       info->Set(odataLED_COMMAND2,id,jointDataBufferNumOfFrames);
    }
    else
    {
       info->Set(odataLED_COMMAND3,id,jointDataBufferNumOfFrames);
    }
    }
  }
  else
  {
    for(int i = 0; i < numOfLED; i++)
      {
    OPENR::OpenPrimitive((const char*) getPrimitiveLEDName(i),&id);
    OCommandInfo* info = cmdVec->GetInfo(JointData::numOfJoint + i);
    info->Set(odataLED_COMMAND2,id,jointDataBufferNumOfFrames);
    }
  }
}

MotorCommandsSender::~MotorCommandsSender()
{
  VERIFY(OPENR::DeleteCommandVectorData(memID) == oSUCCESS);
}

void MotorCommandsSender::preparePackage()
{
  if(!package)
    package = new RCRegion(cmdVec->vectorInfo.memRegionID,
                           cmdVec->vectorInfo.offset,
                           (void*) cmdVec,
                           cmdVec->vectorInfo.totalSize);

  const MotorCommands& motorCommands = *static_cast<const MotorCommands*>(this);

  for (int i = 0; i < JointData::numOfJoint; i++)
    if(!jointGainsSet ||
       motorCommands.pidData.p[i] != lastPidData.p[i] ||
       motorCommands.pidData.i[i] != lastPidData.i[i] ||
       motorCommands.pidData.d[i] != lastPidData.d[i])
    {
      OPENR::SetJointGain(jointId[i], 
        motorCommands.pidData.p[i],
        motorCommands.pidData.i[i],
        motorCommands.pidData.d[i],
        0x0E, 0x02, 0x0F);
      lastPidData.p[i] = motorCommands.pidData.p[i];
      lastPidData.i[i] = motorCommands.pidData.i[i];
      lastPidData.d[i] = motorCommands.pidData.d[i];
    }
  jointGainsSet = true;

  long value;
  for(int k = 0; k < JointData::numOfJoint; ++k)
  {
    OCommandData* data = cmdVec->GetData(k);
    for(int l = 0; l < jointDataBufferNumOfFrames; ++l) 
    {
      value = motorCommands.jointDataBuffer.frame[l].data[k];
      if(value == jointDataInvalidValue) 
        value = 0;
      ((OJointCommandValue2 *)data->value)[l].value = value;
    }
  }

  if(SystemCall::getRobotDesign() == RobotDesign::ERS7)
  {
  for(int k = 0; k < numOfLED; ++k) 
    {
    OCommandData* data = cmdVec->GetData(JointData::numOfJoint + k);
    if ( k < (int)LEDValue::face1 )
    {
       for(int l = 0; l < jointDataBufferNumOfFrames; l++)
       {
       ((OLEDCommandValue2 *)data->value)[l].period = 1;
#ifndef LEDINTENSITY 
       ((OLEDCommandValue2 *)data->value)[l].led = (motorCommands.ledValue.data[l] >> k) & 1;
#else
       ((OLEDCommandValue3 *)data->value)[l].led = (motorCommands.ledValue.intensity[l][k] > 0);
#endif
       }
    }
    else
    {
       if (k <(int)LEDValue::backFrontBlue)
       {
       for(int l = 0; l < jointDataBufferNumOfFrames; l++)
         {
         ((OLEDCommandValue3 *)data->value)[l].period = 1;
#ifndef LEDINTENSITY 
         ((OLEDCommandValue3 *)data->value)[l].intensity = ((motorCommands.ledValue.data[l] >> k) & 1)*255;
#else
         ((OLEDCommandValue3 *)data->value)[l].intensity = motorCommands.ledValue.intensity[l][k];
#endif
         ((OLEDCommandValue3 *)data->value)[l].mode = oled3_MODE_B;
         }
       }
      else 
      {
         for(int l = 0; l < jointDataBufferNumOfFrames; l++)
         {
         ((OLEDCommandValue3 *)data->value)[l].period = 1;
#ifndef LEDINTENSITY 
         ((OLEDCommandValue3 *)data->value)[l].intensity = ((motorCommands.ledValue.data[l] >> k) & 1)*255;
#else
         ((OLEDCommandValue3 *)data->value)[l].intensity = motorCommands.ledValue.intensity[l][k];
#endif
         ((OLEDCommandValue3 *)data->value)[l].mode = oled3_MODE_UNDEF;
         }
      }
    }
    }
  }
  else
  {
  for(int k = 0; k < numOfLED; ++k) 
  {
     OCommandData* data = cmdVec->GetData(JointData::numOfJoint + k);
     for(int l = 0; l < jointDataBufferNumOfFrames; l++)
     {
     ((OLEDCommandValue2 *)data->value)[l].period = 1;
     ((OLEDCommandValue2 *)data->value)[l].led = (motorCommands.ledValue.data[l] >> k) & 1;
     }
  }
  }
}

void MotorCommandsSender::setPackage(const ObserverID& receiver)
{
  VERIFY(SetData(receiver,package) == oSUCCESS);
}

void MotorCommandsSender::freePackage()
{
  // package is not freed
}

---