blob: 2698390c45fe14fe1e90b9803c4cb73c47b6c7b9 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
|
#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((double)richtung*0.01745); // richtung ist winkel
double x = sin((double)richtung*0.01745);
// 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
}
|
