AiboLib: SensorsExample.cc

#include "SensorsExample.h"

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


SensorsExample::SensorsExample() : State(SYSTEM_IDLE)
{
};

OStatus SensorsExample::DoInit(const OSystemEvent& event)
{
  OSYSDEBUG(("SensorsExample::DoInit() \n"));
  
  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;
  
  // turn motor power on for distance sensor
  // done in LedsExample
  // MotorPowerOn();

  return oSUCCESS;
};

OStatus SensorsExample::DoStart(const OSystemEvent& event)
{
  OSYSDEBUG(("SensorsExample::DoStart()\n"));
  
  // set system state
  State = SYSTEM_START;
  
  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

  return oSUCCESS;
};

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

  State = SYSTEM_IDLE;

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  return oSUCCESS;
};

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

// every time the sensors are ready to send data and this object 
// has set ist state to recive data this method will be called
void SensorsExample::NotifySensors(const ONotifyEvent& event)
{
  OSYSDEBUG(("SensorsExample::NotifySensors()\n"));

  // get the sensor data out of the event
  OSensorFrameVectorData* sensorVec = (OSensorFrameVectorData*)event.Data(0);
  
  // if the sensor are not initialized
  if (!Sensors.Initialized()) {
    // init the sensors
    // this have to be done to assure that the local sensors 
    // correspnod to the sensors in the  sensorVec (mapping)
    Sensors.InitSensorIndex(sensorVec);
  }

  // print the number of recived data and the "index"  of the 
  // data
  OSYSPRINT(("\n ERS-220 numData %d frameNumber %d\n",
             sensorVec->vectorInfo.numData, sensorVec->info[0].frameNumber));
  
  // print some extra values
  // Sensors.PrintSensorValues(sensorVec);
  OSYSPRINT(("-------------------MYSENSORDATA----------------------\n"));
  myPrint(sensorVec);
  
  // wait for user input...
  // has the side effect that the aibo is not abled to shutdown correctly
  // WaitReturnKey();
  
  // send "i'm ready for new data"
  observer[event.ObsIndex()]->AssertReady();
};

void SensorsExample::myPrint(OSensorFrameVectorData* sensorVec)
{
  OSYSPRINT(("Distance: %f \n", Sensors.GetDistance(sensorVec)));
  
  OSYSPRINT(("Mean Distance: %f \n", Sensors.GetMeanDistance(sensorVec,4)));
  
  OSYSPRINT(("Temperature: %f \n", Sensors.GetTemp(sensorVec)));

  if( Sensors.IsPressed(sensorVec,S_TIN_SW))
    OSYSPRINT(("TIN_SW: pressed \n"));
  else
    OSYSPRINT(("TIN_SW: not pressed \n"));

 if( Sensors.IsPressed(sensorVec,S_BACK_SW))
    OSYSPRINT(("BACK_SW: pressed \n"));
  else
    OSYSPRINT(("BACK_SW: not pressed \n"));
  
  OSYSPRINT(("Head_tilt: %f \n\n", Sensors.GetJointValue(sensorVec,J_HEAD_TILT)));

};