From 9ed4648cc8c555f4984db098f156b28928c21c66 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Tue, 19 Apr 2016 12:58:48 +0000
Subject: [PATCH] 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.
---
src/examples/test_segwayRMP400.cpp | 26 +-
src/segway_RMP400.cpp | 412 ++++++++++++++---------------
src/segway_RMP400.h | 266 +++++++++++--------
src/segway_RMP400_exception.cpp | 43 ---
src/segway_RMP400_exception.h | 40 ---
5 files changed, 373 insertions(+), 414 deletions(-)
diff --git a/src/examples/test_segwayRMP400.cpp b/src/examples/test_segwayRMP400.cpp
index 2186490..e2e662c 100644
--- a/src/examples/test_segwayRMP400.cpp
+++ b/src/examples/test_segwayRMP400.cpp
@@ -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;
}
diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp
index 71ef1bc..89374f8 100644
--- a/src/segway_RMP400.cpp
+++ b/src/segway_RMP400.cpp
@@ -1,287 +1,277 @@
#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();
}
diff --git a/src/segway_RMP400.h b/src/segway_RMP400.h
index 80647b5..3fa219f 100644
--- a/src/segway_RMP400.h
+++ b/src/segway_RMP400.h
@@ -1,43 +1,35 @@
#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_;
+ double forward_displacement;
/**
* \brief yaw displacement
@@ -45,7 +37,7 @@ class CSegwayRMP400
* This value has the rotation of the whole platform in revolutions.
* TODO: should be a thread periodically updating this value
*/
- float yaw_displacement_;
+ double yaw_displacement;
/**
* \brief yaw rate
@@ -53,92 +45,118 @@ class CSegwayRMP400
* This value has the yaw rate of the platform in degrees per second.
* TODO: should be a thread periodically updating this value
*/
- float yaw_rate_;
-
+ double yaw_rate;
/**
- * \brief default rmp400 wheel radius when inflated to 10 psi (in meters)
+ * \brief
+ *
*/
- static const float default_wheel_radius_ = 0.265;
-
+ bool connected;
/**
- * \brief wheel radius in meters
+ * \brief
*
*/
- float wheel_radius_;
-
+ std::string front_serial;
/**
- * \brief whether use or not the correction factor to fix wheel size
- * (default is true). See fix_with_wheel_factor for more info.
+ * \brief
*
*/
- bool use_correction_wheel_factor_;
-
+ std::string rear_serial;
/**
- * \brief apply a corretion factor to the value given
+ * \brief
*
- * It is suspected that segway is using the RMP200 wheel radius in RMP400
- * firmware which gives a bad measure for displacements. This functions
- * calculate the relation between radius and fix the value given using this
- * factor.
*/
- void fix_with_wheel_factor(float & value);
-
+ CThreadServer *thread_server;
/**
- * \brief disable use of correction wheel factor for displacements
+ * \brief
*
- * Check fix_with_wheel_factor for more info.
*/
- void disable_corretion_wheel_factor();
-
- void scan_ftdi_for_segways();
-
+ std::string monitor_thread_id;
/**
- * \brief set a different wheel radius (in meters)
+ * \brief
*
- * Default, when wheels are inflated to 10psi, is 0.26m. If the segway has
- * a different meassure, please use this function before starting to operate
- * as normal.
*/
- void set_wheel_radius(const float radius);
-
- /*
- * \brief check if the driver is already connected (made a start() call).
+ CEventServer *event_server;
+ /**
+ * \brief
*
- * It will return an RMP400NoConnected exception if fail
*/
- void need_to_be_connected() const;
-
+ std::string finish_thread_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string front_cable_disconnect_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string rear_cable_disconnect_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string front_power_off_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string rear_power_off_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string front_no_heartbeat_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string rear_no_heartbeat_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string new_status_event_id;
+ /**
+ * \brief
+ *
+ */
+ CMutex status_access;
+ /**
+ * \brief
+ *
+ */
+ TSegwayRMP400Status status;
/* \brief aux function to check if platform is really doing the reset in
* integrators.
*/
bool is_valid_reset_integrators();
-
protected:
/**
- * \brief allowed modes:
- * - tractor
- * - power_dow
- */
- op_mode mode;
-
+ * \brief
+ *
+ */
+ static void *monitor_thread(void *param);
+ /**
+ * \brief
+ *
+ */
+ void init_events_threads(void);
public:
+ /**
+ * \brief
+ *
+ */
CSegwayRMP400();
- CSegwayRMP400(const std::string serial1, const std::string serial2);
-
- ~CSegwayRMP400();
-
/**
- * \brief function to set the operation mode
+ * \brief
*
- * Operation modes: tractor or power_down.
- * In the RMP400 the balance mode is disabled.
*/
- void set_operation_mode(op_mode mode);
-
+ CSegwayRMP400(const std::string &front, const std::string &rear);
/**
- * \brief function to check if segway is already connected
+ * \brief
+ *
*/
- bool is_connected() const;
+ void connect(const std::string &front,const std::string &rear);
/**
* \brief function to return whole platform status
@@ -160,7 +178,7 @@ class CSegwayRMP400
* \return the current forward displacement in meters.
*
*/
- float get_forward_displacement(void);
+ double get_forward_displacement(void);
/**
* \brief function to return the total yaw displacement
@@ -175,7 +193,7 @@ class CSegwayRMP400
* \return the current yaw displacement in revolutions per second.
*
*/
- float get_yaw_displacement(void);
+ double get_yaw_displacement(void);
/**
* \brief function to return the yaw rate
@@ -190,54 +208,90 @@ class CSegwayRMP400
* \return the current yaw rate in revolutions per second.
*
*/
- float get_yaw_rate(void);
+ double get_yaw_rate(void);
/**
- * \brief function to reset integrators
- *
- * This function sets all encoder integrators to zero in both platforms.
- * Currently this function seems NOT to be reliable since hardware not
- * always is doing the proper work.
- */
+ * \brief function to reset integrators
+ *
+ * This function sets all encoder integrators to zero in both platforms.
+ * Currently this function seems NOT to be reliable since hardware not
+ * always is doing the proper work.
+ */
void reset_integrators();
/**
- * \brief function to move the platform
- *
- * This function sets the rotational and translational velocities to a desired value
- * vT is in m/sec, vR is in rev/sec
- *
- * Rotation about the center of the platform with skid steer is disabled.
- * Skid steering is not recommended since it produces significant tire wear and unnecessary battery consumption.
- * Odometry readings during skid steering are useless since they depend heavily on soil characteristics.
- * A minimum Instantaneous Center of Rotation (ICR) located on the side of the platform is enforced (0.54cm).
- */
- void move(float vT, float vR);
+ * \brief
+ *
+ */
+ std::string get_front_cable_disconnected_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_rear_cable_disconnected_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_front_power_off_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_rear_power_off_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_front_no_heartbeat_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_rear_no_heartbeat_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_new_status_event_id(void);
- /* \brief function to connect segways RMP200
+ /**
+ * \brief function to move the platform
+ *
+ * This function sets the rotational and translational velocities to a desired value
+ * vT is in m/sec, vR is in rev/sec
*
- * This functions start the vehicle
+ * Rotation about the center of the platform with skid steer is disabled.
+ * Skid steering is not recommended since it produces significant tire wear and unnecessary battery consumption.
+ * Odometry readings during skid steering are useless since they depend heavily on soil characteristics.
+ * A minimum Instantaneous Center of Rotation (ICR) located on the side of the platform is enforced (0.54cm).
*/
- void start();
+ void move(double vT, double vR);
/* \brief function to stop the platform
- *
- * This function stops the vehicle
- */
+ *
+ * This function stops the vehicle
+ */
void stop();
+ /**
+ * \brief
+ *
+ */
void close();
- friend std::ostream& operator<< (std::ostream& out, CSegwayRMP400& segway);
+ /**
+ * \brief
+ *
+ */
+ friend std::ostream& operator<< (std::ostream& out, CSegwayRMP400& segway);
+ /**
+ * \brief
+ *
+ */
+ ~CSegwayRMP400();
};
-inline bool
-CSegwayRMP400::is_connected() const
-{
- return (status_ == rmp400_connected);
-}
-
-
#endif
diff --git a/src/segway_RMP400_exception.cpp b/src/segway_RMP400_exception.cpp
index 4daace4..cfa9db4 100644
--- a/src/segway_RMP400_exception.cpp
+++ b/src/segway_RMP400_exception.cpp
@@ -11,46 +11,3 @@ CSegwayRMP400Exception::CSegwayRMP400Exception(const std::string & where,
{
}
-CSegwayRMP400Exception::CSegwayRMP400Exception(const std::string & where,
- const std::string & error_msg) throw () :
- CException(where, segway_rmp400_exception_msg)
-{
-}
-
-RMP400IdemSerialException::RMP400IdemSerialException(const std::string & where,
- const std::string & serial) throw () :
- CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
-{
- error_msg += "Serial ports are the same for two segways: " + serial;
-}
-
-RMP400NotFoundSerialException::RMP400NotFoundSerialException(const std::string & where,
- const std::string & serial) throw () :
-
- CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
-{
- error_msg += "Serial port found: " + serial + " not inside valid ports supplied";
-}
-
-
-RMP400NotValidNumberRMP200Exception::RMP400NotValidNumberRMP200Exception(const std::string & where,
- const int & number_found) throw () :
- CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
-{
- std::stringstream s;
- s << number_found;
-
- error_msg += "Found " + s.str() + " RMP200 units which is not valid for this device";
-}
-
-RMP400InvalidBalanceModeOperation::RMP400InvalidBalanceModeOperation(const std::string & where) throw () :
- CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
-{
- error_msg += "Balance mode for the segway RMP400 is completly crazy";
-}
-
-RMP400NoConnected::RMP400NoConnected(const std::string & where) throw () :
- CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
-{
- error_msg += "Segway RMP 400 has not been connected";
-}
diff --git a/src/segway_RMP400_exception.h b/src/segway_RMP400_exception.h
index bd8b987..c36db33 100644
--- a/src/segway_RMP400_exception.h
+++ b/src/segway_RMP400_exception.h
@@ -57,44 +57,4 @@ class CSegwayRMP400Exception : public CException
CSegwayRMP400Exception(const std::string& where,const std::string& error_msg) throw();
};
-class RMP400IdemSerialException :
- public CSegwayRMP400Exception
-{
- public:
- /* Constructor */
- RMP400IdemSerialException(const std::string & where, const std::string & serial) throw ();
-};
-
-class RMP400NotFoundSerialException :
- public CSegwayRMP400Exception
-{
- public:
- /* Constructor */
- RMP400NotFoundSerialException(const std::string & where, const std::string & serial) throw ();
-};
-
-class RMP400NotValidNumberRMP200Exception :
- public CSegwayRMP400Exception
-{
- public:
- /* Constructor */
- RMP400NotValidNumberRMP200Exception(const std::string & where, const int & number) throw ();
-};
-
-class RMP400InvalidBalanceModeOperation :
- public CSegwayRMP400Exception
-{
- public:
- /* Constructor */
- RMP400InvalidBalanceModeOperation(const std::string & where) throw ();
-};
-
-class RMP400NoConnected :
- public CSegwayRMP400Exception
-{
- public:
- /* constructor */
- RMP400NoConnected(const std::string & where) throw ();
-};
-
#endif
--
GitLab