AiboLib: HeadExample.cc

#include "HeadExample.h"

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


HeadExample::HeadExample() : State(SYSTEM_IDLE)
{
  OSYSDEBUG(("HeadExample()\n"));
  counter = 0;
};

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

  Head.OpenPrimitives();

  Head.NewCommandVectorData();
  
  // done in LedsExample
  // MotorPowerOn();
  
  
  return oSUCCESS;
};

OStatus HeadExample::DoStart(const OSystemEvent& event)
{
  OSYSDEBUG(("HeadExample::DoStart()Neu\n"));
  

  if (subject[sbjHEADTEST]->IsReady()){
    OSYSDEBUG(("---Joints ready to set\n"));
    Head.RefreshCommandVector(0);
    subject[sbjHEADTEST]->SetData(Head.Region[0]);
    subject[sbjHEADTEST]->NotifyObservers();

    Head.SetState(J_START);
  }

  State = SYSTEM_START;
  
  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

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

  return oSUCCESS;
};

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

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  return oSUCCESS;
};

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


void HeadExample::NotifyHead(const OReadyEvent& event)
{
  //OSYSDEBUG(("HeadExample::NotifyJoints()\n"));

  switch (State) {
  case SYSTEM_START:
    
    if(counter == 0){
      //OSYSDEBUG(("---HEAD_STATE INIT\n"));
      
      Head.SetJointGain();
      
      counter++;
      MoveToZeroPos();
    }
    else{
      
      MoveToZeroPos();
      
      counter++;
      
      if(counter >= ZERO_POS_MAX_COUNTER)
        State = SYSTEM_IDLE;
    }
    
    break;
    
  case (SYSTEM_IDLE):
    OSYSDEBUG(("---IDLE\n"));
    break;
    
  }
};


void HeadExample::MoveToZeroPos()
{
  OSYSDEBUG(("HeadExample::MoveToZeroPos()\n"));

    static double start, d_tilt;
    static double d_pan;
    static double d_roll;
    double dummy = 0.0; 
    static bool initialized = false;

    if (!initialized) {

        // multiplication with radtodeg and degtorad is only done to 
        // emphasize types and formats!
        start = RADTODEG *  Head.GetJointValue(HT);
        d_tilt = DEGTORAD * ((0.0 - start) / (double)ZERO_POS_MAX_COUNTER);

        start = Head.GetJointValue(HP);
        d_pan = ((0.0 - start) / (double)ZERO_POS_MAX_COUNTER);

        start = Head.GetJointValue(HR);
        d_roll = ((0.0 - start) / (double)ZERO_POS_MAX_COUNTER);

        initialized = true;
    } 

    int rgn = Head.FindFreeRegionIndex();
    
    Head.SetJointRel(d_tilt, HT, rgn);
    Head.SetJointRel(d_pan,  HP, rgn);
    Head.SetJointRel(d_roll, HR, rgn);
    subject[sbjHEADTEST]->SetData(Head.Region[rgn]);
    subject[sbjHEADTEST]->NotifyObservers();
};