-
Notifications
You must be signed in to change notification settings - Fork 0
/
UltraSonic.cpp
129 lines (108 loc) · 3.46 KB
/
UltraSonic.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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
#include "Ultrasonic.hpp"
// Implementierung: OBJEKTE
extern Display d;
extern Player p;
extern Led led;
extern Mate mate;
extern Pilot m;
//extern Ultrasonic us;
Ultrasonic::Ultrasonic() {
}
/*****************************************************
initiiert Ultraschallsensoren
*****************************************************/
void Ultrasonic::init(){
for(int i = 0; i < 5; i++) {
Wire.beginTransmission(addresses[i]);
Wire.write(byte(0x02));
Wire.write(byte(70));
Wire.endTransmission();
}
}
/*****************************************************
holt letze Werte ab
*****************************************************/
void Ultrasonic::fetch() {
for(int i = 0; i < 5; i++) {
Wire.beginTransmission(addresses[i]);
Wire.write(byte(0x02));
Wire.endTransmission();
Wire.requestFrom((int) addresses[i], (int) 2);
if (2 <= Wire.available()) {
distance[i] = Wire.read() << 8;
distance[i] |= Wire.read();
}
}
Wire.beginTransmission(addresses[1]);
Wire.write(byte(0x04));
Wire.endTransmission();
Wire.requestFrom((int) addresses[1], (int) 2);
if (2 <= Wire.available()) {
echo2[0] = Wire.read() << 8;
echo2[0] |= Wire.read();
}
Wire.beginTransmission(addresses[3]);
Wire.write(byte(0x04));
Wire.endTransmission();
Wire.requestFrom((int) addresses[3], (int) 2);
if (2 <= Wire.available()) {
echo2[1] = Wire.read() << 8;
echo2[1] |= Wire.read();
}
Wire.beginTransmission(addresses[1]);
Wire.write(byte(0x06));
Wire.endTransmission();
Wire.requestFrom((int) addresses[1], (int) 2);
if (2 <= Wire.available()) {
echo3[0] = Wire.read() << 8;
echo3[0] |= Wire.read();
}
Wire.beginTransmission(addresses[3]);
Wire.write(byte(0x06));
Wire.endTransmission();
Wire.requestFrom((int) addresses[3], (int) 2);
if (2 <= Wire.available()) {
echo3[1] = Wire.read() << 8;
echo3[1] |= Wire.read();
}
}
/*****************************************************
Wenn Timer entsprechend: löst neue Messung aus und holt alte Werte ab.
*****************************************************/
void Ultrasonic::update() {
if(millis()-lastMeasurement > 25 && !fetched){
fetch();
fetched = true;
}
if(millis()-lastMeasurement > 65){
Wire.beginTransmission(0); //auf Adresse 0 hören alle Ultraschallsensor zu. Alternativ können die Befehle an alle Sonsoren einzeln gesendet werden.
Wire.write(byte(0x00));
Wire.write(byte(0x51));
Wire.endTransmission();
lastMeasurement = millis();
fetched = false;
}
}
/*****************************************************
erfragt beim Ultraschallsensor durch einen Interrupt die aktuellen Sensorwerte
empfängt und speichern diese Werte im globalen Array us[]:
1
.--.
/ \
2 \ / 0
'--'
3
*****************************************************/
int Ultrasonic::frontLeft() { return distance[0]; }
int Ultrasonic::left() { return distance[1]; }
int Ultrasonic::back() { return distance[2]; }
int Ultrasonic::right() { return distance[3]; }
int Ultrasonic::frontRight() { return distance[4]; }
int Ultrasonic::front() { return min(frontLeft(), frontRight()); }
int Ultrasonic::left2() { return echo2[0]; }
int Ultrasonic::right2() { return echo2[1]; }
int Ultrasonic::left3() { return echo3[0]; }
int Ultrasonic::right3() { return echo3[1]; }
bool Ultrasonic::check() {
//TODO
}