54 lines
1.4 KiB
C
Executable file
54 lines
1.4 KiB
C
Executable file
#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
|
|
}
|
|
|