55 lines
1.4 KiB
C
55 lines
1.4 KiB
C
|
#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<53>rke der einzelnen Motoren berechnen
|
|||
|
|
float v0 = x;
|
|||
|
|
float v1 = (-x+sqrt(3)*y)/2;
|
|||
|
|
float v2 = (-x-sqrt(3)*y)/2;
|
|||
|
|
|
|||
|
|
// Ausgerechnete St<53>rke an die Motoren <20>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); // <20>bergebene Werte schreiben
|
|||
|
|
SetzeAusrichtung(nAusrichtung);
|
|||
|
|
SetzeGeschwindigkeit(nGeschwindigkeit);
|
|||
|
|
Aktualisieren(); // Und anwenden
|
|||
|
|
}
|
|||
|
|
|