Commit 3dbb6ab9 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented the main internal thread to handle the reception of new status...

Implemented the main internal thread to handle the reception of new status messages from both platforms and make the necessary computations.
Modified the example to use the new driver structure.
parent 9ed4648c
#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;
}
......@@ -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","");
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment