Durch einen Umgebungsscan die neue Zielrichtung finden

**WarChild** · 18.03.2008, 15:24

Hallo,

Ich habe endlich eine Funktion fertig gestellt, die das Umfeld des RP6 mit Hilfe eines SRF02 und dem Kompassmodul CMPS03 misst und in einem Array speichert. Um Zeiger zu vermeiden habe ich einfach ein Globales Array definiert.

Momentan wird die Distanz zur Umgebung in 36 Abschnitten gemessen.
Ich zögere es in 360 Schritten zu messen, da ich den ATmega 32 nicht überfordern will. Hat jemand Erfahrung damit 16bit Arrays mit 360 Gliedern zu verarbeiten? Schafft er das problemlos, oder ist das schon zu viel für ihn? Ein solches Array hat ja schon 720 bytes, wenn ich dann noch für die weitere verabreitung ähnliche Arrays brauche, dann kommt man schnell an die 2-3 kbytes.

Leider gibt es messfelher, wenn der RP6 im flachen Winkel die Distanz zu einigermaßen glatten Oberflächen messen will.
Also z.B. wenn der RP6 im 30° Winkel zur 20cm entfernten Wand steht misst er 400cm.

Was jetzt noch fehlt ist eine Funktion, die mir zuverlässig sagt in welcher Richtung am meißten platz ist, bzw. z.B. bei einem Flur mit knicken in welcher Richtung der Flur weiter geht.
Logischer weise sollten die Messfehler nicht in frage kommen. Also sollte man vlt. mehrere werte nebeneinander aufsummieren, sodass die Messfehler etwas geglättet werden. Ich bin auf der Suche nach einer Relation der Signifikanz von Distanz und Breite.
d.H. Was ist besser vier Messwerte a 2m oder einer mit 4m? Der Einzelne könnte auch ein Messfehler sein, aber was, wenn es eine offene Tür ist, durch die der RP6 den Raum verlassen kann?

Momentan will ich die Summe von je drei nebeneinanderliegenden Messwerten bilden und die Richtung mit der größten Summe wird die neue Fahrtrichtung.
Ich Lösungen, die den Mittelwert der Messwerte mit inbetracht ziehen, wären vlt. auch nicht verkehrt.

Vielleicht hat ja jemand von euch noch kreative Ideen, wie man den Besten Weg finden kann...

hier der Code zum o.g. Text:

Code:

// Includes:

#include "RP6RobotBaseLib.h" 	

#include "RP6I2CmasterTWI.h"

#define CMPS03				0xC0
#define	SRF02				0xE0		
#define attachdistance  	8


uint16_t 	area[36] = {0};

uint16_t distance_SRF02_blocking(void) //Distanzmessung
{
	uint8_t srfbuffer[2];						//Speicher für die übertragenen Bytes

	I2CTWI_transmit2Bytes(SRF02, 0, 81); 	// SRF02 bekommt im Register 0 den Befehl in cm zu messen
	mSleep(65);

	I2CTWI_transmitByte(SRF02, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(SRF02, srfbuffer, 2); 	// und in dem Array speichern

	return srfbuffer[0] * 256 + srfbuffer[1]-attachdistance;
}


uint16_t direction_CMPS03(void) //Richtungsbestimmung
{
	uint8_t cmpsbuffer[2];

	I2CTWI_transmitByte(CMPS03, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(CMPS03, cmpsbuffer, 2); 	// und in dem Array speichern

	return ((cmpsbuffer[0] * 256) + cmpsbuffer[1]); 
}

void allroundscan(void)
{

	uint16_t index = direction_CMPS03() / 100;		// den Index and der aktuellen Richtung starten lassen
	uint16_t counter = 0;
	uint16_t direction = 0;
	uint16_t distance = 0;
	
	writeString_P("\n\tAreascan initiated\n*******************************************\n");
	
	stop();
	changeDirection(RIGHT);
	moveAtSpeed(40,40);


	while(counter < 36)
	{
		task_RP6System();
		direction = direction_CMPS03() / 100;
		
		// Drehrichtung bei übersprungenen Messungen korregieren (funktioniert nicht über den Nullsprung!!!) ausbaufähig
		if (direction > index + 1 && index > 2)
			changeDirection(LEFT);
		if (direction < index -2 && index >= 2)
			changeDirection(RIGHT);
		
		// Messwert abspeichern
		if (direction == index)
		{
			distance = distance_SRF02_blocking();
			if (distance >= 1000)
				area[index] = 0;
			else
				area[index] = distance;
			
			writeString_P("Areascan Value [");
			writeInteger(index*10,DEC);
			writeString_P("]\t=");
			writeInteger(area[index],DEC);
			writeString_P("\n");
			
			changeDirection(RIGHT);
			index++;
			counter++;
		}
		
		// Nullsprung
		if (index >= 36)
			index = 0;
	}
	stop();
	writeString_P("\tAreascan completed\n*******************************************\n");
}

uint16_t direction_calculator(void)
{
	uint16_t maximumsum = 0;
	uint16_t newdirection = 0;
	uint8_t i = 0;
	
	for(i=0;i<36;i++)
	{
		if(i==0)
		{
			if(area[35]+area[i]+area[i+1] > maximumsum)
			{
				maximumsum = area[35]+area[i]+area[i+1];
				newdirection = i;
			}
		}
		else
		{
			if(area[i-1]+area[i]+area[i+1] > maximumsum)
			{
				maximumsum = area[i-1]+area[i]+area[i+1];
				newdirection = i;
			}
		}
		writeString_P("\nMaximum Summe ="); writeInteger(maximumsum,DEC); writeString_P("\t New Direction ="); writeInteger(newdirection*10,DEC);
	}
	return newdirection*100;
}


/*****************************************************************************/
// Main


int main(void)
{
	initRobotBase(); 
	powerON();
	setLEDs(0b111111);
	mSleep(1000);
	setLEDs(0b001001);

	I2CTWI_initMaster(100);			// I2C speed set to 100kHz

	// Main loop
	while(true) 
	{
	      task_RP6System();
	      allroundscan();
	}
	
	return 0;
}

**Dirk** · 18.03.2008, 16:55

Hallo WarChild,

gute Idee, das mit dem Allroundscan!

Was mir bei dem Prog noch auffällt ist diese Zeile, die eigentlich so gar nicht funktionieren dürfte:
if (direction > index + 1 && index > 2)
Da würde ich unbedingt draus machen:
if ((direction > (index + 1)) && (index > 2))

Das gleiche für die andere if-Zeile.

Gruß Dirk

**WarChild** · 18.03.2008, 16:58

Hi nochmal,

Ich habe das program jetzt in mein normales Fahrtprogramm implementiert. Es besteht im kern ähnlich wie das Move 5 Beispiel, jedoch dreht er sich beim ausweichen immer in Richtung der von mir vorgegebenen Himmelsrichtung. Und wenn er "Cruised", dann korregiert er seine Fahrtrichtung entsprechend der Sollrichtung.

Diese Sollrichtung lasse ich nun, wenn der Roboter in einer Ecke festhängt, in der es nicht weiter in Richtung der Sollrichtung geht von der o.g. Funktion neu berechnen.

Aber leider gibt der SRF02 gerade bei engeren Umgebungen häfig falsche Werte zurück, sodass der RP6 manchmal drei bis vier Umgebundsscans braucht, bis er den Ausgang aus seiner Ecke findet.
Kenn jemand von euch einen zuverlässigeren Sensor? Einer der unter jedem Umstand die richtige Distanz misst? Bei Senkrechten messungen ist der SRF02 wunderbar, aber bei schrägen hört es auf. Laser können keine spiegelnden Flächen wahrnemen und IR ist nicht wirklich zur Distanzmessung >1m geeignet.

Aber ich habe gerade eine Idee...

Man könnte ja auch die Sensoren kombinieren. Messwerte, bei denen gleichzeitig das ACS anspring, werden automatisch mit 0 belegt. dadurch kommen diese Himmelsrichtungen nicht mehr in frage...

mfg WarChild

**radbruch** · 18.03.2008, 17:05

Hallo

Wegen der besseren Lesbarkeit würde ich auch Klammern verwenden, aber sie sind nicht nötig:

Code:

Rangfolge der Operatoren

Die folgende Tabelle listet die Operatoren von C und C++ und ihre Assoziativität (d.h. die Bindung der Operanden) in absteigender Rangfolge auf. 
Operatoren, die in einem (durch Linien abgetrennten) Bereich der Tabelle stehen, haben dieselbe Rangstufe. 


Symbol  Name/Bedeutung                      Assoziativität
-----------------------------------------------------------------
++	Erhöhung nach Auswertung            von links nach rechts
--      Erniedrigung nach Auswertung	
( )     Funktionsaufruf	
[ ]     Arrayelement	
->      Zeiger auf Strukturfeld	
.       Felder einer Struktur oder Union	
-----------------------------------------------------------------
++      Erhöhung vor Auswertung             von rechts nach links
--      Erniedrigung vor Auswertung	
:>      Segmentbasis	
!       logisches NOT	
~       bitweises NOT	
-       unäres Minus	
+       unäres Plus	
&       Adresse von	
*       Indirektion	
sizeof  Größe in Bytes	
new     Belegen von Speicherplatz	
delete  Freigabe von Speicherplatz	
(type)  Typumwandlung (cast)	
-----------------------------------------------------------------
.*_     Zeiger auf Felder (Objekte)         von links nach rechts
->*     Zeiger auf Felder (Zeiger)	
-----------------------------------------------------------------
*       Multiplikation                      von links nach rechts
/       Division	
%       Divisionsrest	
-----------------------------------------------------------------
+       Addition                            von links nach rechts
-       Subtraktion	
-----------------------------------------------------------------
<<      bitweises Linksschieben             von links nach rechts
>>      bitweises Rechtsschieben	
-----------------------------------------------------------------
<       kleiner als                         von links nach rechts
<=      kleiner oder gleich	
>       größer als	
>=      größer oder gleich	
-----------------------------------------------------------------
==      gleich                              von links nach rechts
!=      ungleich	
-----------------------------------------------------------------
&       bitweises AND                       von links nach rechts
-----------------------------------------------------------------
^       bitweises exklusives OR             von links nach rechts
-----------------------------------------------------------------
|       bitweises OR                        von links nach rechts
-----------------------------------------------------------------
&&      logisches AND                       von links nach rechts
-----------------------------------------------------------------
||      logisches OR                        von links nach rechts
-----------------------------------------------------------------
? :     Bedingung                           von rechts nach links
-----------------------------------------------------------------
=       Zuweisung                           von rechts nach links
*=	zusammengesetzte Zuweisung
/=
%=
+=
-=
<<=
>>=
&=
^=
|=	
-----------------------------------------------------------------
,       Komma-Operator                      von links nach rechts
-----------------------------------------------------------------

Quelle: http://www.imb-jena.de/~gmueller/kur.../c_operat.html
(ohne Wertung des Inhalts, es war die erste Fundstelle der Suchmaschine)

Gruß

mic

**Dirk** · 18.03.2008, 18:13

... würde ich auch Klammern verwenden, aber sie sind nicht nötig

Stimmt hier offensichtlich. Hätte ich aber nie so hingeschrieben.
Peinlich, peinlich!

Sorry WarChild, danke radbruch!

Gruß Dirk

**WarChild** · 18.03.2008, 20:06

Ich hatte jetzt noch eine Idee, um die Messfehler zu umgehen...

Wenn das ACS auslöst, kommt der Messwert dieser Himmelsrichtung ja logischerweise nicht mehr als neue Fahrtrichtung in Frage. Also werden alle Messwerte während das ACS auslöst mit 0 belegt.

demnach wird nur im "freien Sichtfeld" die Distanz gemessen und verwertet.

Ich hänge mal den gesammten Programmcode ran. Falls jemand auch einen SRF02 und einen CMPS03 hat, kann er das ja mal ausprobieren oder modifizieren.

Code:

// includes and definitions:

#include "RP6RobotBaseLib.h" 	

#include "RP6I2CmasterTWI.h"

#define CMPS03				0xC0
#define	SRF02				0xE0		
#define attachdistance  	8

uint16_t 	desdirection = 2700;
uint8_t 	obstacle_count = 0;
uint16_t 	area[36] = {0};
// funktions:

uint16_t distance_SRF02(void) //Distanzmessung mit Stopwatches
{
	static uint8_t measureactive = false;		//Messungsstatus
	uint8_t srfbuffer[2];						//Speicher für die übertragenen Bytes
	uint16_t distance;
	
	if (measureactive == false)
	{
		setStopwatch6(0);
		startStopwatch6();
		I2CTWI_transmit2Bytes(SRF02, 0, 81); 	// SRF02 bekommt im Register 0 den Befehl in cm zu messen
		measureactive = true;
	}

	if (getStopwatch6() >= 70 && measureactive == true)
	{
		I2CTWI_transmitByte(SRF02, 2); 			// Ab dem zweiten Reigster Daten anfordern
		I2CTWI_readBytes(SRF02, srfbuffer, 2); 	// und in dem Array speichern
		distance = srfbuffer[0] * 256 + srfbuffer[1]-attachdistance; 
		measureactive = false;
		stopStopwatch6();
		
		// hiermit werden Messfehler korregiert, da negative Werte sonst bei anderen funktionen zu Problemen führen			
		if (distance == -attachdistance)			
			distance = 0;			

		 //Anzeige der Distanz auf den LEDs
			statusLEDs.LED1=0;
			statusLEDs.LED2=0;
			statusLEDs.LED3=0;
			statusLEDs.LED4=0;
			statusLEDs.LED5=0;
			statusLEDs.LED6=0;	

			if (((distance % 100)/25 >= 1) || (distance >= 400))		// cm werden auf der einen Seite dargestellt
			statusLEDs.LED1=1;
			if (((distance % 100)/25 >= 2) || (distance >= 400))
			statusLEDs.LED2=1;
			if (((distance % 100)/25 >= 3) || (distance >= 400))
			statusLEDs.LED3=1;											// m auf der anderen
			if (distance / 100 >= 1)
			statusLEDs.LED4=1;
			if (distance / 100 >= 2)
			statusLEDs.LED5=1;
			if (distance / 100 >= 3)
			statusLEDs.LED6=1;

			updateStatusLEDs();
	}
return distance;
}


uint16_t distance_SRF02_blocking(void) //Distanzmessung
{
	uint8_t srfbuffer[2];						//Speicher für die übertragenen Bytes

	I2CTWI_transmit2Bytes(SRF02, 0, 81); 	// SRF02 bekommt im Register 0 den Befehl in cm zu messen
	mSleep(65);

	I2CTWI_transmitByte(SRF02, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(SRF02, srfbuffer, 2); 	// und in dem Array speichern

	return srfbuffer[0] * 256 + srfbuffer[1]-attachdistance;
}

uint16_t direction_CMPS03(void) //Richtungsbestimmung
{
	uint8_t cmpsbuffer[2];

	I2CTWI_transmitByte(CMPS03, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(CMPS03, cmpsbuffer, 2); 	// und in dem Array speichern

	return ((cmpsbuffer[0] * 256) + cmpsbuffer[1]); 
}


void allroundscan(void)
{

	uint16_t index = direction_CMPS03() / 100;		// den Index and der aktuellen Richtung starten lassen
	uint16_t counter = 0;
	uint16_t direction = 0;
	uint16_t distance = 0;
	
	writeString_P("\n\tAreascan initiated\n*******************************************\n");
	
	stop();
	changeDirection(RIGHT);
	moveAtSpeed(40,40);


	while(counter < 36)
	{
		task_RP6System();
		direction = direction_CMPS03() / 100;
		
		// Drehrichtung bei übersprungenen Messungen korregieren (funktioniert nicht über den Nullsprung!!!) ausbaufähig
		if ((direction > (index + 1)) && (index > 2))
			changeDirection(LEFT);
		if ((direction < (index -2)) && (index >= 2))
			changeDirection(RIGHT);
		
		// Messwert abspeichern
		if (direction == index)
		{
			distance = distance_SRF02_blocking();
			if (distance >= 1000 || obstacle_left || obstacle_right)	//10m oder ein vom ACS entdecktes Objekt schließt diese Richtung aus.
				area[index] = 0;
			else
				area[index] = distance;
			
			writeString_P("Areascan Value [");
			writeInteger(index*10,DEC);
			writeString_P("]\t=");
			writeInteger(area[index],DEC);
			writeString_P("\n");
			
			changeDirection(RIGHT);
			index++;
			counter++;
		}
		
		// Nullsprung
		if (index >= 36)
			index = 0;
	}
	stop();
	writeString_P("\tAreascan completed\n*******************************************\n");
}

uint16_t direction_calculator(void)
{
	uint16_t maximumsum = 0;
	uint16_t newdirection = 0;
	uint8_t i = 0;
	
	for(i=0;i<36;i++)
	{
		if(i==0)
		{
			if(area[35]+area[i]+area[i+1] > maximumsum)
			{
				maximumsum = area[35]+area[i]+area[i+1];
				newdirection = i;
			}
		}
		else
		{
			if(area[i-1]+area[i]+area[i+1] > maximumsum)
			{
				maximumsum = area[i-1]+area[i]+area[i+1];
				newdirection = i;
			}
		}
		writeString_P("\nMaximum Summe ="); writeInteger(maximumsum,DEC); writeString_P("\t New Direction ="); writeInteger(newdirection*10,DEC);
	}
	return newdirection*100;
}

// movements :

#define	STOP		0
#define	FORWARD		1
#define	LEFT		2
#define RIGHT		3
#define HARDLEFT	4
#define	HARDRIGHT	5
#define TURNLEFT	6
#define	TURNRIGHT	7
#define TURN		8
#define UTURN		9
#define BACKWARD	10
#define OVERRIDE	11

uint8_t movement = STOP;

//cruise

#define	READY		0
#define CRUISING	1
#define CORRECTING	2

uint8_t cruise_state = READY;

//avoid

#define FREE		0
#define AVOID		1
#define ROTATE		2

uint8_t avoid_state = FREE;

//escape

#define	FREE		0
#define OBSTACLE	1
#define BACKWARD	10
#define ROTATE		2

uint8_t escape_state = FREE;

//scan
#define READY		0
#define INIT		1
#define	SCANNING1	2
#define	SCANNING2	3
#define	COMPLETE	4

uint8_t scan_state = READY;


void cruise(void)
{
	if (avoid_state == FREE && escape_state == FREE)
	{
		if (cruise_state == READY)
		{
			cruise_state = CRUISING;
			movement = FORWARD;
			setStopwatch5(0);
			startStopwatch5();
			setLEDs(0b001001);
		}
		
		uint16_t direction = direction_CMPS03();
		
		if ((cruise_state == CRUISING || cruise_state == CORRECTING) && getStopwatch5() >= 2000)
		{
			setLEDs(0b000000);
			
			if (direction > desdirection + 30)
			{
				cruise_state = CORRECTING;
				
				if (direction - desdirection > 3450 && direction - desdirection <= 3600 && (movement != HARDRIGHT || movement != TURNRIGHT))
					movement = RIGHT;
				if (direction - desdirection > 2700 && direction - desdirection <= 3450 && (movement != TURNRIGHT))
					movement = HARDRIGHT;
				if (direction - desdirection > 1800 && direction - desdirection <= 2700)
					movement = TURNRIGHT;
				if (direction - desdirection > 900 && direction - desdirection <= 1800)
					movement = TURNLEFT;
				if (direction - desdirection > 150 && direction - desdirection <= 900 && (movement != TURNLEFT))
					movement = HARDLEFT;
				if (direction - desdirection > 0 && direction - desdirection <= 150 && (movement != HARDLEFT || movement != TURNLEFT))
					movement = LEFT;
			}
			if (desdirection > direction + 30)
			{
				cruise_state = CORRECTING;
				
				if (desdirection - direction > 3450 && desdirection - direction <= 3600 && (movement != HARDLEFT || movement != TURNLEFT))
					movement = LEFT;
				if (desdirection - direction > 2700 && desdirection - direction <= 3450 && movement != TURNLEFT)
					movement = HARDLEFT;
				if (desdirection - direction > 1800 && desdirection - direction <= 2700)
					movement = TURNLEFT;
				if (desdirection - direction > 900 && desdirection - direction <= 1800)
					movement = TURNRIGHT;
				if (desdirection - direction > 150 && desdirection - direction <= 900 && movement != TURNRIGHT)
					movement = HARDRIGHT;
				if (desdirection - direction > 0 && desdirection - direction <= 150 && (movement != HARDRIGHT || movement != TURNRIGHT))
					movement = RIGHT;
			}
		}

		if (cruise_state == CORRECTING)
		{
			if (direction - desdirection >= -30 || direction - desdirection <= 30) // abweichung kleiner 3°
				cruise_state = READY;
		}
	}
}

void avoid(void)
{
	if (escape_state == FREE)
	{
		if (obstacle_left && obstacle_right && avoid_state != ROTATE)
		{
			avoid_state = ROTATE;
			movement = TURN;
			writeString_P("Obstacle Counter= ");
			++obstacle_count;
			writeInteger(obstacle_count,DEC);
			writeString_P("\n");
		}
		else if (obstacle_left)
		{
			avoid_state = AVOID;
			movement = HARDRIGHT;
		}
		else if (obstacle_right)
		{
			avoid_state = AVOID;
			movement = HARDLEFT;
		}
		if (avoid_state == AVOID && (!obstacle_left && !obstacle_right))
		{
			avoid_state = FREE;
			cruise_state = READY;
		}
	}
}

void escape(void)
{
	
	if (escape_state == FREE && (bumper_left || bumper_right))
	{
		stop();
		escape_state = OBSTACLE;
			writeString_P("Obstacle Counter= ");
			++obstacle_count;
			writeInteger(obstacle_count,DEC);
			writeString_P("\n");
	}
	if (escape_state == OBSTACLE)
	{
		if (bumper_left && bumper_right)
		{
			escape_state = BACKWARD;
			movement = TURN;

		}
		else if (bumper_left)
		{
			escape_state = BACKWARD;
			movement = TURNRIGHT;
		}
		else if (bumper_right)
		{
			escape_state = BACKWARD;
			movement = TURNLEFT;
		}
		else
			escape_state = FREE;
	}
}
uint8_t turndirection(void)
{
	uint16_t direction = direction_CMPS03();

	if (direction >= desdirection)
	{
		if (direction - desdirection >= 1800)
			return TURNRIGHT;
		else 
			return TURNLEFT;
	}
	if (desdirection > direction)
	{
		if (desdirection - direction >= 1800)
			return TURNLEFT;
		else
			return TURNRIGHT;
	}
	return STOP;
}


#define CRUISE		100
#define FAST		90
#define MEDIUM		60
#define SLOW		30

void movecontroller(void)
{
	if (obstacle_count >= 3)			// Funktionen zur Richtungsfindung
	{
		stop();
		movement = OVERRIDE;
		allroundscan();
		desdirection = direction_calculator();
		obstacle_count = 0;
		movement = STOP;
		cruise_state = CORRECTING ;
	}
	if (movement != OVERRIDE)			// Ausweichfunktionen
	{
		cruise();
		avoid();
		escape();
		changeDirection(FWD);
	}
	if (movement == TURN)				// Entscheidung welche Drehrichtung vorteilhafter ist
		movement = turndirection();
										// Ausführung der Gewünschten Fahrtbewegung
	if (escape_state == BACKWARD)
	{
		move(MEDIUM, BWD, DIST_MM(100), BLOCKING);
		escape_state = ROTATE;
	}
	if (movement == TURNRIGHT)
	{
		if (escape_state == ROTATE || avoid_state == ROTATE)
		{
			stop();
			rotate(MEDIUM, RIGHT, 90, BLOCKING);
			escape_state = FREE;
			avoid_state = FREE;
			cruise_state = READY;
		}
		else
		{
			changeDirection(RIGHT);
			moveAtSpeed(MEDIUM,MEDIUM);
		}
	}
	if (movement == TURNLEFT)
	{
		if (escape_state == ROTATE || avoid_state == ROTATE)
		{
			stop();
			rotate(MEDIUM, LEFT, 90, BLOCKING);
			escape_state = FREE;
			avoid_state = FREE;
			cruise_state = READY;
		}
		else
		{
			changeDirection(LEFT);
			moveAtSpeed(MEDIUM,MEDIUM);
		}
	}
	
	if (movement == UTURN)
	{
		stop();
		rotate(MEDIUM, RIGHT, 180, BLOCKING);
	}
	if (movement == FORWARD)
		moveAtSpeed(CRUISE,CRUISE);
	if (movement == HARDRIGHT)
		moveAtSpeed(FAST,SLOW);
	if (movement == HARDLEFT)
		moveAtSpeed(SLOW,FAST);
	if (movement == RIGHT)
		moveAtSpeed(FAST,MEDIUM);
	if (movement == LEFT)
		moveAtSpeed(MEDIUM,FAST);
	if (movement == STOP)
		stop();
	if (movement == BACKWARD)
	{
		changeDirection(BWD);
		moveAtSpeed(MEDIUM,MEDIUM);
	}

}


void acsStateChanged(void) // ACS zustand vorne anzeigen
{
	if(obstacle_left && obstacle_right)
		statusLEDs.byte = 0b100100;
	else
		statusLEDs.byte = 0b000000;
	statusLEDs.LED5 = obstacle_left;
	statusLEDs.LED4 = (!obstacle_left);
	statusLEDs.LED2 = obstacle_right;
	statusLEDs.LED1 = (!obstacle_right);
	updateStatusLEDs();
}

/*****************************************************************************/
// Main


int main(void)
{
	initRobotBase(); 
	powerON();
	setLEDs(0b111111);
	mSleep(1000);
	setLEDs(0b001001);
	
	setACSPwrMed();
	
	I2CTWI_initMaster(100);							// I2C speed set to 100kHz
	
	
	ACS_setStateChangedHandler(acsStateChanged);	// Set ACS state changed event handler:
	
	// Main loop
	while(true) 
	{
		task_RP6System();
		movecontroller();
	}
	return 0;
}

Es arbeitet soweit Einwandfrei, aber die Funktion direction_calculator(void) ist noch ausbaufähig.

**thietho** · 18.03.2008, 20:45

Sehr schön,
dies könnte ich in unserem Büro anwenden, um in bestimmte Räume zu gelangen!

**WarChild** · 22.04.2008, 15:28

So ich habe jetzt das Programm und ein Video fertig.
Ich benutze jetzt zusätzlich eine Relevanz-Maske, um den Umgebungsmesswerten noch einen Trend zu geben.
D.h. die Messwerte aus der Richtung aus der er kommt, bekommen eine niedrigere Relevanz als Werte in der alten Bewegungsrichtung, sodass er immer eher versucht voran als zurück zu kommen.

Außerdem lasse ich jetzt alle Messwerte auf dem LCD am RP6 Control Board ausgeben. Und der Roboter steht bis er durch drücken des Taster [4] gestartet wird.

http://www.youtube.com/v/wvs4u44gTlM
[flash width=425 height=350 loop=false:daca6f054d]http://www.youtube.com/v/wvs4u44gTlM[/flash:daca6f054d]

mfg WarChild

**WarChild** · 22.04.2008, 15:30

Hier der Code Dazu:
RP6 Base, Master:

Code:

// includes:

#include "RP6RobotBaseLib.h" 	

#include "RP6I2CmasterTWI.h"

// definitions:

#define 	CMPS03				0xC0
#define	SRF02				0xE0
#define 	DS1621_Write			0x9E
#define 	DS1621_Read			0x9F
#define 	I2C_RP6_Control_ADR		0x0A		
#define 	attachdistance  			8
	// movements
#define	STOP		0
#define	FORWARD		1
#define	LEFT		2
#define 	RIGHT		3
#define 	HARDLEFT		4
#define	HARDRIGHT	5
#define 	TURNLEFT		6
#define	TURNRIGHT	7
#define 	TURN		8
#define 	UTURN		9
#define 	BACKWARD	10
#define 	OVERRIDE		11
	// cruise states
#define	READY		0
#define 	CRUISING		1
#define 	CORRECTING	2
	// avoid states
#define 	FREE		0
#define 	AVOID		1
#define 	ROTATE		2
	// escape states
#define	FREE		0
#define 	OBSTACLE		1
#define 	BACKWARD	10
#define 	ROTATE		2
	// speeds
#define 	CRUISE		100
#define 	FAST		90
#define 	MEDIUM		60
#define 	SLOW		30

// global variables:

uint16_t 	area[36] = {0};			//memory for the surrounding area
uint8_t 	avoid_state = FREE;
uint8_t 	cruise_state = READY;
uint16_t 	desired_direction = 1800;		//general target direction
uint16_t	global_direction = 1800;
uint8_t 	escape_state = FREE;
uint8_t 	obstacle_count = 0;			//object counter
uint8_t		movement = STOP;			//current movement to do
uint8_t		move_enable = false;				//is he allowed to move

// funktion deklarations:

void 		acsStateChanged(void);
void 		adjustment(void);
void 		avoid(void);
void 		cruise(void);
uint16_t		distance_SRF02(void);
uint16_t		distance_SRF02_blocking(void);
uint16_t 		direction_calculator(void);
uint16_t 		direction_CMPS03(void);
void 		escape(void);
void		frontlight(void);
void		I2C_dataupload(void);
void 		movecontroller(void);
void		task_expansion(void);
uint16_t 		temperature_DS1621(void);
uint8_t 		turndirection(void);


// funktions:
void I2C_dataupload(void)
{
	if(!isStopwatch4Running())
	{
		setStopwatch4(0);
		startStopwatch4();
	}
	if(getStopwatch4()>1000)
	{	
		static uint8_t  uploadRegisters[17] = {0};
		static uint16_t temperature;
		static uint16_t direction = 0;
		static uint16_t distance = 0;
		
		temperature = temperature_DS1621();
		
		if (obstacle_count<3)
		{
			direction = direction_CMPS03();
			distance = distance_SRF02_blocking();
		}
		
		uploadRegisters[0] = 0;							//start Register
		uploadRegisters[1] = (uint8_t)(adcLSL);		//Low-Byte	(im Index des Empfängers ist alles um 1 versetzt)
		uploadRegisters[2] = (uint8_t)(adcLSL>>8);		//High-Byte
		uploadRegisters[3] = (uint8_t)(adcLSR);		//...
		uploadRegisters[4] = (uint8_t)(adcLSR>>8);
		uploadRegisters[5] = (uint8_t)(adcMotorCurrentLeft);
		uploadRegisters[6] = (uint8_t)(adcMotorCurrentLeft>>8);
		uploadRegisters[7] = (uint8_t)(adcMotorCurrentRight);
		uploadRegisters[8] = (uint8_t)(adcMotorCurrentRight>>8);
		uploadRegisters[9] = (uint8_t)(adcBat);
		uploadRegisters[10] = (uint8_t)(adcBat>>8);
		uploadRegisters[11] = (uint8_t)(temperature);		// 0 or 0.5°C
		uploadRegisters[12] = (uint8_t)(temperature>>8);	// whole degrees in C
		uploadRegisters[13] = (uint8_t)(distance);
		uploadRegisters[14] = (uint8_t)(distance>>8);
		uploadRegisters[15] = (uint8_t)(direction);		// 10*whole degrees
		uploadRegisters[16] = (uint8_t)(direction/256);

		
		I2CTWI_transmitBytes(I2C_RP6_Control_ADR,uploadRegisters,17);	//dataupload
		
		I2CTWI_transmitByte(I2C_RP6_Control_ADR,0);						//movedownload
		move_enable = I2CTWI_readByte(I2C_RP6_Control_ADR);
	
		setStopwatch4(0);
	}	
}
uint16_t temperature_DS1621(void) // gibt die 256 fache Umgebungstemperatur zurück
	{		
		uint8_t cmpsbuffer[2] = {0};
		I2CTWI_transmitByte(DS1621_Write, 0xEE);		// Befehl geben die Temperatur bereit zu stellen
		I2CTWI_transmitByte(DS1621_Write, 0xAA); 		// Ab dem 170. Register Daten anfordern
		I2CTWI_readBytes(DS1621_Read,cmpsbuffer,2); 	// und in dem Puffer Array speichern
		return (cmpsbuffer[0]-1) * 256 + cmpsbuffer[1]; //1° Temperaturanpassung
	}

uint16_t distance_SRF02(void) //Distanzmessung mit Stopwatches
{
	static uint8_t measureactive = false;		//Messungsstatus
	uint8_t srfbuffer[2];						//Speicher für die übertragenen Bytes
	static uint16_t distance = 0;
	
	if (measureactive == false)
	{
		setStopwatch6(0);
		startStopwatch6();
		I2CTWI_transmit2Bytes(SRF02, 0, 81); 	// SRF02 bekommt im Register 0 den Befehl in cm zu messen
		measureactive = true;
	}

	if (getStopwatch6() >= 70 && measureactive == true)
	{
		I2CTWI_transmitByte(SRF02, 2); 			// Ab dem zweiten Reigster Daten anfordern
		I2CTWI_readBytes(SRF02, srfbuffer, 2); 	// und in dem Array speichern
		distance = srfbuffer[0] * 256 + srfbuffer[1]-attachdistance; 
		measureactive = false;
		stopStopwatch6();
		
		// hiermit werden Messfehler korregiert, da negative Werte sonst bei anderen funktionen zu Problemen führen			
		if (distance == -attachdistance)			
			distance = 0;			

		 //Anzeige der Distanz auf den LEDs
			statusLEDs.LED1=0;
			statusLEDs.LED2=0;
			statusLEDs.LED3=0;
			statusLEDs.LED4=0;
			statusLEDs.LED5=0;
			statusLEDs.LED6=0;	

			if (((distance % 100)/25 >= 1) || (distance >= 400))		// cm werden auf der einen Seite dargestellt
			statusLEDs.LED1=1;
			if (((distance % 100)/25 >= 2) || (distance >= 400))
			statusLEDs.LED2=1;
			if (((distance % 100)/25 >= 3) || (distance >= 400))
			statusLEDs.LED3=1;											// m auf der anderen
			if (distance / 100 >= 1)
			statusLEDs.LED4=1;
			if (distance / 100 >= 2)
			statusLEDs.LED5=1;
			if (distance / 100 >= 3)
			statusLEDs.LED6=1;

			updateStatusLEDs();
	}
return distance;
}


uint16_t distance_SRF02_blocking(void) //Distanzmessung
{
	uint8_t srfbuffer[2];						//Speicher für die übertragenen Bytes

	I2CTWI_transmit2Bytes(SRF02, 0, 81); 	// SRF02 bekommt im Register 0 den Befehl in cm zu messen
	mSleep(65);

	I2CTWI_transmitByte(SRF02, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(SRF02, srfbuffer, 2); 	// und in dem Array speichern

	return srfbuffer[0] * 256 + srfbuffer[1]-attachdistance;
}

uint16_t direction_CMPS03(void) //Richtungsbestimmung
{
	uint8_t cmpsbuffer[2];

	I2CTWI_transmitByte(CMPS03, 2); 			// Ab dem zweiten Reigster Daten anfordern
	I2CTWI_readBytes(CMPS03, cmpsbuffer, 2); 	// und in dem Array speichern

	return ((cmpsbuffer[0] * 256) + cmpsbuffer[1]); 
}


void allroundscan(void)	// Umgebungsscan wird in area[] gespeichert
{
	uint16_t index = direction_CMPS03() / 100;		// den Index and der aktuellen Richtung starten lassen
	uint16_t counter = 0;
	uint16_t direction = 0;
	uint16_t distance = 0;
	
	writeString_P("\n\tAreascan initiated\n*******************************************\n");
	
	stop();
	changeDirection(RIGHT);
	moveAtSpeed(40,40);

	while(counter < 36)
	{
		task_RP6System();
		task_expansion();
		direction = direction_CMPS03() / 100;
		
		// Drehrichtung bei übersprungenen Messungen korregieren (funktioniert nicht über den Nullsprung!!!) ausbaufähig
		if ((direction > (index + 1)) && (index > 2))
			changeDirection(LEFT);
		if ((direction < (index -2)) && (index >= 2))
			changeDirection(RIGHT);
		
		// Messwert abspeichern
		if (direction == index)
		{
			distance = distance_SRF02_blocking();
			if (distance >= 1000 || obstacle_left || obstacle_right)	//10m oder ein vom ACS entdecktes Objekt schließt diese Richtung aus.
				area[index] = 0;
			else
				area[index] = distance;
			
			writeString_P("Areascan Value [");
			writeInteger(index*10,DEC);
			writeString_P("]\t=");
			writeInteger(area[index],DEC);
			writeString_P("\n");
			
			changeDirection(RIGHT);
			index++;
			counter++;
		}
		
		// Nullsprung
		if (index >= 36)
			index = 0;
	}
	stop();
	writeString_P("\tAreascan completed\n*******************************************\n");
}

uint16_t direction_calculator(void)	// berechnet die beste neue Richtung aus area[]
{
	uint16_t maximumsum = 0;
	uint16_t newdirection = 0;
	uint8_t i = 0;
	uint8_t count;
	if (desired_direction > 1800)
		count = (desired_direction - 1800) / 100;
	else
		count = (1800 - desired_direction) / 100;
	uint8_t relevance[36] = {0};

	for(i=0;i<36;i++)			//relevance calculation
	{
		if(i<=18)
			relevance[count] = i+1;
		if(i>18)
			relevance[count] = 36-i+1;
		
		count++;
		if (count>=36)
			count=0;
	}
	
	for(i=0;i<36;i++)			//measurand smoothing
	{
		if(i==0)
		{
			if(1.5*(area[35]+area[1])<area[0])
			{
				writeString_P("Measurand smoothed: area["); writeInteger(i,DEC);writeString_P("]=");writeInteger(area[i],DEC);
				area[i] = (area[35]+area[1])/2;
				writeString_P("-->"); writeInteger(area[i],DEC);writeString_P("\n");
			}
		}
		else
		{
			if(1.5*(area[i-1]+area[i+1])<area[i])
			{
				writeString_P("Measurand smoothed: area["); writeInteger(i,DEC);writeString_P("]=");writeInteger(area[i],DEC);
				area[i] = (area[i-1]+area[i+1])/2;
				writeString_P("-->"); writeInteger(area[i],DEC);writeString_P("\n");
			}
		}
	
	}
	
	for(i=0;i<36;i++)			//final direction calculation
	{
		if(i==0)
		{
			if(area[35]*relevance[35]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10 > maximumsum)
			{
				maximumsum = area[35]*relevance[35]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10;
				newdirection = i;
			}
		}
		else
		{
			if(area[i-1]*relevance[i-1]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10 > maximumsum)
			{
				maximumsum = area[i-1]*relevance[i-1]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10;
				newdirection = i;
			}
		}
		writeString_P("\nMaximum Sum ="); writeInteger(maximumsum,DEC); writeString_P("\t New Direction ="); writeInteger(newdirection*10,DEC);
	}
	writeString_P("\n\tDirection Calculation completed \n*******************************************\n");
	return newdirection*100;
}

void cruise(void)
{
	if (avoid_state == FREE && escape_state == FREE)
	{
		if (cruise_state == READY)
		{
			cruise_state = CRUISING;
			movement = FORWARD;
			setStopwatch5(0);
			startStopwatch5();
			statusLEDs.LED1 = true;
			statusLEDs.LED4 = true;
			updateStatusLEDs();
		}
		
		uint16_t direction = direction_CMPS03();
		
		if ((cruise_state == CRUISING || cruise_state == CORRECTING) && getStopwatch5() >= 1800)
		{
			if (direction > desired_direction + 30)
			{
				cruise_state = CORRECTING;
				statusLEDs.LED1 = false;
				statusLEDs.LED4 = false;
				updateStatusLEDs();
				
				if (direction - desired_direction > 3450 && direction - desired_direction <= 3600 && (movement != HARDRIGHT || movement != TURNRIGHT))
					movement = RIGHT;
				if (direction - desired_direction > 2700 && direction - desired_direction <= 3450 && (movement != TURNRIGHT))
					movement = HARDRIGHT;
				if (direction - desired_direction > 1800 && direction - desired_direction <= 2700)
					movement = TURNRIGHT;
				if (direction - desired_direction > 900 && direction - desired_direction <= 1800)
					movement = TURNLEFT;
				if (direction - desired_direction > 150 && direction - desired_direction <= 900 && (movement != TURNLEFT))
					movement = HARDLEFT;
				if (direction - desired_direction > 0 && direction - desired_direction <= 150 && (movement != HARDLEFT || movement != TURNLEFT))
					movement = LEFT;
			}
			if (desired_direction > direction + 30)
			{
				cruise_state = CORRECTING;
				statusLEDs.LED1 = false;
				statusLEDs.LED4 = false;
				updateStatusLEDs();
				
				if (desired_direction - direction > 3450 && desired_direction - direction <= 3600 && (movement != HARDLEFT || movement != TURNLEFT))
					movement = LEFT;
				if (desired_direction - direction > 2700 && desired_direction - direction <= 3450 && movement != TURNLEFT)
					movement = HARDLEFT;
				if (desired_direction - direction > 1800 && desired_direction - direction <= 2700)
					movement = TURNLEFT;
				if (desired_direction - direction > 900 && desired_direction - direction <= 1800)
					movement = TURNRIGHT;
				if (desired_direction - direction > 150 && desired_direction - direction <= 900 && movement != TURNRIGHT)
					movement = HARDRIGHT;
				if (desired_direction - direction > 0 && desired_direction - direction <= 150 && (movement != HARDRIGHT || movement != TURNRIGHT))
					movement = RIGHT;
			}
		}

		if (cruise_state == CORRECTING)
		{
			if (direction - desired_direction >= -30 || direction - desired_direction <= 30) // abweichung kleiner 3°
				cruise_state = READY;
		}
	}
}

void avoid(void)
{
	if (escape_state == FREE)
	{
		if (obstacle_left && obstacle_right && avoid_state != ROTATE)
		{
			avoid_state = ROTATE;
			movement = TURN;
			writeString_P("\nObstacle Counter= ");
			++obstacle_count;
			writeInteger(obstacle_count,DEC);
			writeString_P("\n");
		}
		else if (obstacle_left)
		{
			avoid_state = AVOID;
			movement = HARDRIGHT;
		}
		else if (obstacle_right)
		{
			avoid_state = AVOID;
			movement = HARDLEFT;
		}
		if (avoid_state == AVOID && (!obstacle_left && !obstacle_right))
		{
			avoid_state = FREE;
			cruise_state = READY;
		}
	}
}

void escape(void)
{
	if (escape_state == FREE && (bumper_left || bumper_right))
	{
		stop();
		escape_state = OBSTACLE;
			writeString_P("Obstacle Counter= ");
			++obstacle_count;
			writeInteger(obstacle_count,DEC);
			writeString_P("\n");
	}
	if (escape_state == OBSTACLE)
	{
		if (bumper_left && bumper_right)
		{
			escape_state = BACKWARD;
			movement = TURN;

		}
		else if (bumper_left)
		{
			escape_state = BACKWARD;
			movement = TURNRIGHT;
		}
		else if (bumper_right)
		{
			escape_state = BACKWARD;
			movement = TURNLEFT;
		}
		else
			escape_state = FREE;
	}
}
uint8_t turndirection(void) // bestimmt welche Drehrichtung eher in Sollrichtung liegt
{
	uint16_t direction = direction_CMPS03();

	if (direction >= desired_direction)
	{
		if (direction - desired_direction >= 1800)
			return TURNRIGHT;
		else 
			return TURNLEFT;
	}
	if (desired_direction > direction)
	{
		if (desired_direction - direction >= 1800)
			return TURNLEFT;
		else
			return TURNRIGHT;
	}
	return STOP;
}
void adjustment(void) // turns the robot to the desired direction on the shortest way
{
	stop();
	uint16_t direction = direction_CMPS03();
	
	while(direction / 100 != desired_direction / 100)
	{
		task_RP6System();
		task_expansion();
		direction = direction_CMPS03();
		moveAtSpeed(MEDIUM,MEDIUM);
		writeString_P("\nCurrent Direction\t");
		writeInteger(direction,DEC);
		writeString_P("\tDesired Direction\t");
		writeInteger(desired_direction,DEC);
		if (direction > desired_direction)
		{
			if (direction - desired_direction >= 1800)
				changeDirection(RIGHT);
			else 
				changeDirection(LEFT);
		}
		if (direction < desired_direction)
		{
			if (desired_direction - direction >= 1800)
				changeDirection(LEFT);
			else
				changeDirection(RIGHT);
		}
	}
	stop();
}
void movecontroller(void) // gets the movement whishes from other funktions and controlls the engines
{
	if (obstacle_count >= 3)			// Funktionen zur Richtungsfindung
	{
		stop();
		movement = OVERRIDE;
		allroundscan();
		desired_direction = direction_calculator();
		adjustment();
		obstacle_count = 0;
		movement = STOP;
	}
	if (movement != OVERRIDE)			// Ausweichfunktionen
	{
		cruise();
		avoid();
		escape();
		changeDirection(FWD);
	}
	if (movement == TURN)				// Entscheidung welche Drehrichtung vorteilhafter ist
		movement = turndirection();
										// Ausführung der Gewünschten Fahrtbewegung
	if (escape_state == BACKWARD)
	{
		move(MEDIUM, BWD, DIST_MM(100), BLOCKING);
		escape_state = ROTATE;
	}
	if (movement == TURNRIGHT)
	{
		if (escape_state == ROTATE || avoid_state == ROTATE)
		{
			stop();
			rotate(MEDIUM, RIGHT, 90, BLOCKING);
			escape_state = FREE;
			avoid_state = FREE;
			cruise_state = READY;
		}
		else
		{
			changeDirection(RIGHT);
			moveAtSpeed(MEDIUM,MEDIUM);
		}
	}
	if (movement == TURNLEFT)
	{
		if (escape_state == ROTATE || avoid_state == ROTATE)
		{
			stop();
			rotate(MEDIUM, LEFT, 90, BLOCKING);
			escape_state = FREE;
			avoid_state = FREE;
			cruise_state = READY;
		}
		else
		{
			changeDirection(LEFT);
			moveAtSpeed(MEDIUM,MEDIUM);
		}
	}
	
	if (movement == UTURN)
	{
		stop();
		rotate(MEDIUM, RIGHT, 180, BLOCKING);
	}
	if (movement == FORWARD)
		moveAtSpeed(CRUISE,CRUISE);
	if (movement == HARDRIGHT)
		moveAtSpeed(FAST,SLOW);
	if (movement == HARDLEFT)
		moveAtSpeed(SLOW,FAST);
	if (movement == RIGHT)
		moveAtSpeed(FAST,MEDIUM);
	if (movement == LEFT)
		moveAtSpeed(MEDIUM,FAST);
	if (movement == STOP)
		stop();
	if (movement == BACKWARD)
	{
		changeDirection(BWD);
		moveAtSpeed(MEDIUM,MEDIUM);
	}

}


void acsStateChanged(void) // displays the ACS state on the LEDs 2+3+5+6
{
	if (obstacle_left && obstacle_right)
	{
		statusLEDs.LED6 = true;
		statusLEDs.LED3 = true;
	}
	else
	{
		statusLEDs.LED6 = false;
		statusLEDs.LED3 = false;
	}
	statusLEDs.LED5 = obstacle_left;
	statusLEDs.LED2 = obstacle_right;
	updateStatusLEDs();
}
void task_expansion(void)	// runs background funktions while using infinite loops
{
	frontlight();
	I2C_dataupload();
}
void frontlight(void) // front diodes will be switched on in darkness
{
	  if (adcLSL<450)
	  {
		PORTA |= ADC0;
	  } 
	  if (adcLSL>680)
	  {
		PORTA &= ~ADC0;
	  }
	
	  if (adcLSR<450)
	  {
		PORTA |= ADC1;
	  } 
	  if (adcLSR>700)
	  {
		PORTA &= ~ADC1;
	  }
}
/*****************************************************************************/
// Main


int main(void)
{
	initRobotBase(); 
	powerON();
	setLEDs(0b111111);
	mSleep(500);
	setLEDs(0b000000);
	mSleep(500);
	setLEDs(0b111111);

	setACSPwrHigh();								// adjust ACS to Low Med or High depending on the circumstances 
	
	I2CTWI_initMaster(100);							// I2C speed set to 100kHz
	
	DDRA |= (ADC0 | ADC1); 						// definition of ADC0, ADC1 as output-channels
	
	ACS_setStateChangedHandler(acsStateChanged);	// Set ACS state changed event handler
	
	// Main loop
	while(true) 
	{

		task_RP6System();
		task_expansion();
		if(move_enable == true)
			movecontroller();
		else
			stop();
	}
	
	return 0;
}

**WarChild** · 22.04.2008, 15:31

RP6 Control, Slave:

Code:

// Includes:

#include "RP6ControlLib.h" 		// The RP6 Control Library. 
								// Always needs to be included!
#include "RP6I2CslaveTWI.h"
#define I2C_RP6_Control_ADR 	0x0A


/*****************************************************************************/

void LCD_data(void)
{
	static uint8_t menu = 0;
	static uint8_t move = false;
	// imaginary start values to see something on the LCD even if the I2C transfer doesn't work
	static uint16_t adcLSL = 500; 
	static uint16_t adcLSR = 500; 
	static uint16_t adcMotorCurrentLeft = 100; 
	static uint16_t adcMotorCurrentRight = 100; 
	static uint16_t adcBat = 750; 
	static uint16_t temperature = (20*256+128); 
	static uint16_t distance = 300; 
	static uint16_t direction = 1800; 
	
	
	if(getStopwatch2()>500)
	{
		
		if(getPressedKeyNumber() == 1)	// menu selection
		{
			menu = 1;
			sound(250,80,0);
			setStopwatch2(0);
		}
		else if(getPressedKeyNumber() == 2)
		{
			menu = 2;
			sound(250,80,0);
			setStopwatch2(0);
		}
		else if(getPressedKeyNumber() == 3)
		{
			menu = 3;
			sound(250,80,0);
			setStopwatch2(0);
		}
		else if(getPressedKeyNumber() == 4)
		{
			menu = 4;
			sound(250,80,0);
			setStopwatch2(0);
		}
		else if(getPressedKeyNumber() == 5)
		{
			setStopwatch2(0);
			if (move == false)
			{
				move = true;
				sound(180,80,25);
				sound(220,80,0);
			}
			else
			{
				move = false;
				sound(220,80,25);
				sound(180,80,0);
			}
		}
	}
	
	if(!I2CTWI_readBusy)
	{
		I2CTWI_readRegisters[0]=move;
	}
	
	if(!I2CTWI_writeBusy && I2CTWI_writeRegisters[0])
	{

		adcLSL = I2CTWI_writeRegisters[0] + I2CTWI_writeRegisters[1] * 256;
		adcLSR = I2CTWI_writeRegisters[2] + I2CTWI_writeRegisters[3] * 256;
		adcMotorCurrentLeft = I2CTWI_writeRegisters[4] + I2CTWI_writeRegisters[5] * 256;
		adcMotorCurrentRight = I2CTWI_writeRegisters[6] + I2CTWI_writeRegisters[7] * 256;
		adcBat = I2CTWI_writeRegisters[8] + I2CTWI_writeRegisters[9] * 256;
		temperature = I2CTWI_writeRegisters[10] + I2CTWI_writeRegisters[11] * 256;
		distance = I2CTWI_writeRegisters[12] + I2CTWI_writeRegisters[13] * 256;
		direction = I2CTWI_writeRegisters[14] + I2CTWI_writeRegisters[15] * 256;

		I2CTWI_writeRegisters[0] = 0;
		
		switch (menu)
		{
			case (1):
				showScreenLCD("UBat:       .  V", "IL:     IR:     ");
				setCursorPosLCD(2,11);
				writeIntegerLCD(adcBat/100,DEC);
				setCursorPosLCD(2,13);
				writeIntegerLengthLCD(adcBat,DEC,2);
				setCursorPosLCD(1,4);
				writeIntegerLCD(adcMotorCurrentLeft,DEC);
				setCursorPosLCD(1,12);
				writeIntegerLCD(adcMotorCurrentRight,DEC);
			break;
		
			case (2):
				showScreenLCD("AmbTemp:    . ßC", "LSL:    LSR:    ");
				setCursorPosLCD(2,10);
				writeIntegerLCD(temperature>>8,DEC);
				setCursorPosLCD(2,13);
				writeIntegerLCD(((uint8_t) temperature)/25.6,DEC);
				setCursorPosLCD(1,4);
				writeIntegerLCD(adcLSL,DEC);
				setCursorPosLCD(1,12);
				writeIntegerLCD(adcLSR,DEC);
			break;
		
			case (3):
				showScreenLCD("Distance:     cm", "Orient:      . ß");
				setCursorPosLCD(2,10);
				writeIntegerLCD(distance,DEC);
				setCursorPosLCD(1,10);
				writeIntegerLCD(direction/10,DEC);
				setCursorPosLCD(1,14);
				writeIntegerLengthLCD(direction,DEC,1);
			break;
		
			default:
				showScreenLCD("Press Key[1]-[3]", "To see SYS-INFOS");
			break;
		}
	}
}

void runningLight(void)
{
	static uint8_t runLight = 1; 
	static uint8_t dir;
	if(getStopwatch1() > 100) // 100ms
	{
		setLEDs(runLight); 

		if(dir == 0)
			runLight <<= 1; 
		else
			runLight >>= 1;

		if(runLight > 7 ) 
			dir = 1;			
		else if (runLight < 2 ) 
			dir = 0;

		setStopwatch1(0);
	}
}

/*****************************************************************************/
// Main function - The program starts here:

int main(void)
{
	initRP6Control(); // Always call this first! The Processor will not work
					  // correctly otherwise. 

	initLCD(); // Initialize the LC-Display (LCD)
			   // Always call this before using the LCD!

	I2CTWI_initSlave(I2C_RP6_Control_ADR);  // dem Slave die o.g. Adresse zuweisen
	
	
	// Write some text messages to the UART - just like on RP6Base:
	writeString_P("\n\n   _______________________\n");
	writeString_P("   \\| RP6  ROBOT SYSTEM |/\n");
	writeString_P("    \\_-_-_-_-_-_-_-_-_-_/\n\n");
	
	// Set the four Status LEDs:
	setLEDs(0b1111);
	mSleep(500);
	setLEDs(0b0000);
	
	// Play two sounds with the Piezo Beeper on the RP6Control:
	sound(180,80,25);
	sound(220,80,0);
	
	showScreenLCD("    Compass     ", "   Navigation   ");
	mSleep(1000); 
	
	// Start the stopwatches:
	startStopwatch1();
	startStopwatch2();
	
	while(true) 
	{
		runningLight();
		LCD_data();
	}
	return 0;
}

mfg WarChild