while (true)
	{
		// Test Fahrt zum Rechteck
		cout << "Speed: " << speed << endl;		
        brake();
        curr_heading = compass->get_heading();
        // ToDo Wert des $head_val anpassen
        // Der Wert des Headings sollte um $head_val varieren, um eine Wendung zu erkennen
        while(abs(heading - curr_heading) < 90) {
            cout << "Turning left..." << endl << "Curr_Heading: " << curr_heading << endl << "heading: " << heading << endl;
            turn_left();
            //Kleines delay um die Drehung zu ermoeglichen
            delay(20);
            curr_heading = compass->get_heading();
        }
        heading = curr_heading;
        //Vorwaerts
        forward();
	}