-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.py
143 lines (131 loc) · 4.91 KB
/
main.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
from cascade_detection import Detection
from controller import Controller
import cv2
from motor import CalibratedMotor, Motor
import pigpio
import time
from LineTracking import LineTracking
from print_distance import Sonic
drive_motor = Motor(Motor._bp.PORT_B)
steer_motor = CalibratedMotor(Motor._bp.PORT_D, calpow=30)
def drive_steer(pos_new):
pos = steer_motor.position_from_factor(pos_new)
steer_motor.change_position(pos)
def drive_power(power_new):
drive_motor.change_power(power_new)
def stop_all():
drive_motor.stop()
steer_motor.stop()
def main():
###Kameraaufnahme starten
cap = cv2.VideoCapture(0)
###Detectionobjekt für Palettenerkennung
det = Detection()
###Linetracking und controller-Objekt für Linetracking
lt = LineTracking()
controller = Controller()
sonic = Sonic()
steer_motor.calibrate()
state = 0
pi = pigpio.pi()
if not pi.connected:
exit(0)
try:
while True:
ret, image = cap.read()
distances = sonic.getDistance(2)
dist_front_l = distances[1]
dist_front_r = distances[0]
width = 640
height = 480
if(state == 0):
print("Following line! Distance front left/front right: {}/{}".format(dist_front_r, dist_front_l))
if(dist_front_l > 40 or dist_front_r > 40):
###Linie folgen
drive_steer(0)
drive_power(-30)
val = lt.track_line(image, False)
pid = controller.pid(val)
drive_steer(pid/100)
else:
drive_power(0)
state = 1
elif(state == 1):
print("Object in front!")
det = Detection()
###Aktuelles Bild an det übergeben -> Palette erkennen
(x, y, w, h), image = det.detect_palette(image)
###Wenn detect_palette(image) -1 zurückgibt, wurde keine Palette erkannt
if(x == -1):
###Wenn keine Palette erkannt wurde -> x-Wert so setzen, dass er rechts vorbei fährt
x = 1000
if(x < int(width / 2)):
print("Pallet more on the right side -> drive around the left")
state = 2
else:
print("Pallet more on the left side -> drive around the right")
state = 3
elif(state == 2):
###Solange nach vorne links fahren, bis rechter Sensor über die Palette hinaus ist, dann noch etwas weiter fahren
print("Entfernung vorne rechts: {}".format(dist_front_r))
print("Drive around left!")
if(dist_front_r > 80):
print("Right sensor past the pallet!")
drive_steer(-0.8)
drive_power(-30)
time.sleep(0.5)
drive_power(0)
state = 4
else:
drive_steer(-0.8)
drive_power(-30)
elif(state == 3):
###Solange nach vorne rechts fahren, bis linker Sensor über die Palette hinaus ist, dann noch etwas weiter fahren
print("Drive around right!")
if(dist_front_l > 80):
print("Left sensor past the pallet!")
drive_steer(0.8)
drive_power(-30)
time.sleep(0.5)
drive_power(0)
state = 5
else:
drive_steer(0.8)
drive_power(-30)
elif(state == 4):
###track_line(image) gibt solange -1 zurück, bis eine Linie erkannt wurde
print("Trying to return on line!")
val = lt.track_line(image, True)
print(val)
if(val == -1):
drive_steer(1)
drive_power(-30)
else:
drive_steer(-0.8)
time.sleep(1.2)
drive_power(0)
state = 0
elif(state == 5):
print("Trying to return on line!")
val = lt.track_line(image, True)
print(val)
if(val == -1):
drive_steer(-1)
drive_power(-30)
else:
drive_steer(0.8)
time.sleep(1.2)
drive_power(0)
state = 0
###Aktuelles Bild anzeigen
cv2.imshow("frame", image)
cv2.waitKey(1)
except KeyboardInterrupt:
print("\nCTRL-C pressed. Cleaning up and exiting!")
finally:
###Alles sicher beenden
cap.release()
cv2.destroyAllWindows()
pi.stop()
stop_all()
main()