From 05c8b0af602b222d00c227560682158333b726a2 Mon Sep 17 00:00:00 2001
From: sicarius <devnull@localhost>
Date: Sun, 11 Feb 2007 19:03:00 +0000
Subject: Added lot's of code-files used during work

---
 source/AVR_Studio/Soccer/navigation.c | 54 +++++++++++++++++++++++++++++++++++
 1 file changed, 54 insertions(+)
 create mode 100755 source/AVR_Studio/Soccer/navigation.c

(limited to 'source/AVR_Studio/Soccer/navigation.c')

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
+}
+
-- 
cgit v1.2.3