forked from JanekOstendorf/THOMAS
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathCollisionDetection.cpp
92 lines (67 loc) · 2.77 KB
/
CollisionDetection.cpp
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
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
/*
-- ArduinoProtocol-KLASSE :: IMPLEMENTIERUNG --
*/
/* INCLUDES */
// Klassenheader
#include "CollisionDetection.h"
//Arduino Protokoll Klasse
#include "ArduinoProtocol.h"
// Vector Klasse
#include <vector>
using namespace THOMAS;
/* FUNKTIONEN */
CollisionDetection::CollisionDetection(ArduinoProtocol *arduinoProtocol)
{
// Arduino Protocol Klasse speichern.
_arduinoProtocol = arduinoProtocol;
}
void CollisionDetection::UpdateUSensorData()
{
// Wiederhole immer
while(true)
{
// Messwerte abrufen und speichern
std::vector<int> distance = _arduinoProtocol->GetDistance();
// Zugriff von anderen Threads sperren
USensorMutex.lock();
// Aktuelle Messwerte in das Array kopieren
USensorMessurements = distance;
// Zugriff von anderen Threads erlauben
USensorMutex.unlock();
}
}
std::vector<short> CollisionDetection::CorrectWantedSpeed(short wantedSpeed[])
{
// Neuen Vector mit dem Speed erstellen
std::vector<short> newWantedSpeed (2);
// Ist vorne frei?
if(USensorMessurements.at(US_FRONT_MIDDLE) > _warnDistance || USensorMessurements.at(US_FRONT_MIDDLE) == 0)
{
// Geradeaus weiter fahren
newWantedSpeed.at(0) = wantedSpeed[0];
newWantedSpeed.at(1) = wantedSpeed[1];
return newWantedSpeed;
}
// Ist Links frei
if(USensorMessurements.at(US_FRONT_LEFT) > _warnDistanceRL || USensorMessurements.at(US_FRONT_LEFT) == 0)
{
// Nach links fahren
newWantedSpeed.at(0) = -100;
newWantedSpeed.at(1) = 100;
return newWantedSpeed;
}
// Ist Rechts frei
if(USensorMessurements.at(US_FRONT_RIGHT) > _warnDistanceRL || USensorMessurements.at(US_FRONT_RIGHT) == 0)
{
// Nach rechts fahren
newWantedSpeed.at(0) = 100;
newWantedSpeed.at(1) = -100;
return newWantedSpeed;
}
// Anhalten
newWantedSpeed.at(0) = 0;
newWantedSpeed.at(1) = 0;
// OsterEi: THOMAS Dead Sound -> Wer hat da eine Tür hingestellt?
//system("beep -f 523 -l 38 -D 2 -n -f 554 -l 34 -D 2 -n -f 587 -l 28 -D 489 -n -f 195 -l 20 -D 0 -n -f 391 -l 20 -D 0 -n -f 493 -l 20 -n -f 195 -l 20 -D 0 -n -f 391 -l 20 -D 0 -n -f 493 -l 20 -D 0 -n -f 587 -l 20 -D 0 -n -f 698 -l 20 -n -f 587 -l 20 -D 0 -n -f 698 -l 20 -n -f 587 -l 20 -D 0 -n -f 698 -l 20 -D 145 -n -f 195 -l 20 -D 0 -n -f 587 -l 20 -D 0 -n -f 698 -l 20 -n -f 195 -l 20 -D 0 -n -f 587 -l 20 -D 0 -n -f 698 -l 20 -D 0 -n -f 195 -l 20 -D 0 -n -f 587 -l 20 -D 0 -n -f 698 -l 20 -D 99 -n -f 220 -l 20 -D 0 -n -f 523 -l 20 -D 0 -n -f 659 -l 20 -D 99 -n -f 246 -l 20 -D 0 -n -f 493 -l 20 -D 0 -n -f 587 -l 20 -D 99 -n -f 261 -l 20 -D 0 -n -f 391 -l 20 -D 0 -n -f 523 -l 20 -n -f 261 -l 20 -D 0 -n -f 391 -l 20 -D 0 -n -f 523 -l 20 -D 0 -n -f 329 -l 148 -D 0 -n -f 195 -l 147 -D 0 -n -f 329 -l 147 -D 0 -n -f 130 -l 20 -D 0 -n -f 261 -l 20 -n -f 130 -l 20 -D 0 -n -f 261 -l 20 -n -f 130 -l 20 -D 0 -n -f 261 -l 20 &");
return newWantedSpeed;
}