Create TrafficQuadratDG

TrafficQuadratDG

@@ -0,0 +1,292 @@
+#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);
+			for (int i=0; i<75; i++){
+				for (int i=0; i<8; i++){
+					myout.setOutputNumber(i);
+					myout.setValue(false);
+				}
+				myout.setOutputNumber(7);
+				myout.setValue(true);
+				omniDrive.setVelocity(0,0,0);
+				msleep(50);
+			}
+			d = 1;
+		}
+	}
+
+	if (1 == d){
+		for (int i=0; i<8; i++){
+			myout.setOutputNumber(i);
+			myout.setValue(false);
+		}
+
+		myout.setOutputNumber(5);
+		myout.setValue(true);
+		omniDrive.setVelocity(0, 0.2, 0);
+		msleep(50);
+		
+		if (0.5 <= y_distance){
+			odo.set(0,0,0,-1);
+			for (int i=0; i<75; i++){
+					for (int i=0; i<8; i++){
+					myout.setOutputNumber(i);
+					myout.setValue(false);
+				}
+				omniDrive.setVelocity(0,0,0);
+				myout.setOutputNumber(7);
+				myout.setValue(true);
+				msleep(50);
+			}
+			d = 2;
+		}
+	}
+	if (2 == d){
+		for (int i=0; i<8; i++){
+			myout.setOutputNumber(i);
+			myout.setValue(false);
+		}
+
+		myout.setOutputNumber(5);
+		myout.setValue(true);
+		omniDrive.setVelocity(-0.2, 0, 0);
+		msleep(50);
+		
+		if (-0.5 >= x_distance){
+			odo.set(0,0,0,-1);
+			for (int i=0; i<75; i++){
+				for (int i=0; i<8; i++){
+					myout.setOutputNumber(i);
+					myout.setValue(false);
+					}
+				myout.setOutputNumber(7);
+				myout.setValue(true);
+				omniDrive.setVelocity(0,0,0);
+				msleep(50);
+			}
+			d = 3;
+		}
+	}
+	if (3 == d){
+		for (int i=0; i<8; i++){
+			myout.setOutputNumber(i);
+			myout.setValue(false);
+		}
+
+		myout.setOutputNumber(5);
+		myout.setValue(true);
+		omniDrive.setVelocity(0, -0.2, 0);
+		msleep(50);
+		
+		if (-0.5 >= y_distance){
+
+			odo.set(0,0,0,-1);
+			for (int i=0; i<75; i++){
+					for (int i=0; i<8; i++){
+					myout.setOutputNumber(i);
+					myout.setValue(false);
+				}
+				myout.setOutputNumber(7);
+				myout.setValue(true);
+				omniDrive.setVelocity(0,0,0);
+				msleep(50);
+			}
+			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
+}