diff --git a/src/examples/test_segwayRMP400.cpp b/src/examples/test_segwayRMP400.cpp
index e2e662c02de134e9397874ccfe6ad66d463673b2..0a9959293008180f60a60092b0af4dbf8064a0e2 100644
--- a/src/examples/test_segwayRMP400.cpp
+++ b/src/examples/test_segwayRMP400.cpp
@@ -1,74 +1,59 @@
#include "segway_RMP400.h"
#include "segway_RMP400_exception.h"
+#include "eventexceptions.h"
#include <iostream>
using namespace std;
int main(int argc, char *argv[])
-{
- cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
- CSegwayRMP400 segway;
-
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events;
+ unsigned int i=0;
+
try
{
+ cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
+ CSegwayRMP400 segway;
segway.connect("00000269","00000261");
segway.reset_integrators();
cout << "Platform started" << endl;
- }
- catch (CException & e)
- {
- cout<< e.what().c_str()<<endl;
- return false;
- }
-
- char c;
- do{
- cout << "Do you want to move the robot? (y/n):" << endl;
- cin >> c;
- }while(c!='y' && c!='n');
- if(c=='n')
- {
- cout << "Press any key+intro to close the program" << endl;
- cin >> c;
- }else if(c=='y')
- {
- cout << "CAUTION!! THIS PROGRAM MOVES THE SEGWAYRMP400" << endl;
- cout << "You need at least 4x4 meter clearance around the robot for these tests!" << endl;
- 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);
+ cout << "start moving forward" << endl;
+ events.push_back(segway.get_new_status_event_id());
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;
+ try{
+ event_server->wait_first(events,1000);
+ cout << segway << endl;
+ }catch(CEventTimeoutException &e){
+ std::cout << "No status data received for over a second" << std::endl;
+ }
}
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
// center right on the side of the base (about 54 cm from center of base)
+ for(i=0;i<10;i++)
+ {
+ sleep(1);
+ cout << segway << endl;
+ }
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
- // center of rotation vT/vR = 1meter from the center of the base
- // tensecs(segway);
- 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
- // center of rotation vT/vR = 1meter from the center of the base
+ for(i=0;i<10;i++)
+ {
+ sleep(1);
+ cout << segway << endl;
+ }
+ }
+ catch (CException & e)
+ {
+ cout<< e.what().c_str()<<endl;
}
-
- segway.stop();
- return 0;
}
diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp
index 89374f82b7175b4bc156c895f590d8736cc6cc5f..027bb49e7f7a9710c5dd072d04355a026097416a 100644
--- a/src/segway_RMP400.cpp
+++ b/src/segway_RMP400.cpp
@@ -9,6 +9,7 @@ CSegwayRMP400::CSegwayRMP400() :
yaw_rate(0.0)
{
this->connected=false;
+ this->segways.resize(NUM_SEGWAY_200);
}
CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear) :
@@ -17,12 +18,60 @@ CSegwayRMP400::CSegwayRMP400(const std::string &front, const std::string &rear)
yaw_rate(0.0)
{
this->connected=false;
+ this->segways.resize(NUM_SEGWAY_200);
this->connect(front,rear);
}
void *CSegwayRMP400::monitor_thread(void *param)
{
CSegwayRMP400 *segway=(CSegwayRMP400 *)param;
+ std::vector<bool> status_received(false,NUM_SEGWAY_200);
+ double fwd_displ_front,fwd_displ_rear;
+ std::list<std::string> events;
+ int event_id=0;
+ bool end=false;
+
+ events.push_back(segway->finish_thread_event_id);
+ events.push_back(segway->segways[front_segway].get_new_status_event());
+ events.push_back(segway->segways[rear_segway].get_new_status_event());
+ while(!end)
+ {
+ event_id=segway->event_server->wait_first(events);
+ segway->status_access.enter();
+ try{
+ if(event_id==0)
+ end=true;
+ else if(event_id==1)// front status
+ {
+ status_received[front_segway]=true;
+ segway->status.rmp200[front_segway]=segway->segways[front_segway].get_status();
+ }
+ else if(event_id==2)// rear status
+ {
+ status_received[rear_segway]=true;
+ segway->status.rmp200[rear_segway]=segway->segways[rear_segway].get_status();
+ }
+ if(status_received[front_segway] && status_received[rear_segway])
+ {
+ /* compute the forward displacement */
+ fwd_displ_front=(segway->segways[front_segway].get_right_wheel_displacement()+segway->segways[front_segway].get_left_wheel_displacement())/2;
+ fwd_displ_rear=(segway->segways[rear_segway].get_right_wheel_displacement()+segway->segways[rear_segway].get_left_wheel_displacement())/2;
+ segway->forward_displacement=(fwd_displ_front+fwd_displ_rear)/2;
+ /* compute yaw displacement*/
+ segway->yaw_displacement=(segway->segways[front_segway].get_yaw_displacement()+segway->segways[rear_segway].get_yaw_displacement())/2;
+ /* compute yaw rate */
+ segway->yaw_rate=(segway->segways[front_segway].get_yaw_rate()+segway->segways[rear_segway].get_yaw_rate())/2;
+ /* activate event */
+ if(!segway->event_server->event_is_set(segway->new_status_event_id))
+ segway->event_server->set_event(segway->new_status_event_id);
+ status_received[front_segway]=false;
+ status_received[rear_segway]=false;
+ }
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+ segway->status_access.exit();
+ }
pthread_exit(NULL);
}
@@ -136,25 +185,31 @@ std::ostream & operator<< (std::ostream& data, CSegwayRMP400 & segway)
TSegwayRMP400Status CSegwayRMP400::get_status()
{
- return this->status;
+ TSegwayRMP400Status status_tmp;
+
+ if(this->connected)
+ {
+ this->status_access.enter();
+ status_tmp=this->status;
+ this->status_access.exit();
+
+ return status_tmp;
+ }
+ else
+ throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
}
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.
+ double fwd_disp;
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;
+ this->status_access.enter();
+ fwd_disp=this->forward_displacement;
+ this->status_access.exit();
- return this->forward_displacement;
+ return fwd_disp;
}
else
throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
@@ -162,11 +217,15 @@ double CSegwayRMP400::get_forward_displacement()
double CSegwayRMP400::get_yaw_displacement()
{
+ double yaw_disp;
+
if(this->connected)
{
- this->yaw_displacement=(this->segways[front_segway].get_yaw_displacement()+this->segways[rear_segway].get_yaw_displacement())/2;
+ this->status_access.enter();
+ yaw_disp=this->yaw_displacement;
+ this->status_access.exit();
- return this->yaw_displacement;
+ return yaw_disp;
}
else
throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");
@@ -174,11 +233,15 @@ double CSegwayRMP400::get_yaw_displacement()
double CSegwayRMP400::get_yaw_rate()
{
+ double yaw_rate_tmp;
+
if(this->connected)
{
- this->yaw_rate=(this->segways[front_segway].get_yaw_rate()+this->segways[rear_segway].get_yaw_rate())/2;
+ this->status_access.enter();
+ yaw_rate_tmp=this->yaw_rate;
+ this->status_access.exit();
- return this->yaw_rate;
+ return yaw_rate_tmp;
}
else
throw CSegwayRMP400Exception(_HERE_,"The driver is not connected","");