55 lines
1.5 KiB
C
Executable file
55 lines
1.5 KiB
C
Executable file
#include "navigation.h"
|
|
|
|
// Initialisiert die Klasse
|
|
Navigation::Navigation() {
|
|
// Am Anfang stehen wir an der Stelle
|
|
richtung = 0;
|
|
ausrichtung = 0;
|
|
geschwindigkeit = 0;
|
|
Aktualisieren(); // und ä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(richtung); // richtung ist winkel
|
|
double x = sin(richtung);
|
|
|
|
// Abweichung der Ausrichtung ermitteln(als winkel)
|
|
int w = sensor.GetAusrichtung() - ausrichtung;
|
|
|
|
// Stärke der einzelnen Motoren berechnen
|
|
double v0 = (-x-sqrt(3)*y)/2;
|
|
double v1 = x;
|
|
double v2 = (-x+sqrt(3)*y)/2;
|
|
|
|
// Ausgerechnete Stärke an die Motoren ü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); // Übergebene Werte schreiben
|
|
SetzeAusrichtung(nAusrichtung);
|
|
SetzeGeschwindigkeit(nGeschwindigkeit);
|
|
Aktualisieren(); // Und anwenden
|
|
}
|
|
|