-
Notifications
You must be signed in to change notification settings - Fork 0
/
Robocup.py
242 lines (226 loc) · 8.6 KB
/
Robocup.py
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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
import smbus
import time
import picamera
import picamera.array
import random
# for RPI version 1, use "bus = smbus.SMBus(0)"
bus = smbus.SMBus(1)
# I2C Adresse festlegen
MASTER_ADDRESS = 0x04
#Definition der Pins
SERVO_A = 0
MOTOR_L = 2
MOTOR_R = 3
MOTOR_DIR = 4
TASTER = 6
LAMPE = 8
MOTOR_STANDBY = 10
#Richtungen definieren
FORWARD = 0b0
BACKWARD = 0b11000000
LINKS = 0b1000000
RECHTS = 0b10000000
####Kamera-KLasse
class MyAnalysis(picamera.array.PiRGBAnalysis):
def __init__(self, camera):
self.set(LAMPE,1)
self.geschwindigkeit = 90
self.set(MOTOR_STANDBY, 1)
self.set(MOTOR_DIR,FORWARD)
self.status = 0 #aus
self.time = time.time()
super(MyAnalysis, self).__init__(camera)
def analyse(self, foto):#Lichtwerte ins Array
#self.geschwindigkeit = 90
if self.status == 0: # Warten bis der mittlere Taster gedrueckt wird
if self.get(TASTER)^0b1111 == 0b0010:
print('Start')
while self.get(TASTER)^0b1111 == 0b0010: #Warten solange Taster gedrueckt ist
time.sleep(0.1)
self.set(MOTOR_STANDBY,1)
self.status = 1
else:
if self.get(TASTER)^0b1111 == 0b0010:
while self.get(TASTER)^0b1111 == 0b0010: #Warten solange Taster gedrueckt ist
time.sleep(0.5)
self.set(MOTOR_L, 0)
self.set(MOTOR_R, 0)
self.set(MOTOR_DIR,FORWARD)
self.set(TASTER,7)
self.set(MOTOR_STANDBY, 0)
self.status=0
else:
self.set(LAMPE,1)
self.set(MOTOR_DIR, FORWARD)
self.set(MOTOR_STANDBY, 1)
#print('Aufnahme')
licht1 = []
licht2 = []
licht3 = []
weiss1 = []
weiss2 = []
weiss3 = []
for i in range(9, 55, 5):
r1 = int(foto[40][i][0])
r2 = int(foto[51][i][0])
r3 = int(foto[62][i][0])
g1 = int(foto[40][i][1])
g2 = int(foto[51][i][1])
g3 = int(foto[62][i][1])
b1 = int(foto[40][i][2])
b2 = int(foto[51][i][2])
b3 = int(foto[62][i][2])
wert1 = (r1 + g1 + b1) // 3
wert2 = (r2 + g2 + b2) // 3
wert3 = (r3 + g3 + b3) // 3
if wert1 > 50:
weiss1.append(0)
else:
weiss1.append(1)
if wert2 > 50:
weiss2.append(0)
else:
weiss2.append(1)
if wert3 > 50:
weiss3.append(0)
else:
weiss3.append(1)
licht1.append(int(wert1))
licht2.append(int(wert2))
licht3.append(int(wert3))
print(r1,g1,b1)
'''if weiss1.count(0)>8 and weiss2.count(1)>7 and weiss3.count(1) >= 2:
print('DOMINIKS PROGRAMM')
self.set(MOTOR_DIR,FORWARD)
self.set(MOTOR_L,self.geschwindigkeit)
self.set(MOTOR_R,self.geschwindigkeit)
time.sleep(0.15)
a = random.randint(0,1)
if a == 0:
self.set(MOTOR_DIR,RECHTS)
else:
self.set(MOTOR_DIR,LINKS)
time.sleep(0.5)
else:
self.liniefolgen(licht1)'''
if not (1 in weiss1):#Falls nur noch weiss, analysiert er die hinteren Zeilen
self.linieverloren(licht2,weiss3)
if not (0 in weiss1):#Falls nur noch Schwarz -> Kreuzung: Fallunterscheidung, bei gruenem Punkt abbiegen, sonst geradeaus
self.kreuzung(licht2,licht3)
elif weiss1[:5].count(1)>=5 and weiss2.count(1)<4:
pass
elif weiss1[5:].count(1)>=5 and weiss2.count(1)<4:
pass
else:
self.liniefolgen(licht1)
def werte_berechnen(self, liste):#Berechnet die Helligkeitsdifferenzen
gewichtung = [15, 4, 3.7, 1.3, 1, 1, 1.3, 3.7, 4, 15]
for i in range (len(liste)): # Belegt die Ausgelesenen Pixel mit Prioritaeten um die Motorgeschwindigkeit zu steuern
liste[i]=gewichtung[i]*liste[i]
Licht1 = abs(sum(liste[:5]))
Licht2 = abs(sum(liste[5:]))
dif = Licht1 - Licht2
return dif
def hindernis_umfahren(self, richtung):
self.set(MOTOR_L,self.geschwindigkeit)
self.set(MOTOR_R,self.geschwindigkeit)
if richtung == 0:
self.set(MOTOR_DIR, RECHTS)
time.sleep(1.2)
self.set(MOTOR_DIR, FORWARD)
time.sleep(2)
self.set(MOTOR_DIR, LINKS)
time.sleep(1.2)
self.set(MOTOR_DIR, FORWARD)
time.sleep(2)
else:
self.set(MOTOR_DIR, LINKS)
time.sleep(1.2)
self.set(MOTOR_DIR, FORWARD)
time.sleep(2)
self.set(MOTOR_DIR, RECHTS)
time.sleep(1.2)
self.set(MOTOR_DIR, FORWARD)
time.sleep(2)
def liniefolgen(self, liste):
dif = self.werte_berechnen(liste)
faktor = 0.1
rechterMotor, linkerMotor = int(self.geschwindigkeit + dif*faktor), int(self.geschwindigkeit - dif*faktor)
if rechterMotor > 200: rechterMotor = 200
if linkerMotor > 200: linkerMotor = 200
if rechterMotor < 0: rechterMotor= 0
if linkerMotor < 0: linkerMotor=0
self.set(MOTOR_R, abs(rechterMotor)) #setzt die Motordrehzahl
self.set(MOTOR_L, abs(linkerMotor))
def linieverloren(self,licht2,weiss3):
print('Linie verloren')
sum_li = sum(licht2[0:5])
sum_re = sum(licht2[5:])
dif = sum_li - sum_re
self.set(MOTOR_DIR,FORWARD)
self.set(MOTOR_L,self.geschwindigkeit)
self.set(MOTOR_R,self.geschwindigkeit)
if dif < 450 and dif > -450:
if sum(weiss3)>=5: # zuvor Gruener Punkt gesehen, dementsprechend abbiegen
self.linieverloren(weiss3*120,[0])
else: # Differenz ist zu niedrig -> Luecke erkannt
time.sleep(0.1)
elif dif >= 450: #Auf der rechten Seite befand sich schwarz
self.set(MOTOR_DIR,RECHTS)
time.sleep(0.2)
self.set(MOTOR_DIR,BACKWARD)
time.sleep(0.15)
else: #auf der linken Seite befand sich schwarz
self.set(MOTOR_DIR,LINKS)
time.sleep(0.2)
self.set(MOTOR_DIR,BACKWARD)
time.sleep(0.15)
self.set(MOTOR_DIR,FORWARD)
def kreuzung(self, licht2, licht3):
print('Kreuzung')
sum_li = sum(licht2[0:5]+licht3[0:5])
sum_re = sum(licht2[5:]+licht3[5:])
dif = sum_re - sum_li
self.set(MOTOR_DIR,FORWARD)
self.set(MOTOR_L,self.geschwindigkeit)
self.set(MOTOR_R,self.geschwindigkeit)
if dif < 300 and dif > -300:
print('Gerade')
time.sleep(0.1)
elif dif <= 300:
print('Rechts')
time.sleep(0.1)
self.set(MOTOR_DIR,RECHTS)
time.sleep(0.4)
else:
print('Links')
time.sleep(0.1)
self.set(MOTOR_DIR,LINKS)
time.sleep(0.4)
def set(self, addresse, wert): #Schickt 1 Byte mit einer Position und dann 1 Byte mit dem Inhalt
try:
bus.write_byte_data(MASTER_ADDRESS,addresse,wert)
time.sleep(0.001)
except:
print('set() Fehler')
return -1
def get(self, position):#man bekommt den Wert der uebergebenen Position im Array des Arduino
try:
self.set(255,position) #Legt einen Pointer fest
wert=(bus.read_byte(MASTER_ADDRESS)) #Arduino schickt Wert mit Position des Pointers
time.sleep(0.001)
except:
print('get() Fehler')
wert=0
return wert
time1 = 60000
with picamera.PiCamera() as camera:
camera.resolution = (64,64)
camera.framerate = 25
output = MyAnalysis(camera)
camera.shutter_speed = camera.exposure_speed
camera.exposure_mode = 'off'
camera.awb_mode = 'auto'
camera.start_recording(output, 'rgb')
camera.wait_recording(time1)
camera.stop_recording()