home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 2003 March / VPR0303A.ISO / AIBO / MoNet / MotionAgents / MTNAgent.cc < prev    next >
C/C++ Source or Header  |  2002-12-19  |  4KB  |  160 lines
  1. //
  2. // Copyright 2002 Sony Corporation 
  3. //
  4. // Permission to use, copy, modify, and redistribute this software for
  5. // non-commercial use is hereby granted.
  6. //
  7. // This software is provided "as is" without warranty of any kind,
  8. // either expressed or implied, including but not limited to the
  9. // implied warranties of fitness for a particular purpose.
  10. //
  11.  
  12. #include <OPENR/OPENRAPI.h>
  13. #include "MTNAgent.h"
  14.  
  15. MTNAgent::MTNAgent() : motionDataID(odesigndataID_UNDEF), motionODA(), mtn()
  16. {
  17. }
  18.  
  19. void
  20. MTNAgent::Init()
  21. {
  22.     LoadMotionODA();
  23. }
  24.  
  25. bool
  26. MTNAgent::AreYou(MoNetAgentID agent)
  27. {
  28.     OSYSDEBUG(("MTNAgent::AreYou(%d)\n", agent));
  29.     return (agent == monetagentMTN) ? true : false;
  30. }
  31.  
  32. void
  33. MTNAgent::NotifyCommand(const MoNetAgentCommand& command,
  34.                         MoNetAgentResult* result)
  35. {
  36.     OSYSDEBUG(("MTNAgent::NotifyCommand()\n"));
  37.     OSYSDEBUG(("  agent        : %d\n",   command.agent));
  38.     OSYSDEBUG(("  index        : %d\n",   command.index));
  39.     OSYSDEBUG(("  syncKey      : %08x\n", command.syncKey));
  40.     OSYSDEBUG(("  startPosture : %d\n",   command.startPosture));
  41.     OSYSDEBUG(("  endPosture   : %d\n",   command.endPosture));
  42.  
  43.     if (command.index == monetagentMTN_NULL_MOTION) {
  44.         result->agent      = command.agent;
  45.         result->index      = command.index;
  46.         result->status     = monetCOMPLETION;
  47.         result->endPosture = command.endPosture;
  48.         return;
  49.     }
  50.  
  51.     MTNFile* mtnfile = (MTNFile*)motionODA.GetData(command.index);
  52.     if (mtnfile == 0) {
  53.         result->agent      = command.agent;
  54.         result->index      = command.index;
  55.         result->status     = monetINVALID_ARG;
  56.         result->endPosture = command.startPosture;
  57.         return;
  58.     }
  59.     
  60.     mtn.Set(mtnfile);
  61.     MoNetStatus status = Move(command.syncKey);
  62.  
  63.     result->agent      = command.agent;
  64.     result->index      = command.index;
  65.     result->status     = status;
  66.     result->endPosture = monetpostureUNDEF;
  67. }
  68.  
  69. void
  70. MTNAgent::ReadyEffector(const MoNetAgentCommand& command,
  71.                         MoNetAgentResult* result)
  72. {
  73.     result->agent      = command.agent;
  74.     result->index      = command.index;
  75.     result->endPosture = monetpostureUNDEF;
  76.  
  77.     MoNetStatus status = Move(ovrsynckeyUNDEF);
  78.     if (status == monetCOMPLETION) {
  79.         result->endPosture = command.endPosture;
  80.     }
  81.  
  82.     result->status = status;
  83. }
  84.  
  85. void
  86. MTNAgent::LoadMotionODA()
  87. {
  88.     OSYSDEBUG(("LoadMotionODA()\n"));
  89.  
  90.     OStatus result;
  91.     byte*   addr;
  92.     size_t  size;
  93.  
  94.     result = OPENR::FindDesignData(MONET_MOTION_KEYWORD,
  95.                                    &motionDataID, &addr, &size);
  96.     if (result != oSUCCESS) {
  97.         OSYSLOG1((osyslogERROR, "%s : %s %d",
  98.                   "MTNAgent::LoadMotionODA()",
  99.                   "OPENR::FindDesignData() FAILED", result));
  100.         return;
  101.     }
  102.  
  103.     motionODA.Set(addr);
  104. }
  105.  
  106. MoNetStatus
  107. MTNAgent::Move(OVRSyncKey syncKey)
  108. {
  109.     RCRegion* rgn = moNetAgentManager->FindFreeCommandRegion();
  110.     OCommandVectorData* cmdVecData = (OCommandVectorData*)rgn->Base();
  111.     cmdVecData->vectorInfo.syncKey = syncKey;
  112.  
  113.     MoNetStatus st = SetPrimitiveID(cmdVecData, mtn.GetRobotDesign());
  114.     if (st != monetSUCCESS) {
  115.         OSYSLOG1((osyslogERROR, "%s : %s %s",
  116.                   "MTNAgent::Move()",
  117.                   "SetPrimitiveID() FAILED", mtn.GetName()));
  118.         return monetINVALID_ARG;
  119.     }
  120.  
  121.     int nframes = mtn.InterpolateCommandVectorData(cmdVecData, NUM_FRAMES);
  122.     if (nframes == -1) {
  123.         OSYSLOG1((osyslogERROR, "%s : %s %s",
  124.                   "MTNAgent::Move()",
  125.                   "mtn.InterpolateCommandVectorData() FAILED", mtn.GetName()));
  126.         return monetINVALID_ARG;
  127.     }
  128.  
  129.     mtn.Next(nframes);
  130.  
  131.     moNetAgentManager->Effector()->SetData(rgn);
  132.     moNetAgentManager->Effector()->NotifyObservers();
  133.  
  134.     return (mtn.More() == true) ? monetSUCCESS : monetCOMPLETION;
  135. }
  136.  
  137. MoNetStatus
  138. MTNAgent::SetPrimitiveID(OCommandVectorData* cmdVec, char* robotDesign)
  139. {
  140.     if (strcmp(robotDesign, "DRX-900") == 0) {
  141.  
  142.         if (cmdVec->vectorInfo.maxNumData < DRX900_NUM_JOINTS) {
  143.             return monetINVALID_ARG;
  144.         }
  145.  
  146.         for (int i = 0; i < DRX900_NUM_JOINTS; i++) {
  147.             OCommandInfo* info = cmdVec->GetInfo(i);
  148.             OPrimitiveID id = moNetAgentManager->PrimitiveID(DRX900_INDEX[i]);
  149.             info->Set(odataJOINT_COMMAND2, id, 0);
  150.         }
  151.         
  152.         return monetSUCCESS;
  153.  
  154.     } else {
  155.  
  156.         return monetINVALID_ARG;
  157.  
  158.     }
  159. }
  160.