AiboLib: LegsExample.cc

#include "LegsExample.h"

#include <OPENR/ODataFormats.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>


LegsExample::LegsExample() : State(SYSTEM_IDLE)
{
  OSYSDEBUG(("LegsExample()\n"));
  LegCounter = -1;
  BaseCounter = -1;
};

OStatus LegsExample::DoInit(const OSystemEvent& event)
{
  OSYSDEBUG(("LegsExample::DoInit() \n"));
  
  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;

  // FIRST open primitives
  Legs.OpenPrimitives();

  // create new command vectors
  Legs.NewCommandVectorData();

  // turn motor power on
  // Done in LedsExample
  // MotorPowerOn();

  return oSUCCESS;
};

OStatus LegsExample::DoStart(const OSystemEvent& event)
{
  OSYSDEBUG(("LegsExample::DoStart()\n"));
  
  // if legs ready init command vector 
  if (subject[sbjLEGSTEST]->IsReady()){
    OSYSDEBUG(("---Legs ready to set\n"));
    Legs.RefreshCommandVector(0);
    subject[sbjLEGSTEST]->SetData(Legs.Region[0]);
    subject[sbjLEGSTEST]->NotifyObservers();
    Legs.SetState(J_START);
  }

  // enable motion
  State = SYSTEM_START;
  
  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

  OSYSDEBUG(("---DoStart:END!! \n"));

  return oSUCCESS;
};

OStatus LegsExample::DoStop(const OSystemEvent& event)
{
  
  OSYSDEBUG(("LegsExample::DoStop()\n"));

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  return oSUCCESS;
};

OStatus LegsExample::DoDestroy(const OSystemEvent& event)
{
  OSYSDEBUG(("LegsExample::DoDestroy()\n"));
  
  DELETE_ALL_SUBJECT_AND_OBSERVER;
  return oSUCCESS;
};


void LegsExample::NotifyLegs(const OReadyEvent& event)
{
  //OSYSDEBUG(("LegsExample::NotifyLegs()\n"));

  //depening on actual state make different motions
  switch (State) {
 
  case MOVE_SLEEPING:
    MoveSleeping();
    
    if(LegCounter >= BASE_POS_MAX_COUNTER){
      State = SYSTEM_IDLE;
    }
    break;

  case SYSTEM_START:
    
    //if called for fisrt time set joint gain
    if(BaseCounter == -1){
      OSYSDEBUG(("---LEGS JOINT_STATE INIT\n"));
      Legs.SetJointGain();
      MoveToBase();
    }
    else{
      MoveToBase();
      
      // if move to base is finished go to MoveSleeping
      if(BaseCounter >= BASE_POS_MAX_COUNTER){
        LegCounter = -1;
        BaseCounter = -1;
        State = MOVE_SLEEPING;
      }
    }
       
    break;

  case SYSTEM_IDLE:
    OSYSDEBUG(("---LEGS IDLE\n"));
    break;

  default:
    OSYSDEBUG(("---LEGS IDLE default\n"));
    break;

  }
};


void LegsExample::MoveToBase()
{
  OSYSDEBUG(("LegsExample::MoveBase()\n"));
  
  //start angles
  double start[NUM_LEG_JOINTS];

  // RCRegion index
  int region = 0;
  
  // if called for first time update start values and calculate 
  // Delta angles 
  if (BaseCounter == -1) {

    OSYSDEBUG(("MoveBase() BaseCounter = -1\n"));
    OJointValue current;
    double divv = (double)BASE_POS_MAX_COUNTER;
    
    double baseRad[NUM_LEG_JOINTS];
    
    for(int i = 0 ; i < NUM_LEG_JOINTS; i++){
      baseRad[i] =  DEGTORAD*BROADBASE_ANGLES_LEGS[i];
      start[i] = Legs.GetOPENRJointValue(LEG_JOINT(i));
      Delta[i] = (baseRad[i] - start[i]) / divv;
    }

    BaseCounter = 0;
    
    // find free command vector
    region = Legs.FindFreeRegionIndex();

    // add Delta to each joint
    for(int i = 0 ; i < NUM_LEG_JOINTS; i++)
      Legs.SetJointRel(Delta[i], LEG_JOINT(i), region);

    // set command vector 
    subject[sbjLEGSTEST]->SetData(Legs.Region[region]);
    
    BaseCounter++;
  } 
  
  //find free command vector
  region = Legs.FindFreeRegionIndex();
  
   // add Delta to each joint
  for(int i = 0 ; i < NUM_LEG_JOINTS; i++)
     Legs.SetJointRel(Delta[i], LEG_JOINT(i), region);
    
  // set command vector 
  subject[sbjLEGSTEST]->SetData(Legs.Region[region]);
  
  //notify observer (in this case OVirtualRobotComm.Effector.OCommandVectorData.O)
  subject[sbjLEGSTEST]->NotifyObservers();
  
  //increase step counter
  BaseCounter++;
};


void LegsExample::MoveSleeping()
{
  //nearly the same as MoveToBase
  OSYSDEBUG(("LegsExample::MoveSleeping()\n"));
  
  int region = 0;
  
  if (LegCounter == -1) {

    OSYSDEBUG(("MoveSleeping() LegCounter = -1\n"));
    OJointValue current;
    double divv = (double)BASE_POS_MAX_COUNTER;
    
    for(int i = 0 ; i < NUM_LEG_JOINTS; i++)
      Delta[i] = ((DEGTORAD * SLEEPING_ANGLES_LEGS[i]) -  Legs.GetJointValue(LEG_JOINT(i))) / divv;
       
    LegCounter = 0;
  } 
  
  region = Legs.FindFreeRegionIndex();
  
  for(int i = 0 ; i < NUM_LEG_JOINTS; i++)
    Legs.SetJointRel(Delta[i], LEG_JOINT(i), region); 

  subject[sbjLEGSTEST]->SetData(Legs.Region[region]);
  subject[sbjLEGSTEST]->NotifyObservers();

  LegCounter++;  
};