AiboLib: LedsExample.cc

#include "LedsExample.h"

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


LedsExample::LedsExample() : State(SYSTEM_IDLE)
{
  OSYSDEBUG(("LedsExample()\n"));
};

OStatus LedsExample::DoInit(const OSystemEvent& event)
{
  OSYSDEBUG(("LedsExample::DoInit() \n"));
  
  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;
  
 
  // open primitves
  Leds.OpenPrimitives();
  
  
  // create new command vectors
  // you have  to create one for each led pattern...
  // AiboLed::MaxLedRegions patterns are maximum allowed...but you can change 
  // patterns while programm is running
  Leds.NewCommandVectorData();
  Leds.NewCommandVectorData();
  
  // initialize the led values 
  InitLedVectors();

  // turn motorpower on
  if(!MotorPowerOn())
    OSYSPRINT(("LedsExample::DoInit(...) MotorPowerOn failed!\n"));
  

  return oSUCCESS;
};

OStatus LedsExample::DoStart(const OSystemEvent& event)
{

  // set each command vector once to OVirtualRoboComm 
  // (never do this for more than two vectors)
  subject[sbjLEDTEST]->SetData( Leds.GetRegion(0));
  subject[sbjLEDTEST]->SetData( Leds.GetRegion(1));

  // notify OVirtualRoboComm
  subject[sbjLEDTEST]->NotifyObservers();

  State = SYSTEM_START;
  
  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

  return oSUCCESS;
};

OStatus LedsExample::DoStop(const OSystemEvent& event)
{
  
  OSYSDEBUG(("LedsExample::DoStop()\n"));
  State = SYSTEM_IDLE;

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  return oSUCCESS;
};

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


void LedsExample::NotifyLeds(const OReadyEvent& event)
{
  OSYSDEBUG(("LedsExample::NotifyLeds()\n"));

  // variable to determine which pattern should be loaded
  static bool toggle = true;

  // if system is up and running
  if(State == SYSTEM_START){
     
    // if toggle is set, enable pattern 0 as next pattern
    if(toggle){
      // set pattern 0
      subject[sbjLEDTEST]->SetData(Leds.GetRegion(0));
      OSYSDEBUG(("---Next: Led's on \n"));
    }
    // else enable pattern 1
    else{
      subject[sbjLEDTEST]->SetData(Leds.GetRegion(1));
      OSYSDEBUG(("---Next: Led's off \n"));
    }
    subject[sbjLEDTEST]->NotifyObservers();
    toggle = (toggle) ? false : true;
  }

};

void LedsExample::InitLedVectors()
{
  OSYSDEBUG(("LedsExample::InitLedvectors()\n"));
  
  // for all leds
  for(int i= NUM_LEDS-1; i >= 0; i--){
    // set leds on
    if(i == LED(HL))
      // except the head light
      Leds.SetLed(LED(i), 375, false, 0);
    else
      //all leds on in command vector 0
      Leds.SetLed(LED(i), 375);

    // all leds off in cmdvector 1
    Leds.SetLed(LED(i), 375, false, 1); 
  }
};