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;
       ((OLEDCommandValue2 *)data->value)[l].led = (motorCommands.ledValue.data[l] >> k) & 1;
       }
    }
    else
    {
       if (k <(int)LEDValue::backFrontBlue)
       {
       for(int l = 0; l < jointDataBufferNumOfFrames; l++)
         {
         ((OLEDCommandValue3 *)data->value)[l].period = 1;
         ((OLEDCommandValue3 *)data->value)[l].intensity = ((motorCommands.ledValue.data[l] >> k) & 1)*255;
         ((OLEDCommandValue3 *)data->value)[l].mode = oled3_MODE_B;
         }
       }
      else 
      {
         for(int l = 0; l < jointDataBufferNumOfFrames; l++)
         {
         ((OLEDCommandValue3 *)data->value)[l].period = 1;
         ((OLEDCommandValue3 *)data->value)[l].intensity = ((motorCommands.ledValue.data[l] >> k) & 1)*255;
         ((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
}

/*
 * Change log :
 * 
 * $Log: MotorCommandsSender.cpp,v $
 * Revision 1.1.1.1  2004/05/22 17:23:26  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.5  2004/02/29 14:50:29  wachter
 * bugfix
 *
 * Revision 1.4  2004/02/27 23:58:39  wachter
 * ERS7 face and back LEDs working now.
 *
 * Revision 1.3  2003/12/31 14:29:19  roefer
 * Joints and LEDs for ERS-7
 *
 * Revision 1.2  2003/10/29 14:23:47  roefer
 * Start delay bug fixed
 *
 * Revision 1.1  2003/10/07 10:06:59  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.3  2003/07/03 22:51:11  dueffert
 * no spastic startup movements anymore: sending after calculating
 *
 * Revision 1.2  2003/07/03 15:57:57  dueffert
 * no spastic startup movements anymore
 *
 * Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.8  2003/05/16 17:02:17  roefer
 * JAM removed
 *
 * Revision 1.7  2003/03/10 18:21:15  dueffert
 * const cast warnings removed
 *
 * Revision 1.6  2002/12/02 11:00:13  dueffert
 * doxygen docu corrected
 *
 * Revision 1.5  2002/11/26 17:20:07  risler
 * removed PID initial low settings since wakeUp special action now performs smooth startup
 *
 * Revision 1.4  2002/11/25 15:58:37  risler
 * added return in preparePackage when initializing
 *
 * Revision 1.3  2002/11/25 15:17:34  dueffert
 * PID=1 at startup
 *
 * Revision 1.2  2002/11/19 17:08:48  risler
 * added datatype PIDData
 * support for sending new pid values at runtime
 *
 * Revision 1.1  2002/09/10 15:40:04  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed challenge related source code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.5  2002/08/30 17:10:57  dueffert
 * removed unused includes
 *
 * Revision 1.4  2002/08/14 17:11:00  dueffert
 * adapted by Thomas Roefer to OPENR_SDK-1.1.3-r1 and OPENR_SYS-007
 *
 * Revision 1.3  2002/07/23 13:39:39  loetzsch
 * - new streaming classes
 * - removed many #include statements
 * - new design of debugging architecture
 * - exchanged StaticQueue with MessageQueue
 *
 * Revision 1.2  2002/07/14 14:03:07  roefer
 * First working gcc-version
 *
 * Revision 1.1  2002/07/13 10:54:58  roefer
 * New command and sound sender
 *
 *
 *
 */

---