rc2007-soccer/source/AVR_Studio/Soccer/navigation.c

#include "navigation.h"

// Initialisiert die Klasse
Navigation::Navigation() {
	// Am Anfang stehen wir an der Stelle
	richtung = 0;
	ausrichtung = 0;
	geschwindigkeit = 0;
	Aktualisieren(); // und <20>nderungen anwenden
}
Navigation::~Navigation() {
}

// Setze eine neue Richtung
void Navigation::SetzeRichtung(int nRichtung) {
	richtung = nRichtung;
}
// Setze eine neue Ausrichtung
void Navigation::SetzeAusrichtung(int nAusrichtung) {
	ausrichtung = nAusrichtung;
}
// Setze eine neue Geschwindigkeit
void Navigation::SetzeGeschwindigkeit(int nGeschwindigkeit) {
	geschwindigkeit = nGeschwindigkeit;
}


// Aktualieren ohne Parameter
void Navigation::Aktualisieren() {	
	// Richtung in x und y-Kompontente zerlegen
	double y = cos((double)richtung*0.01745); // richtung ist winkel
	double x = sin((double)richtung*0.01745);
	
	// Abweichung der Ausrichtung ermitteln(als winkel)
	int w = sensor.GetAusrichtung() - ausrichtung;

	// St<53>rke der einzelnen Motoren berechnen	
	double v0 = (-x+sqrt(3)*y)/2;
	double v1 = x;
	double v2 = (-x-sqrt(3)*y)/2;

	// Ausgerechnete St<53>rke an die Motoren <20>bergeben
	board.motor(0,(int)((double)v0*geschwindigkeit +w));
	board.motor(1,(int)((double)v1*geschwindigkeit +w));
	board.motor(2,(int)((double)v2*geschwindigkeit +w));
}

// Aktualieren mit allen Parametern
void Navigation::Aktualisieren(int nRichtung,int nAusrichtung,int nGeschwindigkeit) {
	SetzeRichtung(nRichtung); // <20>bergebene Werte schreiben
	SetzeAusrichtung(nAusrichtung);
	SetzeGeschwindigkeit(nGeschwindigkeit);
	Aktualisieren(); // Und anwenden
}