#include "robolaus.h"

const unsigned byte suchfarbe = BOJE_ROT;

const double e = 2.71828183;
const double pi = 3.14159265;
const int v_max = 99;
const int v_min = 7;

const int linie = 3;
const int feld = 4;
const byte undef = 0;
const byte frei = 1;
const byte wand = 2;

const int halbfeld = 175; // 140

const byte durchlaufrichtung = RECHTS; // hat nichts mit der Startwand zu tun

#define ende 9
#define fahre_r 10
#define fahre_w 11
#define k_rechts 12
#define k_links 13
#define nichts 14
#define pre_k_links 15
#define pre_k_rechts 16
#define may_k_links 17
#define may_sackgasse_links 18
#define goto_test_boje 19
#define test_boje 20
#define turn_boje_r 21
#define turn_boje_l 22
#define boje_r_1 23
#define boje_r_2 24
#define boje_r_3 25
#define boje_r_4 26
#define boje_r_pause 27
#define boje_g_1 28
#define boje_g_2 29
#define boje_g_pause 30
#define boje_l_1 31
#define boje_l_2 32
#define boje_l_3 33
#define boje_l_4 34
#define boje_l_pause 35
#define entkomme 36
#define entkomme_r 37

#define boje_oben 0
#define boje_rechts 1
#define boje_unten 2
#define boje_links 3
#define boje_keine 4
#define boje_unbekannt 5

#define entdecke 100
#define heimweg 101

//Variable die während der gesamten Ausfühttp://start.ubuntu.com/8.10/hrung vorhanden ist (<- static),
//und außerdem durch den Compiler nicht wegoptimiert werden darf,
//da sie in einer Interruptroutine verändert wird (<- volatile)
static volatile byte wand_suchen;

static byte zustand;
static byte status;

static volatile int striche;

static int boden_counter_a[2];
static int boden_counter_b[2];
static int boden[2];

static unsigned byte soll_links;
static unsigned byte soll_rechts;

static unsigned byte abgebogen;

static int max_incs;
static int abbiege_incs;

static byte ausrichtung; // blickrichtung des roboters 0 = oben dann im uhzeigersinn mod 4
static byte position_x; // Position in x-Richtung; #Felder
static byte position_y; // Position in y-Richtung; 0,0 = Start

static byte karte[8][8]; // dummyfelder außen herum

static unsigned byte bojenzustand_alt;
static byte test_boje_links;
static byte test_boje_gerade;
static byte test_boje_rechts;
static byte zustand_nach_bojentest;

//####################//
// Runden auf Integer // 
//####################//
int runde(double in)
{
	if (in < 0)
	{
		in = in - 0.5;
	}
	else
	{
		in = in + 0.5;
	}
	return ((int)in);
}
//##################################################################################//
// berechnet die Inkremente um sich einen bestimmten Wikel auf der stelle zu drehen //
// die beiden Räder werden entgegengesetzt angesteuert, beide fahren gleich lang    //
//##################################################################################//
int winkel(int grad)
{	// r = 7,6 rad = 2,5
	return runde(((((double)grad) / 360.0) * 1277.0)); // 1277
}

//########################################################################//
// berechnet die Inkremente um angegebene Strecke in Millimeter zu fahren //
//########################################################################//
int millimeter(int milli)
{	// d = 50
	return runde((((double)milli) * 2.6738));
}

//###############################################################################//
// berechnet aud der angegebenen Geschwindigkeit die Ansteuerung für die Motoren //
//###############################################################################//
int m_ansteuer(int geschwindigkeit)
{
	double v;

	v = pow((double)e, (((double)(geschwindigkeit + 15)) / 25.0)) + 5.0;
	if (v > 100)
	{
		return 100;
	}
	else
	{
		if (v < 7)
		{
			return 7;
		}
		else
		{
			return (int)v;
		}
	}
}

//###############################################//
// Absatand messen mit dem eingestellten Winkel  //
// Umrerchnung in cm - keine weitere Bearbeitung //
//###############################################//
double abstand(unsigned byte welcher)
{
	double ausgabe;
	int messwert;

	messwert = auge_get(welcher);
	if (welcher == LINKS)
	{
		if (messwert < 34)
		{
			ausgabe = ((log(((double)messwert) / 83.272)) / -0.045); // 83,272 e ^ (-0.045 x)
		}
		else
		{
			if (messwert < 70)
			{
				ausgabe = pow((((double)messwert) / 793.44), -0.9506); // Umkehr -1.052 // von 10 bis 20 cm
			}
			else
			{
				ausgabe = pow((((double)messwert) / 484.33), -1.1933); // Umkehr -0.838 // von 5 bis 10 cm
			}
		}
	}
	else	// rechts
	{
		ausgabe = pow((((double)messwert) / 921.15), -1.233); // Umkehr -0.811 // von 10 bis 60 cm
	}
	return ausgabe;
}

//#######################################################################//
// Regelung um den berechneten Geradeaus- oder Kuvenfehler auszugleichen //
//#######################################################################//
void regle(double faktor_links, double faktor_rechts, byte delta)
{
// für gerade: links = außen

	int links;
	int rechts;

	byte v_links;
	byte v_rechts;

	links = runde(faktor_links * ((double)weggeber_get(LINKS)));
	rechts = runde(faktor_rechts * ((double)weggeber_get(RECHTS)));

	if (delta > 0)
	{
		links = links + delta * delta;
	}
	else
	{
		rechts = rechts + delta * delta;
	}
	if (links != rechts)
	{
		if (links < rechts) // rechts, innen schneller - links, außen langsamer
		{
			v_links = v_max;
			v_rechts = max((v_max - rechts + links), v_min);
		}
		else // rechts langsamer links schneller
		{
			v_rechts = v_max;
			v_links = max((v_max - links + rechts), v_min);
		}
		if (faktor_links < 0)
		{
			motor_set(LINKS, -m_ansteuer(v_links));
		}
		else
		{
			motor_set(LINKS, m_ansteuer(v_links));
		}
		if (faktor_rechts < 0)
		{
			motor_set(RECHTS, -m_ansteuer(v_rechts));
		}
		else
		{
			motor_set(RECHTS, m_ansteuer(v_rechts));
		}
	}
	
}

//###################################//
// Geradeausfaht mittels Wandabstand //
//###################################//
void gerade_w(byte delta)
{
	regle(0.0, 0.0, delta);
}

//######################################//
// Geradeausfaht mittels Radinkrementen //
//######################################//

void gerade_r(void)
{
	regle(1.0, 1.0, 0);
}

//#######################################//
// Rückwärtsfahrt mittels Radincrementen //
//#######################################//
void zurueck(void)
{
	regle(-1.0, -1.0, 0);
}

//##################################################//
// Sterung um eine 90°-Kurve in der Mitte zu fahren //
//##################################################//
void l_kurve(void)
{
	regle(-1.0, 1.0, 0);
}
void r_kurve(void)
{
	regle(1.0, -1.0, 0);
}

//######################################//
// Roboter auf der Stelle warten lassen //
//######################################//
void stehe(void)
{
	motor_set(LINKS|RECHTS, 0);
}

//##################################################################//
// Linien auf dem Boden ekennen, um eigene Position zu lokalisieren //
// als Status - Linie nicht mehrfach erkennn - nur Wechsel wichtig  //
//##################################################################//
byte bodenerkennung(unsigned byte seite)
{
	int helligkeit;
	byte welcher;

	if (seite == LINKS)
	{
		welcher = 0;
	}
	else
	{
		welcher = 1;
	}

	helligkeit = bodensensor_get(seite); // Helligkeitsdifferenz 30 derzeit 50
	if ((helligkeit > 150) && (boden[welcher] == feld)) // misst schwarz hat blau als status
	{
		boden_counter_a[welcher] = boden_counter_a[welcher] + 1;
		if (boden_counter_a[welcher] > 3)
		{
			boden[welcher] = linie; // befindet sich auf Linie
			boden_counter_a[welcher] = 0;
			led_set(LED_ROT, TRUE);
			return TRUE;
		}
	}
	else
	{
		boden_counter_a[welcher] = 0; //
		led_set(LED_ROT, FALSE);
	}
	if ((helligkeit < 50) && (boden[welcher] == linie)) // misst blau hat schwarz als status
	{
		boden_counter_b[welcher] = boden_counter_b[welcher] + 1;
		if (boden_counter_b[welcher] > 3)
		{
			boden[welcher] = feld; // befindet sich auf einem Feld
			boden_counter_b[welcher] = 0;
		}
	}
	else
	{
		boden_counter_b[welcher] = 0;
	}
	return FALSE;
}

//##############################################################//
// Messwert mit Schwellwert vergleichen, bei zu weit keine Wand //
//##############################################################//
void is_wand(unsigned byte welcher, unsigned byte messwert, byte* wandcounter)
{
	if (welcher == LINKS)
	{
		if (messwert < ((double)soll_rechts / 3.0 * 2.0))
		{
			(*wandcounter) = (*wandcounter) + 1;
		}
		else
		{
			(*wandcounter) = 0;
		}
	}
	else
	{
		if (messwert < ((double)soll_links / 3.0 * 2.0))
		{
			(*wandcounter) = (*wandcounter) + 1;
		}
		else
		{
			(*wandcounter) = 0;
		}
	}
	if ((*wandcounter) > 5)
	{
		(*wandcounter) = 5;
	}
}

//#########################################################################//
// bei jedem Überlauf des Timer-Zählers erfolgt ein Aufruf dieser Funktion //
//#########################################################################//
void timerfunc(void)
{
	wand_suchen = 1;
}

//#######################################################################################//
// ermittelt den Status eines Nachbarfeldes, mit Richtung wird angegeben welcher Nachbar //
//#######################################################################################//
byte get_feld(byte richtung)
{
	byte x;
	byte y;

	switch ((ausrichtung + richtung) % 4)
	{
		case 0:
			x = position_x;
			y = position_y + 1;
			break;
		case 1:
			x = position_x + 1;
			y = position_y;
			break;
		case 2:
			x = position_x;
			y = position_y - 1;
			break;
		case 3:
			x = position_x - 1;
			y = position_y;
			break;
	}
	x = x % 8; // karte per modulo - kein fehlzugriff möglich
	y = y % 8;
	return (karte[x][y]);
}

//##############################################################//
// aktualisiert die Position des Roboters ja nach Blickrichtung //
//##############################################################//
void actualize_position(void)
{
	switch (status)
	{
		case entdecke:
			karte[position_x][position_y] = 1;// feld wird als betreten markiert
			break;
		case heimweg:
			karte[position_x][position_y] = 2;
			break;
	}
	switch (ausrichtung)
	{
		case 0: // nach oben
			position_y = position_y + 1;
			break;
		case 1:	// nach rechts
			position_x = position_x + 1;
			break;
		case 2:	// nach unten
			position_y = position_y - 1;
			break;
		case 3:	// nach links
			position_x = position_x - 1;
			break;
	}
	position_x = position_x % 8; // modulo wegen karte
	position_y = position_y % 8;
}

//##################################################################################//
// das fahren in eine eventuell erkannte Abzweigung veranlassen oder gerader weiter //
//##################################################################################//
void linienaktion(byte is_linie, byte wandcounter_r, byte wandcounter_l)
{

	unsigned byte boje;

	if (is_linie) // Linie wird erkannt
	{
		boje = boje_get();
		actualize_position();
		abgebogen = 100;
		if ((wandcounter_r == 5) && ((0 == get_feld(1)) || ((1 == get_feld(1)) && (status == heimweg)))) // rechts Lücke gefunden, nicht gesperrt
		{
			if (get_feld(1) == 0) // nächstes feld noch nicht betreten => entdecken
			{
				status = entdecke;
			}
			zustand = pre_k_rechts; // rechts abbiegen
			max_incs = millimeter(halbfeld);
			weggeber_set(LINKS|RECHTS, 0);
		}
		else
		{
			if ((abstand(RECHTS) > 35.0) && ((0 == get_feld(0)) || ((1 == get_feld(0)) && (status == heimweg)))) // gerade aus kein feld
			{
				zustand = fahre_w;
				if (get_feld(0) == 0) // nächstes feld noch nicht betreten => entdecke
				{
					status = entdecke;
				}
			}
			else
			{
				zustand = may_k_links; // mögl links abbiegen
				max_incs = millimeter(halbfeld);
				weggeber_set(LINKS|RECHTS, 0);
			}
		}
		if (boje)
		{
			if (boje != suchfarbe)
			{
				led_set(LED_GRUEN, TRUE);
				bojenzustand_alt = boje; // alten zustand der boje merken für test auf veränderung
				switch (zustand)
				{
					case fahre_w:
						zustand_nach_bojentest = fahre_w;
						test_boje_links = 1;
						test_boje_gerade = 0;
						if (wandcounter_r == 5)
						{ // abzweigung erkannt
							test_boje_rechts = 0;
						}
						else
						{
							test_boje_rechts = 1;
						}
						break;
					case pre_k_rechts:
						zustand_nach_bojentest = k_rechts;
						test_boje_links = 1;
						if (abstand(RECHTS) > 35.0)
						{ // vorm Roboter ein freies Feld
							test_boje_gerade = 0;
						}
						else
						{
							test_boje_gerade = 1;
						}
						test_boje_rechts = 0;
						break;
					case may_k_links:
						zustand_nach_bojentest = may_sackgasse_links;
						test_boje_links = 1;
						if (abstand(RECHTS) > 35.0)
						{ // vorm Roboter ein freies Feld
							test_boje_gerade = 0;
						}
						else
						{
							test_boje_gerade = 1;
						}
						if (wandcounter_r == 5)
						{ // abzweigung erkannt
							test_boje_rechts = 0;
						}
						else
						{
							test_boje_rechts = 1;
						}
						break;
				}
				zustand = goto_test_boje;
				max_incs = millimeter(halbfeld);
			}
		}
		weggeber_set(LINKS|RECHTS, 0);
	}
}

//###################################################//
// aktualisiert die ausrichtung +1 = 90° nach rechts //
//###################################################//
void actualize_richtung(byte new)
{
	ausrichtung = ((ausrichtung + new) % 4); 
}

//##################//
// löscht die Karte //
//##################//
void reset_karte(void)
{
	byte i;
	byte j;

	for (i = 0; i < 8; i++) // karte leeren
		{
			for (j = 0; j < 8; j++)
			{
				karte[i][j] = 0;
			}
		}
}

int main(void)
{
	static unsigned byte online;
	static byte is_linie_r;
	static byte is_linie_l;
	static volatile byte wandcounter_r;
	static volatile byte wandcounter_l;

	static byte v_links;
	static byte v_rechts;

	static unsigned byte messwert;

	int delta;
	int d_winkel;
	int i;
	int j;
	unsigned byte boje;

	byte zaehler;

	//Roboter Initialisieren
	roboter_init();

	// Startwerte setzen
	v_links = v_max;
	v_rechts = v_max;
	zaehler = 0;
	boden_counter_a[0] = 0; // oft genug messen
	boden_counter_b[0] = 0;
	boden[0] = feld;
	boden_counter_a[1] = 0; // oft genug messen
	boden_counter_b[1] = 0;
	boden[1] = feld;
	d_winkel = 0;
	online = 0;
	wandcounter_r = 0; // oft genug messen
	wandcounter_l = 0;
	striche = 0;
	wand_suchen = 1;
	max_incs = 0;
	position_x = 0; //starten immer in 0,0 egal wo er ist, karte läuft per modulo
	position_y = 0;
	reset_karte();
	abgebogen = 100;

	zustand = fahre_r; // am start soll er gerade aus fahren
	status = entdecke; // entdeckt das laby

	auge_winkel_set(LINKS|RECHTS, -70);
	warte(1000);
	led_set(LED_GRUEN, TRUE);
	taster_warte(TASTER_GRUEN);
	led_set(LED_GRUEN, FALSE);
	led_set(LED_ROT, TRUE);
	warte(1000);
	soll_links = auge_get(LINKS);
	soll_rechts = auge_get(RECHTS);;
	warte(500);
	led_set(LED_ROT, FALSE);

	// auf den Taster Warten
	led_set(LED_GRUEN, TRUE);
	taster_warte(TASTER_GRUEN);
	ausrichtung = 0; // wird immer auf null gesetzt egal wie er steht
	auge_winkel_set(durchlaufrichtung, 0);
	led_set(LED_GRUEN, FALSE);
	warte(500);

	motor_set(LINKS|RECHTS, 100);

	lcd_leeren();

	warte(200); // motor erstmal ein bissel anfahren lassen
	
	timer_starten(TIMER_TEILER_1024, timerfunc);

	while (zustand != ende)
	{
	 if (wand_suchen == 1) // Messungen um eine Wand zu finden ausführen? Scheduler schaltet das an
	 {
	  messwert = auge_get(LINKS);
	  is_wand(RECHTS, messwert, &wandcounter_r); // rechter Sensor misst über Kreuz linke Wand
	  if ((zustand == fahre_w) && (wandcounter_r > 0))
	  {
	   zustand = fahre_r;
	  }
	  wand_suchen = 0; // wieder ausschalten bis zum nächsten mal
	 }
		if (stossstange_get() & BUMPER_ALLE)
		{
			led_set(LED_GELB, TRUE);
			switch (zustand) // welche Regelung für welchen fahrttyp nutzen
			{
	  			case fahre_w:
					max_incs = winkel(170);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case fahre_r:
					max_incs = winkel(170);
					if ((abgebogen == RECHTS) | (abgebogen == LINKS))
					{
						abbiege_incs = weggeber_get(LINKS);
					}
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case may_k_links:
					max_incs = winkel(170);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case may_sackgasse_links:
					max_incs = max_incs + winkel(85);
					break;
				case pre_k_rechts:
					max_incs = winkel(170);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case pre_k_links:
					max_incs = winkel(170);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case k_rechts:
					max_incs = max_incs + winkel(85);
					zustand = entkomme_r;
					break;
				case k_links:
					max_incs = max_incs + winkel(85);
					break;
				case goto_test_boje:
					max_incs = winkel(170);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case turn_boje_l:
					max_incs = max_incs + winkel(85);
					break;
				case turn_boje_r:
					max_incs = max_incs + winkel(85);
					zustand = entkomme_r;
					break;
				case test_boje:
					max_incs = max_incs + winkel(85);
					break;
				case boje_r_1:
					max_incs = max_incs + winkel(85);
					zustand = entkomme_r;
					break;
				/*case boje_r_3:
					max_incs = winkel(85);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case boje_r_4:
					max_incs = max_incs + winkel(85);
					break;
				case boje_r_pause:	
					max_incs = winkel(85);
					weggeber_set(LINKS|RECHTS, 0);
					break;*/
				/*case boje_g_2:
					max_incs = winkel(170);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case boje_g_pause:	
					max_incs = winkel(170);
					weggeber_set(LINKS|RECHTS, 0);
					break;*/
				case boje_l_1:
					max_incs = max_incs + winkel(85);
					break;
				/*case boje_l_3:
					max_incs = winkel(85);
					weggeber_set(LINKS|RECHTS, 0);
					break;
				case boje_l_4:
					max_incs = max_incs + winkel(85);
					zustand = entkomme_r;
					break;
				case boje_l_pause:
					max_incs = winkel(85);
					weggeber_set(LINKS|RECHTS, 0);
					break;*/
			}
			if (zustand != entkomme_r)
			{
				zustand = entkomme;
			}
			reset_karte();
		}
		is_linie_r = bodenerkennung(RECHTS);
		is_linie_l = bodenerkennung(LINKS); // Linie oder nicht?
		linienaktion(((is_linie_l || is_linie_r) && (!online)), wandcounter_r, wandcounter_l);
		if ((online) && (is_linie_r ^ is_linie_l))
		{
			delta = weggeber_get(RECHTS);
			d_winkel = runde(((atan(((double)delta) / 217.0)) / pi) * 180.0); // winkel abweichung zur optimalen fahrtrichtung
			if (is_linie_r) // weicht nach rechts ab -> neg Winkel
			{
				d_winkel = -d_winkel;
			}
		}
		if (is_linie_l && is_linie_r)
		{
			d_winkel = 0;
		}
		online = online ^ (is_linie_r ^ is_linie_l);

		if ((max_incs != 0) && ((abs(weggeber_get(LINKS)) >= max_incs) || (abs(weggeber_get(RECHTS)) >= max_incs)))
		{
			switch (zustand)
			{
				case entkomme:
					zustand = fahre_r;
					max_incs = 0;
					if ((abgebogen == LINKS) | (abgebogen == RECHTS))
					{
						max_incs = abbiege_incs;
						if (abgebogen == LINKS)
						{
							zustand = pre_k_rechts;
						}
						else
						{
							zustand = pre_k_links;
						}
						abgebogen = 100;
					}
					break;
				case entkomme_r:
					zustand = fahre_r;
					max_incs = 0;
					if ((abgebogen == LINKS) | (abgebogen == RECHTS))
					{
						max_incs = abbiege_incs;
						if (abgebogen == LINKS)
						{
							zustand = pre_k_rechts;
						}
						else
						{
							zustand = pre_k_links;
						}
						abgebogen = 100;
					}
					break;
				case may_k_links:
					zustand = may_sackgasse_links;
					max_incs = winkel(90 - d_winkel);
					break;
				case may_sackgasse_links: // durchlauf = rechts
					actualize_richtung(-1);
					if ((abstand(RECHTS) > 20) && ((0 == get_feld(0)) || ((1 == get_feld(0)) && (status == heimweg))))
					{ // links abbiegen, da das die rechtsmöglichste abbiegung ist
						abgebogen = LINKS;
						zustand = fahre_r;
						max_incs = 0;
						if (get_feld(0) == 0) // nächstes feld noch nicht betreten => entdecke
						{
							status = entdecke;
						}
					}
					else
					{ // sackgasse
						zustand = k_links;
						max_incs = winkel(90);
						if (2 != get_feld(3))
						{
							status = heimweg; // rueckweg aus einer Sackgasse
						}
						else
						{
							status = entdecke;
							reset_karte();
						}
					}
					break;
				case pre_k_links:
					zustand = k_links;
					max_incs = winkel(90 - d_winkel);
					break;
				case pre_k_rechts:
					zustand = k_rechts;
					max_incs = winkel(90 + d_winkel - 5);
					break;
				case k_links:
					abgebogen = LINKS;
					zustand = fahre_r;
					actualize_richtung(-1);
					max_incs = 0;
					break;
				case k_rechts:
					abgebogen = RECHTS;
					zustand = fahre_r;
					actualize_richtung(+1);
					max_incs = 0;
					break;
				case goto_test_boje:
					if (d_winkel != 0)
					{
						if (d_winkel < 0)
						{
							d_winkel = -d_winkel + 2; // +0	
							zustand = turn_boje_r;
						}
						else
						{
							zustand = turn_boje_l;
						}
						max_incs = winkel(d_winkel);
					}
					else
					{
						zustand = test_boje;
					}
					break;
				case turn_boje_r:
					zustand = test_boje;
					break;
				case turn_boje_l:
					zustand = test_boje;
					break;
				case boje_r_1:
					zustand = boje_r_2;
					break;
				case boje_r_3:
					boje = boje_get();
					if ((suchfarbe == boje) || (bojenzustand_alt == boje) || (boje == BOJE_KEINE))
					{ // richtige Farbe oder keine Veränderung -> keine Boje
						test_boje_rechts = 0;
						if (boje && (bojenzustand_alt != boje))
						{	//boje gefunden -> andere wände nicht mehr absuchen
							test_boje_links = 0;
							test_boje_gerade = 0;
						}
						zustand = boje_r_4;
						max_incs = winkel(92);
					}
					else
					{
						zustand = boje_r_pause;
					}
					break;
				case boje_r_4:
					zustand = test_boje;
					break;
				case boje_g_2:
					boje = boje_get();
					if ((suchfarbe == boje) || (bojenzustand_alt == boje) || (boje == BOJE_KEINE))
					{ // richtige Farbe oder keine Veränderung -> keine Boje
						test_boje_gerade = 0;
						if (boje && (bojenzustand_alt != boje))
						{	//boje gefunden -> andere wände nicht mehr absuchen
							test_boje_links = 0;
							test_boje_rechts = 0;
						}
						zustand = test_boje;
					}
					else
					{
						zustand = boje_g_pause;
					}
					break;
				case boje_l_1:
					if (abstand(RECHTS) > 20)
					{ // keine Wand -> keine Boje
						test_boje_links = 0;
						if (zustand_nach_bojentest == may_sackgasse_links)
						{
							actualize_richtung(-1);
							zustand = fahre_w;
							max_incs = 0;
						}
						else
						{
							max_incs = winkel(85); // 90
							zustand = boje_l_4;
						}
					}
					else
					{
						zustand = boje_l_2;
					}
					break;
				case boje_l_3:
					boje = boje_get();
					if ((suchfarbe == boje) || (bojenzustand_alt == boje) || (boje == BOJE_KEINE))
					{ // richtige Farbe oder keine Veränderung -> keine Boje
						test_boje_links = 0;
						if (boje && (bojenzustand_alt != boje))
						{	//boje gefunden -> andere wände nicht mehr absuchen
							test_boje_rechts = 0;
							test_boje_gerade = 0;
						}
						if (zustand_nach_bojentest == may_sackgasse_links)
						{
							zustand = k_links;
							actualize_richtung(-1);
							max_incs = winkel(75); // 90
							weggeber_set(LINKS|RECHTS, 0);
							status = heimweg;
						}
						else
						{
							zustand = boje_l_4;
							max_incs = winkel(85); // 95
						}
					}
					else
					{
						zustand = boje_l_pause;
					}
					break;
				case boje_l_4:
					zustand = test_boje;
					break;
			}
			weggeber_set(LINKS|RECHTS, 0);
			d_winkel = 0;
		}
lcd_position_set(2,1);
lcd_ausgabe("%i - %i ", position_x, position_y);
		switch (zustand) // welcher Regelung für welchen fahrttyp nutzen
		{
			case entkomme:
				l_kurve();
				break;
			case entkomme_r:
				r_kurve();
				break;
	  		case fahre_w:
				delta = messwert - soll_links;
				if (delta > 0)
				{
					delta = delta * 2;
				}
				gerade_w(delta);
				break;
			case fahre_r:
				gerade_r();
				break;
			case may_k_links:
				gerade_r();
				break;
			case may_sackgasse_links:
				l_kurve();
				break;
			case pre_k_rechts:
				gerade_r();
				break;
			case pre_k_links:
				gerade_r();
				break;
			case k_rechts:
				r_kurve();
				break;
			case k_links:
				l_kurve();
				break;
			case goto_test_boje:
				gerade_r();
				break;
			case turn_boje_l:
				l_kurve();
				break;
			case turn_boje_r:
				r_kurve();
				break;
			case test_boje:
				stehe();
				if (test_boje_gerade == 1)
				{
					zustand = boje_g_1;
				}
				else
				{
					if (test_boje_rechts == 1)
					{
						zustand = boje_r_1;
						max_incs = winkel(90);
						weggeber_set(LINKS|RECHTS, 0);
					}
					else
					{	
						if (test_boje_links == 1)
						{
							zustand = boje_l_1;
							max_incs = winkel(90);
							weggeber_set(LINKS|RECHTS, 0);
						}
						else
						{
							zustand = zustand_nach_bojentest;
							if (zustand == fahre_w)
							{
								max_incs = 0;
							}
							else
							{
								max_incs = winkel(90);
								weggeber_set(LINKS|RECHTS, 0);
							}
						}
					}
				}
				break;
			case boje_r_1:
				r_kurve();
				break;
			case boje_r_2:
				if ((stossstange_get() & BUMPER_VL) || (stossstange_get() & BUMPER_VR))
				{
					zustand = boje_r_3;
					max_incs = millimeter(35);
					weggeber_set(LINKS|RECHTS, 0);
				}
				else
				{
					gerade_r();
				}
				break;
			case boje_r_3:
				zurueck();
				break;
			case boje_r_4:
				l_kurve();
				break;
			case boje_r_pause:	
				if (zaehler >= 50)
				{
					zustand = boje_r_2;
					zaehler = 0;
				}
				else
				{
					zaehler = zaehler + 1;
					warte(10);
					stehe();
				}
				break;
			case boje_g_1:
				if ((stossstange_get() & BUMPER_VL) || (stossstange_get() & BUMPER_VR))
				{
					zustand = boje_g_2;
					max_incs = millimeter(35);
					weggeber_set(LINKS|RECHTS, 0);
				}
				else
				{
					gerade_r();
				}
				break;
			case boje_g_2:
				zurueck();
				break;
			case boje_g_pause:	
				if (zaehler >= 50)
				{
					zustand = boje_g_1;
					zaehler = 0;
				}
				else
				{
					zaehler = zaehler + 1;
					warte(10);
					stehe();
				}
				break;
			case boje_l_1:
				l_kurve();
				break;
			case boje_l_2:
				if ((stossstange_get() & BUMPER_VL) || (stossstange_get() & BUMPER_VR))
				{
					zustand = boje_l_3;
					max_incs = millimeter(35);
					weggeber_set(LINKS|RECHTS, 0);
				}
				else
				{
					gerade_r();
				}
				break;
			case boje_l_3:
				zurueck();
				break;
			case boje_l_4:
				r_kurve();
				break;
			case boje_l_pause:
				if (zaehler >= 50)
				{
					zustand = boje_l_2;
					zaehler = 0;
				}
				else
				{
					zaehler = zaehler + 1;
					warte(10);
					stehe();
				}
				break;
		}
	}

	timer_stoppen();

	motor_set(LINKS|RECHTS, 0);	

    return 0;
}