segway_RMP400.cpp

#include "segway_RMP400.h"
#include "segway_RMP400_exception.h"
#include "segway_rmp200_exceptions.h"
#include <cmath>

CSegwayRMP400::CSegwayRMP400() :
  forward_displacement(0.0),
  yaw_displacement(0.0),
  yaw_rate(0.0)
{
  this->connected=false;
}

CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear) :
  forward_displacement(0.0),
  yaw_displacement(0.0),
  yaw_rate(0.0)
{
  this->connected=false;
  this->connect(front,rear);
}

void *CSegwayRMP400::monitor_thread(void *param)
{
  CSegwayRMP400 *segway=(CSegwayRMP400 *)param;

  pthread_exit(NULL);
}

void CSegwayRMP400::init_events_threads(void)
{
  std::string id="segway_"+this->front_serial+"_"+this->rear_serial;

  /* initialize threads */
  this->thread_server=CThreadServer::instance();
  this->monitor_thread_id=id+"_monitor_thread";
  this->thread_server->create_thread(this->monitor_thread_id);
  this->thread_server->attach_thread(this->monitor_thread_id,this->monitor_thread,this);
  /* initialize events */
  this->event_server=CEventServer::instance();
  this->finish_thread_event_id=id+"_finish_thread_event";
  this->event_server->create_event(this->finish_thread_event_id);
  this->front_cable_disconnect_event_id=id+"_front_disconnect_event";
  this->event_server->create_event(this->front_cable_disconnect_event_id);
  this->rear_cable_disconnect_event_id=id+"_rear_disconnect_event";
  this->event_server->create_event(this->rear_cable_disconnect_event_id);
  this->front_power_off_event_id=id+"_front_power_off_event";
  this->event_server->create_event(this->front_power_off_event_id);
  this->rear_power_off_event_id=id+"_rear_power_off_event";
  this->event_server->create_event(this->rear_power_off_event_id);
  this->front_no_heartbeat_event_id=id+"_front_no_heartbeat_event";
  this->event_server->create_event(this->front_no_heartbeat_event_id);
  this->rear_no_heartbeat_event_id=id+"_rear_no_heartbeat_event";
  this->event_server->create_event(this->rear_no_heartbeat_event_id);
  this->new_status_event_id=id+"_new_status_event";
  this->event_server->create_event(this->new_status_event_id);
  // start the internal thread
  this->thread_server->start_thread(this->monitor_thread_id);
}

void CSegwayRMP400::connect(const std::string &front, const std::string &rear)
{
  if(!this->connected)
  {
    if(front==rear)
      throw CSegwayRMP400Exception(_HERE_,"Rear and front serial number can't be the same",front);
    else
    {
      try{
        this->segways[front_segway].connect(front);
        this->front_serial=front;
        this->segways[rear_segway].connect(rear);
        this->rear_serial=rear;
        this->init_events_threads();
        this->connected=true;
      }catch(CException &e){
        this->close();
        throw e;
      }
    }
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is already connected",front+"_"+rear);
}

void CSegwayRMP400::close()
{
  // signal the thread to end
  if(this->thread_server->get_thread_state(this->monitor_thread_id)==starting ||
     this->thread_server->get_thread_state(this->monitor_thread_id)==active)
  {
    this->event_server->set_event(this->finish_thread_event_id);
    this->thread_server->end_thread(this->monitor_thread_id);
    this->thread_server->detach_thread(this->monitor_thread_id);
    this->thread_server->delete_thread(this->monitor_thread_id);
    this->monitor_thread_id.clear();
    this->event_server->delete_event(this->finish_thread_event_id);
    this->finish_thread_event_id.clear();
    this->event_server->delete_event(this->front_cable_disconnect_event_id);
    this->front_cable_disconnect_event_id.clear();
    this->event_server->delete_event(this->rear_cable_disconnect_event_id);
    this->rear_cable_disconnect_event_id.clear();
    this->event_server->delete_event(this->front_power_off_event_id);
    this->front_power_off_event_id.clear();
    this->event_server->delete_event(this->rear_power_off_event_id);
    this->rear_power_off_event_id.clear();
    this->event_server->delete_event(this->front_no_heartbeat_event_id);
    this->front_no_heartbeat_event_id.clear();
    this->event_server->delete_event(this->rear_no_heartbeat_event_id);
    this->rear_no_heartbeat_event_id.clear();
    this->event_server->delete_event(this->new_status_event_id);
    this->new_status_event_id.clear();
  }
  if(this->front_serial.size()>0) 
  {
    this->segways[front_segway].close();
    this->front_serial.clear();
  }
  if(this->rear_serial.size()>0)
  {
    this->segways[rear_segway].close();
    this->rear_serial.clear();
  }
  this->connected=false;
}

std::ostream & operator<< (std::ostream& data, CSegwayRMP400 & segway)
{
  data << "Front segway status data:" << std::endl;
  data << segway.segways[front_segway] << std::endl;
  data << "Rear segway status data:" << std::endl;
  data << segway.segways[rear_segway] << std::endl;

  return data;
}

TSegwayRMP400Status CSegwayRMP400::get_status()
{
  return this->status;
}

double CSegwayRMP400::get_forward_displacement()
{
  double fwd_displ_front,fwd_displ_rear; 

  // The get_forward_displacement function in segway RMP 200 doesn't
  // return the accumulate value of displacement, wheel_displacement
  // function do. Use them instead.

  if(this->connected)
  {
    fwd_displ_front=(this->segways[front_segway].get_right_wheel_displacement()+this->segways[front_segway].get_left_wheel_displacement())/2;
    fwd_displ_rear=(this->segways[rear_segway].get_right_wheel_displacement()+this->segways[rear_segway].get_left_wheel_displacement())/2;

    this->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;

    return this->forward_displacement;
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}

double CSegwayRMP400::get_yaw_displacement()
{
  if(this->connected)
  {
    this->yaw_displacement=(this->segways[front_segway].get_yaw_displacement()+this->segways[rear_segway].get_yaw_displacement())/2;

    return this->yaw_displacement;
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}

double CSegwayRMP400::get_yaw_rate()
{
  if(this->connected)
  {
    this->yaw_rate=(this->segways[front_segway].get_yaw_rate()+this->segways[rear_segway].get_yaw_rate())/2;

    return this->yaw_rate;
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}

void CSegwayRMP400::reset_integrators()
{
  double fd=0.0,yd=0.0;
  unsigned int i=0;
  bool reset=false;

  if(this->connected)
  {
    do {
      for(i=0;i<this->segways.size();i++)
      {
        this->segways[i].reset_right_wheel_integrator();
        this->segways[i].reset_left_wheel_integrator();
        this->segways[i].reset_yaw_integrator();
        this->segways[i].reset_forward_integrator();
      }
      // Need to leave the segwayrmp 200 driver to read need values
      sleep(1);
      fd=this->get_forward_displacement();
      yd=this->get_yaw_displacement();
      if(std::fabs(fd)<0.1 && std::fabs(yd)<0.1)
        reset=true;
      else
        reset=false;
    }while(!reset);
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}

std::string CSegwayRMP400::get_front_cable_disconnected_event_id(void)
{
  return this->front_cable_disconnect_event_id;
}

std::string CSegwayRMP400::get_rear_cable_disconnected_event_id(void)
{
  return this->rear_cable_disconnect_event_id;
}

std::string CSegwayRMP400::get_front_power_off_event_id(void)
{
  return this->front_power_off_event_id;
}

std::string CSegwayRMP400::get_rear_power_off_event_id(void)
{
  return this->rear_power_off_event_id;
}

std::string CSegwayRMP400::get_front_no_heartbeat_event_id(void)
{
  return this->front_no_heartbeat_event_id;
}

std::string CSegwayRMP400::get_rear_no_heartbeat_event_id(void)
{
  return this->rear_no_heartbeat_event_id;
}

std::string CSegwayRMP400::get_new_status_event_id(void)
{
  return this->new_status_event_id;
}

void CSegwayRMP400::move(double vT, double vR)
{
  if(this->connected)
  {
    this->segways[front_segway].move(vT,vR);
    this->segways[rear_segway].move(vT,vR);
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}

void CSegwayRMP400::stop()
{
  if(this->connected)
  {
    this->segways[front_segway].stop();
    this->segways[rear_segway].stop();
  }
  else
    throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}

CSegwayRMP400::~CSegwayRMP400()
{
  this->close();
}