Commit 9ed4648c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Removed some exceptions specific to a single error.

Exported the Segway RMP200 events to this driver.
Added a new event to notify the availability of the two status structures.
parent af2aea42
......@@ -7,13 +7,12 @@ using namespace std;
int main(int argc, char *argv[])
{
cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
CSegwayRMP400 *segway;
CSegwayRMP400 segway;
segway=new CSegwayRMP400();
try
{
segway->start();
segway->reset_integrators();
segway.connect("00000269","00000261");
segway.reset_integrators();
cout << "Platform started" << endl;
}
catch (CException & e)
......@@ -38,39 +37,38 @@ int main(int argc, char *argv[])
cout << "No obstacle avoidance is implemented. If there is something in the way the robot will hit it!" << endl;
cout << "Moving 0.5 meters at 0.05m/sec" << endl;
sleep(1);
segway->move(0.05,0.0); // 5cm per second 10 seconds = half a meter
segway.move(0.05,0.0); // 5cm per second 10 seconds = half a meter
int i;
//for(i=0;i<10;i++)
for(;;)
{
sleep(1);
cout << (*segway) << endl;
cout << segway << endl;
}
segway->stop();
segway.stop();
sleep(2);
cout << "Rotating 360 degrees with center of rotation on the side of the base" << endl;
sleep(1);
segway->move(0.0,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn
segway.move(0.0,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn
// center right on the side of the base (about 54 cm from center of base)
segway->stop();
segway.stop();
sleep(2);
cout << "Rotating 360 degrees with center of rotation 1m away from the base center" << endl;
sleep(1);
segway->move(0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn
segway.move(0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn
// center of rotation vT/vR = 1meter from the center of the base
// tensecs(segway);
segway->stop();
segway.stop();
sleep(2);
cout << "Rotating backwards 360 degrees with center of rotation 1m away from the base center" << endl;
sleep(1);
segway->move(-0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn BACKWARDS
segway.move(-0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn BACKWARDS
// center of rotation vT/vR = 1meter from the center of the base
}
segway->stop();
delete segway;
segway.stop();
return 0;
}
#include "segway_RMP400.h"
#include "segway_RMP400_exception.h"
#include <iridrivers/segway_rmp200_exceptions.h>
#include "segway_rmp200_exceptions.h"
#include <cmath>
CSegwayRMP400::CSegwayRMP400() :
status_(rmp400_off),
forward_displacement_(0.0),
yaw_displacement_(0.0),
yaw_rate_ (0.0),
wheel_radius_(default_wheel_radius_),
use_correction_wheel_factor_(true)
forward_displacement(0.0),
yaw_displacement(0.0),
yaw_rate(0.0)
{
scan_ftdi_for_segways();
for (unsigned int i = 0;i < NUM_SEGWAY_200 ;i++) {
CSegwayRMP200 * segway = new CSegwayRMP200();
segways_.push_back(segway);
}
this->connected=false;
}
CSegwayRMP400::CSegwayRMP400(const std::string serial1, const std::string serial2) :
status_(rmp400_off),
forward_displacement_(0.0),
yaw_displacement_(0.0),
yaw_rate_ (0.0),
wheel_radius_(default_wheel_radius_),
use_correction_wheel_factor_(true)
CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear) :
forward_displacement(0.0),
yaw_displacement(0.0),
yaw_rate(0.0)
{
if (serial1 == serial2)
throw RMP400IdemSerialException(_HERE_, serial1);
scan_ftdi_for_segways();
for (unsigned int i = 0; i < serial_ftdi_segway_devices_.size(); i++) {
std::string serial = serial_ftdi_segway_devices_[i];
if ((serial != serial1) && (serial != serial2))
throw RMP400NotFoundSerialException(_HERE_, serial);
}
this->connected=false;
this->connect(front,rear);
}
void
CSegwayRMP400::scan_ftdi_for_segways()
void *CSegwayRMP400::monitor_thread(void *param)
{
CFTDIServer * ftdi_server_ = CFTDIServer::instance();
ftdi_server_->add_custom_PID(CSegwayRMP200::pid);
std::vector<int> ftdi_devs = ftdi_server_->get_ids_by_description(CSegwayRMP200::description);
for (unsigned int i = 0;i < ftdi_devs.size();i++) {
serial_ftdi_segway_devices_.push_back(
ftdi_server_->get_serial_number(ftdi_devs.at(i)));
}
if (serial_ftdi_segway_devices_.size() != NUM_SEGWAY_200)
throw RMP400NotValidNumberRMP200Exception(_HERE_, serial_ftdi_segway_devices_.size());
}
CSegwayRMP400 *segway=(CSegwayRMP400 *)param;
void
CSegwayRMP400::start()
{
for (unsigned int i = 0;i < segways_.size();i++) {
try
{
segways_[i]->connect(serial_ftdi_segway_devices_[i]);
}
catch (CSegwayRMP200Exception & e)
{
// Something failed while starting engines
// clean up previous (if any) started engine
for (unsigned int i = 0; i < segways_.size(); i++) {
segways_[i]->close();
}
status_ = rmp400_off;
throw;
}
}
status_ = rmp400_connected;
pthread_exit(NULL);
}
void
CSegwayRMP400::close()
void CSegwayRMP400::init_events_threads(void)
{
for (unsigned int i = 0;i < segways_.size();i++) {
segways_[i]->close();
}
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);
}
std::ostream &
operator<< (std::ostream& data, CSegwayRMP400 & segway)
void CSegwayRMP400::connect(const std::string &front, const std::string &rear)
{
for (unsigned int i=0; i<segway.segways_.size(); i++)
if(!this->connected)
{
data << "Segway unit: " << i << std::endl;
data << "Pitch angle: " << segway.segways_[i]->get_pitch_angle() << " degrees" << std::endl;
data << "Pitch rate " << segway.segways_[i]->get_pitch_rate() << " degrees/s" << std::endl;
data << "Roll angle: " << segway.segways_[i]->get_roll_angle() << " degrees" << std::endl;
data << "Roll rate: " << segway.segways_[i]->get_roll_rate() << " degrees/s" << std::endl;
data << "Left wheel velocity: " << segway.segways_[i]->get_left_wheel_velocity() << " m/s" << std::endl;
data << "Right wheel velocity: " << segway.segways_[i]->get_right_wheel_velocity() << " m/s" << std::endl;
data << "Yaw rate: " << segway.segways_[i]->get_yaw_rate() << " degrees/s" << std::endl;
data << "Up time: " << segway.segways_[i]->get_uptime() << " s" << std::endl;
data << "Left wheel displacement: " << segway.segways_[i]->get_left_wheel_displacement() << " m" << std::endl;
data << "Right wheel displacement: " << segway.segways_[i]->get_right_wheel_displacement() << " m" << std::endl;
data << "Forward displacement: " << segway.segways_[i]->get_forward_displacement() << " m" << std::endl;
data << "Yaw displacement: " << segway.segways_[i]->get_yaw_displacement() << " rev" << std::endl;
data << "Left motor torque: " << segway.segways_[i]->get_left_motor_torque() << " Nm" << std::endl;
data << "Right motor torque: " << segway.segways_[i]->get_right_motor_torque() << " Nm" << std::endl;
if(segway.segways_[i]->get_operation_mode()==tractor)
data << "Operation mode: tractor" << std::endl;
else
data << "Operation mode: power down" << std::endl;
if(segway.segways_[i]->get_gain_schedule()==light)
data << "Gain schedule: light" << std::endl;
else if(segway.segways_[i]->get_gain_schedule()==tall)
data << "Gain schedule: tall" << std::endl;
if(front==rear)
throw CSegwayRMP400Exception(_HERE_,"Rear and front serial number can't be the same",front);
else
data << "Gain schedule: heavy" << std::endl;
data << "UI battery voltage: " << segway.segways_[i]->get_ui_battery_voltage() << " V" << std::endl;
data << "Powerbase battery voltage: " << segway.segways_[i]->get_powerbase_battery_voltage() << " V" << std::endl << std::endl;
}
data << std::endl;
return data;
}
void
CSegwayRMP400::set_operation_mode(op_mode mode)
{
if (mode == balance)
throw RMP400InvalidBalanceModeOperation(_HERE_);
for (unsigned int i=0;i < segways_.size();i++)
{
segways_[i]->set_operation_mode(mode);
usleep(10000);
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::need_to_be_connected() const
void CSegwayRMP400::close()
{
if (! is_connected())
throw RMP400NoConnected(_HERE_);
// 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;
}
void
CSegwayRMP400::disable_corretion_wheel_factor()
std::ostream & operator<< (std::ostream& data, CSegwayRMP400 & segway)
{
use_correction_wheel_factor_ = false;
}
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;
void
CSegwayRMP400::fix_with_wheel_factor(float & value)
{
/* corretion factor is the diference between real wheel and supposed
* firmware wheel (probaby segway using the rmp200 value) */
value = value * (wheel_radius_ / RMP200_DEFAULT_WHEEL_RADIUS);
}
void
CSegwayRMP400::set_wheel_radius(const float radius)
{
wheel_radius_ = radius;
return data;
}
TSegwayRMP400Status
CSegwayRMP400::get_status()
TSegwayRMP400Status CSegwayRMP400::get_status()
{
TSegwayRMP400Status status;
status.rmp200[0] = segways_[0]->get_status();
status.rmp200[1] = segways_[1]->get_status();
return status;
return this->status;
}
float
CSegwayRMP400::get_forward_displacement()
double CSegwayRMP400::get_forward_displacement()
{
need_to_be_connected();
// The get_forward_displacement function in segway RMP 200 doesn't
// return the accumulate value of displacement, wheel_displacement
// function do. Use them instead.
double fwd_displ_front,fwd_displ_rear;
double fwd_displ_segway0 = (segways_[0]->get_right_wheel_displacement() +
segways_[0]->get_left_wheel_displacement()) / 2;
double fwd_displ_segway1 = (segways_[1]->get_right_wheel_displacement() +
segways_[1]->get_left_wheel_displacement()) / 2;
// The get_forward_displacement function in segway RMP 200 doesn't
// return the accumulate value of displacement, wheel_displacement
// function do. Use them instead.
forward_displacement_ = (fwd_displ_segway0 + fwd_displ_segway1) / 2;
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;
if (use_correction_wheel_factor_)
fix_with_wheel_factor(forward_displacement_);
this->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;
return forward_displacement_;
return this->forward_displacement;
}
else
throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}
float
CSegwayRMP400::get_yaw_displacement()
double CSegwayRMP400::get_yaw_displacement()
{
need_to_be_connected();
float yaw_displacement_tmp = (segways_[0]->get_yaw_displacement() +
segways_[1]->get_yaw_displacement()) / 2;
if (use_correction_wheel_factor_)
fix_with_wheel_factor(yaw_displacement_tmp);
if(this->connected)
{
this->yaw_displacement=(this->segways[front_segway].get_yaw_displacement()+this->segways[rear_segway].get_yaw_displacement())/2;
yaw_displacement_ = yaw_displacement_tmp;
return yaw_displacement_;
return this->yaw_displacement;
}
else
throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}
float
CSegwayRMP400::get_yaw_rate()
double CSegwayRMP400::get_yaw_rate()
{
need_to_be_connected();
yaw_rate_ = (segways_[0]->get_yaw_rate() +
segways_[1]->get_yaw_rate()) / 2;
if(this->connected)
{
this->yaw_rate=(this->segways[front_segway].get_yaw_rate()+this->segways[rear_segway].get_yaw_rate())/2;
return yaw_rate_;
return this->yaw_rate;
}
else
throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}
void
CSegwayRMP400::reset_integrators()
void CSegwayRMP400::reset_integrators()
{
need_to_be_connected();
double fd=0.0,yd=0.0;
unsigned int i=0;
bool reset=false;
if(this->connected)
{
do {
for (unsigned int i=0; i < segways_.size(); i++) {
segways_[i]->reset_right_wheel_integrator();
segways_[i]->reset_left_wheel_integrator();
segways_[i]->reset_yaw_integrator();
segways_[i]->reset_forward_integrator();
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);
} while (! is_valid_reset_integrators());
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","");
}
bool
CSegwayRMP400::is_valid_reset_integrators()
std::string CSegwayRMP400::get_front_cable_disconnected_event_id(void)
{
float fd = get_forward_displacement();
float yd = get_yaw_displacement();
return this->front_cable_disconnect_event_id;
}
if (std::fabs(0.0 - fd) > 0.1) {
std::cerr << "[segway] Invalid forward reset value " << fd << std::endl;
return false;
}
std::string CSegwayRMP400::get_rear_cable_disconnected_event_id(void)
{
return this->rear_cable_disconnect_event_id;
}
if (std::fabs(0.0 - yd) > 0.1) {
std::cerr << "[segway] Invalid yaw reset value " << yd << std::endl;
return false;
}
std::string CSegwayRMP400::get_front_power_off_event_id(void)
{
return this->front_power_off_event_id;
}
return true;
std::string CSegwayRMP400::get_rear_power_off_event_id(void)
{
return this->rear_power_off_event_id;
}
void
CSegwayRMP400::move(float vT, float vR)
std::string CSegwayRMP400::get_front_no_heartbeat_event_id(void)
{
need_to_be_connected();
return this->front_no_heartbeat_event_id;
}
for (unsigned int i=0 ;i < segways_.size(); i++)
segways_[i]->move(vT,vR);
std::string CSegwayRMP400::get_rear_no_heartbeat_event_id(void)
{
return this->rear_no_heartbeat_event_id;
}
void
CSegwayRMP400::stop()
std::string CSegwayRMP400::get_new_status_event_id(void)
{
need_to_be_connected();
return this->new_status_event_id;
}
for (unsigned int i=0;i < segways_.size();i++)
segways_[i]->stop();
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","");
}
//status_ = rmp400_off;
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()
{
for (unsigned int i=0;i < segways_.size(); i++)
delete segways_[i];
this->close();
}
#ifndef _SEGWAYRMP400_H
#define _SEGWAYRMP400_H
#include <segway_rmp200.h>
#include <ftdiserver.h>
#include "segway_rmp200.h"
#include "ftdiserver.h"
#include "threadserver.h"
#include "eventserver.h"
#include "mutex.h"
#include <vector>
#include <string>
#include <iostream>
#define RMP400_b 0.54 // distance between weels for one rmp unit in meters
#define RMP200_DEFAULT_WHEEL_RADIUS 0.265
const unsigned int NUM_SEGWAY_200 = 2;
enum SegwayRMP400_status { rmp400_off, rmp400_connected };
typedef enum {front_segway=0,rear_segway} segway_id_t;
struct TSegwayRMP400Status
{
TSegwayRMP200Status rmp200[2];
TSegwayRMP200Status rmp200[NUM_SEGWAY_200];
};
class CSegwayRMP400
{
private:
std::vector<CSegwayRMP200 *> segways_;
/* vector of valid segway serials found in the ftdi bus */
std::vector<std::string> serial_ftdi_segway_devices_;
/**
* \brief Segway status
*/
SegwayRMP400_status status_;
std::vector<CSegwayRMP200> segways;
/**
* \brief forward displacement
*
* This value has the displacement of the whole platform in meters
* TODO: should be a thread periodically updating this value
*/
float forward_displacement_;