blob: 6bf5d4d0aa0d0ee6465045674a45c49b1f7310db (
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
|
#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
}
|
