Robotino/TrafficQuadrat

252 lines
4.0 KiB
Plaintext
Raw Normal View History

2023-01-30 11:37:42 +01:00
#include <iostream>
#include <stdlib.h>
#include "rec/robotino/api2/all.h"
#include <iostream>
using namespace rec::robotino::api2;
using namespace std;
bool _run = true;
class MyCom : public Com
{
public:
MyCom()
: Com( "TrafficLight" )
{
}
void errorEvent( const char* errorString )
{
std::cerr << "Error: " << errorString << std::endl;
}
void connectedEvent()
{
std::cout << "Connected." << std::endl;
}
void connectionClosedEvent()
{
std::cout << "Connection closed." << std::endl;
}
void logEvent( const char* message, int level )
{
std::cout << message << std::endl;
}
};
class MyBumper : public Bumper
{
public:
MyBumper()
: bumped( false )
{
}
void bumperEvent( bool hasContact )
{
bumped |= hasContact;
if (true == bumped){
std::cout << "Bumper has " << ( hasContact ? "contact" : "no contact") << std::endl;
}
}
bool bumped;
};
double x_distance = 0;
double y_distance = 0;
int phi_absolute = 0;
unsigned int sequence = 0;
class MyOdometry: public Odometry{
void readingsEvent(double x,double y, double phi, float vx, float vy, float omega, unsigned int seq ){
cout << "X: " << x << " Y: " << y << " Richtung: " << phi << " Sequenz: " << seq << endl;
x_distance = x;
y_distance = y;
phi_absolute = phi;
sequence = seq;
}
};
MyCom com;
MyBumper bumper;
OmniDrive omniDrive;
MyOdometry odo;
DigitalOutput myout;
void init( const std::string& hostname )
{
// Connect
std::cout << "Connecting...";
com.setAddress( hostname.c_str() );
com.connectToServer( true );
if( false == com.isConnected() )
{
std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
rec::robotino::api2::shutdown();
exit( 1 );
}
else
{
std::cout << "success" << std::endl;
}
}
void drive()
{
odo.set(0,0,0,-1);
int d = 0;
while( com.isConnected() && false == bumper.value() && _run )
{
for (int i=0; i<8; i++){
myout.setOutputNumber(i);
myout.setValue(false);
}
rec::robotino::api2::msleep(100);
if (0 == d){
for (int i=0; i<8; i++){
myout.setOutputNumber(i);
myout.setValue(false);
}
msleep(50);
myout.setOutputNumber(5);
myout.setValue(true);
omniDrive.setVelocity(0.2, 0, 0);
if (0.5 <= x_distance){
odo.set(0,0,0,-1);
d = 1;
}
}
if (1 == d){
for (int i=0; i<8; i++){
myout.setOutputNumber(i);
myout.setValue(false);
}
myout.setOutputNumber(6);
myout.setValue(true);
omniDrive.setVelocity(0, 0.2, 0);
msleep(50);
if (0.5 <= y_distance){
odo.set(0,0,0,-1);
d = 2;
}
}
if (2 == d){
for (int i=0; i<8; i++){
myout.setOutputNumber(i);
myout.setValue(false);
}
myout.setOutputNumber(7);
myout.setValue(true);
omniDrive.setVelocity(-0.2, 0, 0);
msleep(50);
if (-0.5 >= x_distance){
odo.set(0,0,0,-1);
d = 3;
}
}
if (3 == d){
for (int i=0; i<8; i++){
myout.setOutputNumber(i);
myout.setValue(false);
}
myout.setOutputNumber(7);
myout.setValue(true);
omniDrive.setVelocity(0, -0.2, 0);
msleep(50);
if (-0.5 >= y_distance){
odo.set(0,0,0,-1);
d = 0;
}
}
com.processEvents();
rec::robotino::api2::msleep(100);
}
}
int main( int argc, char **argv )
{
std::string hostname = "192.168.100.174";
if( argc > 1 )
{
hostname = argv[1];
}
//main program routine starts in here
try
{
//connect to your robotino (or try at least)
init( hostname );
//drive (this contains a while loop)
drive();
//exit
for (int i=0; i<8; i++){
myout.setOutputNumber(i);
myout.setValue(false);
}
com.disconnectFromServer();
}
catch( const rec::robotino::api2::RobotinoException& e )
{
std::cerr << "Com Error: " << e.what() << std::endl;
}
catch( const std::exception& e )
{
std::cerr << "Error: " << e.what() << std::endl;
}
catch( ... )
{
std::cerr << "Unknow Error" << std::endl;
}
rec::robotino::api2::shutdown();
#ifdef WIN32
std::cout << "Press any key to exit..." << std::endl;
rec::robotino::api2::waitForKey();
#endif
}