1 files changed, 46 insertions, 0 deletions
diff --git a/source/AVR_Studio/Soccer/sensor/position.c b/source/AVR_Studio/Soccer/sensor/position.c
new file mode 100755
index 0000000..cc7a345
--- /dev/null
+++ b/source/AVR_Studio/Soccer/sensor/position.c
@@ -0,0 +1,46 @@
+#include "position.h"
+#include "../global.h"
+
+Position::Position() {
+	// inititalisiere die Maussensoren
+	maussensor0.init(MAUSSENSOR0);
+	maussensor1.init(MAUSSENSOR1);
+
+	// Wir starten in der Mitte, nach vorne gerichtet
+	x = 0;
+	y = 0;
+	ausrichtung = 0;
+}
+
+Position::~Position() {
+}
+
+// Aktualisiert die internen Variablen x,y und ausrichtung
+void Position::Aktualisieren() {
+	// Lese die Maussensoren ein
+	uint8 maus0_x = maussensor0.GetX();
+	uint8 maus0_y = maussensor0.GetY();
+	uint8 maus1_x = maussensor1.GetX();
+	uint8 maus1_y = maussensor1.GetY();
+
+	// Und berechne die neuen Werte 
+	// ACHTUNG!!!!! Die Funktionen sind noch ungetestet und werden fehler enthalten
+	x += (maus0_x*sin(ausrichtung+45)+maus1_x*cos(ausrichtung+45))/2;
+	y += (maus0_y*sin(ausrichtung+45)+maus1_y*cos(ausrichtung+45))/2;
+	ausrichtung += ((maus0_y + maus1_y)/2)*(360/MAUS_FULL_TURN);
+}
+
+// Gibt die X-Koordinate zurück
+int Position::GetX() {
+	return (int)x;
+}
+
+// Gibt die Y-Koordinate zurück
+int Position::GetY() {
+	return (int)y;
+}
+
+// Gibt die Ausrichtung zurück
+int Position::GetAusrichtung() {
+	return ausrichtung;
+}