Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Platform/Aperios1.3.2/MotorCommandsSender.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file
    00003  * This file contains the implementation of a sender for motor commands.
    00004  */
    00005
    00006 #include "ProcessFramework.h"
    00007 #include "Tools/Debugging/Debugging.h"
    00008
    00009 MotorCommandsSender::MotorCommandsSender(PlatformProcess* process,bool blocking)
    00010 : SenderBase<MotorCommands>(process,"Sender.OCommandVectorData.S",blocking),
    00011   jointGainsSet(false)
    00012 {
    00013   package = 0;
    00014   numOfLED = SystemCall::getRobotDesign() == RobotDesign::ERS210 ? LEDValue::numOfLED_ERS210 : LEDValue::numOfLED_ERS7;
    00015   VERIFY(OPENR::NewCommandVectorData(JointData::numOfJoint + numOfLED,&memID,&cmdVec) == oSUCCESS);
    00016   cmdVec->SetNumData(JointData::numOfJoint + numOfLED);
    00017
    00018   OPENR::SetMotorPower(opowerON);
    00019
    00020   for(int i = 0; i < JointData::numOfJoint; i++)
    00021   {
    00022     OPENR::OpenPrimitive((const char*) getPrimitiveJointName(i),&jointId[i]);
    00023     OPENR::EnableJointGain(jointId[i]);
    00024     OCommandInfo* info = cmdVec->GetInfo(i);
    00025     info->Set(odataJOINT_COMMAND2,jointId[i],jointDataBufferNumOfFrames);
    00026   }
    00027
    00028   OPrimitiveID id;
    00029
    00030   if(SystemCall::getRobotDesign() == RobotDesign::ERS7)
    00031   {
    00032     for(int i = 0; i < numOfLED; i++)
    00033     {
    00034     OPENR::OpenPrimitive((const char*) getPrimitiveLEDName(i),&id);
    00035     OCommandInfo* info = cmdVec->GetInfo(JointData::numOfJoint + i);
    00036     if (i < (int)LEDValue::face1)
    00037     {
    00038        info->Set(odataLED_COMMAND2,id,jointDataBufferNumOfFrames);
    00039     }
    00040     else
    00041     {
    00042        info->Set(odataLED_COMMAND3,id,jointDataBufferNumOfFrames);
    00043     }
    00044     }
    00045   }
    00046   else
    00047   {
    00048     for(int i = 0; i < numOfLED; i++)
    00049       {
    00050     OPENR::OpenPrimitive((const char*) getPrimitiveLEDName(i),&id);
    00051     OCommandInfo* info = cmdVec->GetInfo(JointData::numOfJoint + i);
    00052     info->Set(odataLED_COMMAND2,id,jointDataBufferNumOfFrames);
    00053     }
    00054   }
    00055 }
    00056
    00057 MotorCommandsSender::~MotorCommandsSender()
    00058 {
    00059   VERIFY(OPENR::DeleteCommandVectorData(memID) == oSUCCESS);
    00060 }
    00061
    00062 void MotorCommandsSender::preparePackage()
    00063 {
    00064   if(!package)
    00065     package = new RCRegion(cmdVec->vectorInfo.memRegionID,
    00066                            cmdVec->vectorInfo.offset,
    00067                            (void*) cmdVec,
    00068                            cmdVec->vectorInfo.totalSize);
    00069
    00070   const MotorCommands& motorCommands = *static_cast<const MotorCommands*>(this);
    00071
    00072   for (int i = 0; i < JointData::numOfJoint; i++)
    00073     if(!jointGainsSet ||
    00074        motorCommands.pidData.p[i] != lastPidData.p[i] ||
    00075        motorCommands.pidData.i[i] != lastPidData.i[i] ||
    00076        motorCommands.pidData.d[i] != lastPidData.d[i])
    00077     {
    00078       OPENR::SetJointGain(jointId[i],
    00079         motorCommands.pidData.p[i],
    00080         motorCommands.pidData.i[i],
    00081         motorCommands.pidData.d[i],
    00082         0x0E, 0x02, 0x0F);
    00083       lastPidData.p[i] = motorCommands.pidData.p[i];
    00084       lastPidData.i[i] = motorCommands.pidData.i[i];
    00085       lastPidData.d[i] = motorCommands.pidData.d[i];
    00086     }
    00087   jointGainsSet = true;
    00088
    00089   long value;
    00090   for(int k = 0; k < JointData::numOfJoint; ++k)
    00091   {
    00092     OCommandData* data = cmdVec->GetData(k);
    00093     for(int l = 0; l < jointDataBufferNumOfFrames; ++l)
    00094     {
    00095       value = motorCommands.jointDataBuffer.frame[l].data[k];
    00096       if(value == jointDataInvalidValue)
    00097         value = 0;
    00098       ((OJointCommandValue2 *)data->value)[l].value = value;
    00099     }
    00100   }
    00101
    00102   if(SystemCall::getRobotDesign() == RobotDesign::ERS7)
    00103   {
    00104   for(int k = 0; k < numOfLED; ++k)
    00105     {
    00106     OCommandData* data = cmdVec->GetData(JointData::numOfJoint + k);
    00107     if ( k < (int)LEDValue::face1 )
    00108     {
    00109        for(int l = 0; l < jointDataBufferNumOfFrames; l++)
    00110        {
    00111        ((OLEDCommandValue2 *)data->value)[l].period = 1;
    00112 #ifndef LEDINTENSITY
    00113        ((OLEDCommandValue2 *)data->value)[l].led = (motorCommands.ledValue.data[l] >> k) & 1;
    00114 #else
    00115        ((OLEDCommandValue3 *)data->value)[l].led = (motorCommands.ledValue.intensity[l][k] > 0);
    00116 #endif
    00117        }
    00118     }
    00119     else
    00120     {
    00121        if (k <(int)LEDValue::backFrontBlue)
    00122        {
    00123        for(int l = 0; l < jointDataBufferNumOfFrames; l++)
    00124          {
    00125          ((OLEDCommandValue3 *)data->value)[l].period = 1;
    00126 #ifndef LEDINTENSITY
    00127          ((OLEDCommandValue3 *)data->value)[l].intensity = ((motorCommands.ledValue.data[l] >> k) & 1)*255;
    00128 #else
    00129          ((OLEDCommandValue3 *)data->value)[l].intensity = motorCommands.ledValue.intensity[l][k];
    00130 #endif
    00131          ((OLEDCommandValue3 *)data->value)[l].mode = oled3_MODE_B;
    00132          }
    00133        }
    00134       else
    00135       {
    00136          for(int l = 0; l < jointDataBufferNumOfFrames; l++)
    00137          {
    00138          ((OLEDCommandValue3 *)data->value)[l].period = 1;
    00139 #ifndef LEDINTENSITY
    00140          ((OLEDCommandValue3 *)data->value)[l].intensity = ((motorCommands.ledValue.data[l] >> k) & 1)*255;
    00141 #else
    00142          ((OLEDCommandValue3 *)data->value)[l].intensity = motorCommands.ledValue.intensity[l][k];
    00143 #endif
    00144          ((OLEDCommandValue3 *)data->value)[l].mode = oled3_MODE_UNDEF;
    00145          }
    00146       }
    00147     }
    00148     }
    00149   }
    00150   else
    00151   {
    00152   for(int k = 0; k < numOfLED; ++k)
    00153   {
    00154      OCommandData* data = cmdVec->GetData(JointData::numOfJoint + k);
    00155      for(int l = 0; l < jointDataBufferNumOfFrames; l++)
    00156      {
    00157      ((OLEDCommandValue2 *)data->value)[l].period = 1;
    00158      ((OLEDCommandValue2 *)data->value)[l].led = (motorCommands.ledValue.data[l] >> k) & 1;
    00159      }
    00160   }
    00161   }
    00162 }
    00163
    00164 void MotorCommandsSender::setPackage(const ObserverID& receiver)
    00165 {
    00166   VERIFY(SetData(receiver,package) == oSUCCESS);
    00167 }
    00168
    00169 void MotorCommandsSender::freePackage()
    00170 {
    00171   // package is not freed
    00172 }