Create LineFollow

LineFollow

@@ -0,0 +1,197 @@
+#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( "LineFollow" )
+	{
+	}
+
+	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;
+double phi_absolute = 0;
+unsigned int sequence = 0;
+
+
+int digital_inputs[8] = {0,0,0,0,0,0,0,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;
+	}
+};
+
+
+
+
+class MyDigitalInputArray: public DigitalInputArray{
+
+    void valuesChangedEvent(const int *value, unsigned int size){
+
+        for (int i=0; i<size; i++){
+            digital_inputs[i] = value[i];
+        }    
+        cout << *value;
+    } 
+};
+
+
+MyCom com;
+MyBumper bumper;
+OmniDrive omniDrive;
+MyOdometry odo;
+MyDigitalInputArray mydia;
+
+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);
+	
+
+	while( com.isConnected() && false == bumper.value() && _run )
+	{
+
+	rec::robotino::api2::msleep(100); 
+    
+    
+    DigitalInput();
+
+    if (0 == digital_inputs[0]){
+        omniDrive.setVelocity(0.2,0,0);
+    }
+    if (1 == digital_inputs[0]){
+        omniDrive.setVelocity(0,0,0);
+    }
+
+
+	com.processEvents();
+    rec::robotino::api2::msleep(100); 
+	}
+}
+
+
+
+
+void destroy()
+{
+	com.disconnectFromServer();
+}
+
+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
+		destroy();
+	}
+	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
+}
+