segway_head.cpp

#include "segway_head.h"
#include "eventexceptions.h"
#include "exceptions.h"
#include <iostream>

CSegwayHead::CSegwayHead(std::string &device_id)
{
  this->servo_ids.bus_id=-1;
  this->servo_ids.pan=-1;
  this->servo_ids.tilt=-1;
  this->servo_ids.side=-1;
  this->servo_ids.lights=-1;
  this->pan=NULL;
  this->pan_motion_event_id="";
  this->tilt=NULL;
  this->tilt_motion_event_id="";
  this->side=NULL;
  this->side_motion_event_id="";
  this->lights=NULL; 
  this->dyn_server=CDynamixelServer::instance();
  if(this->dyn_server->get_num_buses()==0)
  {
    /* handle exceptions */
  }
  else
  {
    this->dyn_server->set_bus_id(0);
    this->dyn_server->set_baudrate(1000000);
  }
}

void CSegwayHead::set_pan_servo_id(unsigned char id)
{
  std::string pan_motor="pan_motor";

  if(id>0xFD)
  {
    /* handle exceptions */
  }
  else
  {
    if(id==this->servo_ids.tilt || id==this->servo_ids.side || id==this->servo_ids.lights)
    {
      /* handle errors */
    }
    else
    {
      try{
        if(this->pan!=NULL)
        {
          delete this->pan;
          this->pan=NULL;
        }
        this->pan=new CDynamixelMotor(pan_motor,this->servo_ids.bus_id,id);
        // configure the position feedback
        this->pan_motion_event_id=this->pan->config_position_feedback(fb_polling,100.0);
        this->servo_ids.pan=id;
      }catch(CException &e){
        /* handle exceptions */
        throw;
      }
    }
  }
}

void CSegwayHead::set_tilt_servo_id(unsigned char id)
{
  std::string tilt_motor="tilt_motor";

  if(id>0xFD)
  {
    /* handle exceptions */
  }
  else
  {
    if(id==this->servo_ids.pan || id==this->servo_ids.side || id==this->servo_ids.lights)
    {
      /* handle errors */
    }
    else
    {
      try{
        if(this->tilt!=NULL)
        {
          delete this->tilt;
          this->tilt=NULL;
        }
        this->tilt=new CDynamixelMotor(tilt_motor,this->servo_ids.bus_id,id);
        // configure the position feedback
        this->tilt_motion_event_id=this->tilt->config_position_feedback(fb_polling,100.0);
        this->servo_ids.tilt=id;
      }catch(CException &e){
        /* handle exceptions */
        throw;
      }
    }
  }
}

void CSegwayHead::set_side_servo_id(unsigned char id)
{
  std::string side_motor="side_motor";

  if(id>0xFD)
  {
    /* handle exceptions */
  }
  else
  {
    if(id==this->servo_ids.pan || id==this->servo_ids.tilt || id==this->servo_ids.lights)
    {
      /* handle errors */
    }
    else
    {
      try{
        if(this->side!=NULL)
        {
          delete this->side;
          this->side!=NULL;
        }
        this->side=new CDynamixelMotor(side_motor,this->servo_ids.bus_id,id);
        // configure the position feedback
        this->side_motion_event_id=this->side->config_position_feedback(fb_polling,100.0);
        this->servo_ids.side=id;
      }catch(CException &e){
        /* handle exceptions */
        throw;
      }
    }
  }
}

void CSegwayHead::set_light_id(unsigned char id)
{
  if(id>0xFD)
  {
    /* handle exceptions */
  }
  else
  {
    if(id==this->servo_ids.pan || id==this->servo_ids.tilt || id==this->servo_ids.side)
    {
      /* handle errors */
    }
    else
    {
      try{
        if(this->lights!=NULL)
        {
          delete this->lights;
          this->lights=NULL;
        }
        this->lights=this->dyn_server->get_device(id);
        this->servo_ids.lights=id;
      }catch(CException &e){
        /* handle exceptions */
        throw;
      }
    }
  }
}

void CSegwayHead::turn_on_all(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(0x1a,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::turn_off_all(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(0x1a,0x00);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::config(THeadServos *config)
{
  this->servo_ids.bus_id=config->bus_id;
  this->dyn_server->set_bus_id(config->bus_id);
  try{
    this->set_pan_servo_id(config->pan);
    this->set_tilt_servo_id(config->tilt);
    this->set_side_servo_id(config->side);
    this->set_light_id(config->lights);
    this->stand_by();
  }catch(CException &e){
    throw;
  }
}

unsigned char CSegwayHead::get_pan_servo_id(void)
{
  return this->servo_ids.pan;
}

unsigned char CSegwayHead::get_tilt_servo_id(void)
{
  return this->servo_ids.tilt;
}

unsigned char CSegwayHead::get_side_servo_id(void)
{
  return this->servo_ids.side;
}

unsigned char CSegwayHead::get_lights_id(void)
{
  return this->servo_ids.lights;
}

void CSegwayHead::move_pan(float degrees,float speed)
{
  CEventServer *event_server=CEventServer::instance();
  std::vector<float> position(1),velocity(1),accel(1);
  std::list<std::string> events;

  if(this->servo_ids.pan!=-1)
  {
    try{
      events.push_back(this->pan_motion_event_id);
      event_server->wait_all(events);
      position[0]=degrees;
      velocity[0]=speed;
      accel[0]=speed*speed/(0.1*degrees);
      this->pan->load(position,velocity,accel);
      this->pan->move();
    }catch(CException &e){
      std::cout << e.what() << std::endl;
    }
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::move_tilt(float degrees,float speed)
{
  CEventServer *event_server=CEventServer::instance();
  std::vector<float> position(1),velocity(1),accel(1);
  std::list<std::string> events;

  if(this->servo_ids.tilt!=-1)
  {
    try{
      events.push_back(this->tilt_motion_event_id);
      event_server->wait_all(events);
      position[0]=degrees;
      velocity[0]=speed;
      accel[0]=speed*speed/(0.1*degrees);
      this->tilt->load(position,velocity,accel);
      this->tilt->move();
    }catch(CException &e){
      std::cout << e.what() << std::endl;
    }
  }  
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::move_side(float degrees,float speed)
{
  CEventServer *event_server=CEventServer::instance();
  std::vector<float> position(1),velocity(1),accel(1);
  std::list<std::string> events;

  if(this->servo_ids.side!=-1)
  {
    try{
      events.push_back(this->side_motion_event_id);
      event_server->wait_all(events);
      position[0]=degrees;
      velocity[0]=speed;
      accel[0]=speed*speed/(0.1*degrees);
      this->side->load(position,velocity,accel);
      this->side->move();
    }catch(CException &e){
      std::cout << e.what() << std::endl;
    }
  }
  else
  {
    /* handle exceptions */
  }
}
  
void CSegwayHead::move_head(float pan, float tilt, float side,float speed)
{
  CEventServer *event_server=CEventServer::instance();
  std::vector<float> position(1),velocity(1),accel(1);
  std::list<std::string> events;

  if(this->servo_ids.pan!=-1 && this->servo_ids.tilt!=-1 && this->servo_ids.side!=-1)
  {
    events.push_back(this->pan_motion_event_id);
    events.push_back(this->tilt_motion_event_id);
    events.push_back(this->side_motion_event_id);
    event_server->wait_all(events);
    velocity[0]=speed;
    accel[0]=speed*speed/(0.1*pan);
    position[0]=pan;
    this->pan->load(position,velocity,accel);
    accel[0]=speed*speed/(0.1*tilt);
    position[0]=tilt;
    this->tilt->load(position,velocity,accel);
    accel[0]=speed*speed/(0.1*side);
    position[0]=side;
    this->side->load(position,velocity,accel);
    this->pan->move();
  }
  else
  {
    /* handle exceptions */
  }

}

float CSegwayHead::get_pan_position(void)
{
  if(this->servo_ids.pan!=-1)
  {
    return this->pan->get_position()[0];
  }
  else
  {
    /* handle exceptions */
  }
}

float CSegwayHead::get_tilt_position(void)
{
  if(this->servo_ids.tilt!=-1)
  {
    return this->tilt->get_position()[0];
  }
  else
  {
    /* handle exceptions */
  }
}

float CSegwayHead::get_side_position(void)
{
  if(this->servo_ids.side!=-1)
  {
    return this->side->get_position()[0];
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::get_head_position(float *pan, float *tilt, float *side)
{
  *pan=this->get_pan_position();
  *tilt=this->get_tilt_position();
  *side=this->get_side_position();
}

void CSegwayHead::set_brightness(float brightness)
{
  unsigned char brightness_value;

  if(this->servo_ids.lights!=-1)
  {
    if(brightness<0.0 || brightness>100.0)
    {
      /* handle exceptions */
    }
    else
    {
      brightness_value=(unsigned char)(brightness*255.0/100.0);
      this->lights->write_byte_register(brightness_reg,brightness_value);
    }
  }
  else
  {
    /* handle exceptions */
  }
}

float CSegwayHead::get_brightness(void)
{
  unsigned char brightness_value;

  if(this->servo_ids.lights!=-1)
  {
    this->lights->read_byte_register(brightness_reg,&brightness_value);
    return ((float)brightness_value)*100.0/255.0;
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::turn_on_segment(segment_id id)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register((unsigned char)id,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::turn_off_segment(segment_id id)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register((unsigned char)id,0x00);
  }
  else
  {
    /* handle exceptions */
  }
}

bool CSegwayHead::is_segment_on(segment_id id)
{ 
  unsigned char state;

  if(this->servo_ids.lights!=-1)
  {
    this->lights->read_byte_register((unsigned char)id,&state);
    if(state==0x01) 
      return true;
    else
      return false;
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::stand_by(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(standby_reg,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::happy(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(happy_reg,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::sat(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(sat_reg,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::angry(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(angry_reg,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::confused(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(confused_reg,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

void CSegwayHead::shy(void)
{
  if(this->servo_ids.lights!=-1)
  {
    this->lights->write_byte_register(shy_reg,0x01);
  }
  else
  {
    /* handle exceptions */
  }
}

CSegwayHead::~CSegwayHead()
{
  this->servo_ids.bus_id=-1;
  this->servo_ids.pan=-1;
  this->servo_ids.tilt=-1;
  this->servo_ids.side=-1;
}