1 files changed, 54 insertions, 0 deletions
diff --git a/source/AVR_Studio/Soccer/navigation.c b/source/AVR_Studio/Soccer/navigation.c
new file mode 100755
index 0000000..6bf5d4d
--- /dev/null
+++ b/source/AVR_Studio/Soccer/navigation.c
@@ -0,0 +1,54 @@
+#include "navigation.h"
+
+// Initialisiert die Klasse
+Navigation::Navigation() {
+	richtung = 0;
+	ausrichtung = 0;
+	geschwindigkeit = 0;
+	Aktualisieren();
+}
+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
+	int y = (int)cos(richtung); // richtung ist winkel
+	int x = (int)sin(richtung);
+	
+	// Abweichung der Ausrichtung ermitteln(als winkel)
+	int w = sensor.getAusrichtung() - ausrichtung;
+
+	// Stärke der einzelnen Motoren berechnen
+	float v0 = x;
+	float v1 = (-x+sqrt(3)*y)/2;
+	float v2 = (-x-sqrt(3)*y)/2;
+
+	// Ausgerechnete Stärke an die Motoren übergeben
+	board.motor(0,(int)v0*geschwindigkeit +w);
+	board.motor(1,(int)v1*geschwindigkeit +w);
+	board.motor(2,(int)v2*geschwindigkeit +w);
+}
+
+// Aktualieren mit allen Parametern
+void Navigation::Aktualisieren(int nRichtung,int nAusrichtung,int nGeschwindigkeit) {
+	SetzeRichtung(nRichtung); // Übergebene Werte schreiben
+	SetzeAusrichtung(nAusrichtung);
+	SetzeGeschwindigkeit(nGeschwindigkeit);
+	Aktualisieren(); // Und anwenden
+}
+