-
Notifications
You must be signed in to change notification settings - Fork 0
/
pathfinding2.py
126 lines (100 loc) · 3.43 KB
/
pathfinding2.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
import math
import servo_handler
import ultrasonic_sensors
import time
import colour_in_shape_filtered as cl
THRESHOLD_DISTANCE = 60
indicator = 0 # stores 0 or 1 to indicate if moving straight is an option
ultra = ultrasonic_sensors.UltrasonicHandler([38, 40], [37, 35], [29, 31])
ultra.start_measuring()
servo = servo_handler.ServoHandler(21, 23)
#cam = c_detect.colour.CameraHandler("192.168.148.82")
cam = cl.CameraHandler("192.168.148.236")
coordinates = []
def rotate_away(num):
if num == 1:
servo.rotate(35) # turn left
elif num == 0:
servo.rotate(-35) # turn right
def left_turn():
servo.rotate(90)
#indicator = 0
def right_turn():
# if distance from the front and left are small
servo.rotate(-90)
#indicator = 0
def move_straight(colour, isFound=False):
#global indicator
# time.sleep(1)
servo.move_forever()
while(ultra.front > THRESHOLD_DISTANCE):
#print("Front: "+str(ultra.front)[:6]+" | Left: "+str(ultra.left)[:6]+"| Right: "+str(ultra.right)[:6])
found_objects = cam.find_obj(colour)
# print(found_objects)
if not isFound and (len(found_objects) != 0):
# servo.stop_move_forever()
servo.stop_move_forever()
cam.save_picture()
isFound = True
if isFound and (len(found_objects) == 0):
isFound = False
time.sleep(0.4)
print("Stopping")
servo.stop_move_forever()
# moving right
if ultra.right <= 15:
print("Too Close, turning left")
rotate_away(1)
ultra.front = ultra._update_distance(
ultra.front_trigger, ultra.front_echo)
move_straight(isFound)
if ultra.left <= 15:
print("Too Close, turning right")
rotate_away(0)
ultra.front = ultra._update_distance(
ultra.front_trigger, ultra.front_echo)
move_straight(isFound)
if ultra.left <= THRESHOLD_DISTANCE and ultra.right > THRESHOLD_DISTANCE*2:
print("Going right")
right_turn()
#indicator = 0
ultra.front = ultra._update_distance(
ultra.front_trigger, ultra.front_echo)
move_straight(isFound)
# moving left
elif ultra.right <= THRESHOLD_DISTANCE and ultra.left > THRESHOLD_DISTANCE*2:
print("Going left")
left_turn()
#indicator = 0
ultra.front = ultra._update_distance(
ultra.front_trigger, ultra.front_echo)
move_straight(isFound)
elif ultra.right > (THRESHOLD_DISTANCE)*2 and ultra.left > (THRESHOLD_DISTANCE)*2:
print("Free space. Go right")
coordinates.append([servo.pos, 90])
right_turn()
#indicator = 0
ultra.front = ultra._update_distance(
ultra.front_trigger, ultra.front_echo)
move_straight(isFound)
else:
print("Free space. Go right")
coordinates.append([servo.pos, 90])
right_turn()
#indicator = 0
#ultra.front = ultra._update_distance(ultra.front_trigger, ultra.front_echo)
move_straight(isFound)
'''def object_in_path():
# if we're travaeling in a atraight line, but we see a thing
# turn 90
servo.rotate(90)
revs = 0 # store the number of revoultions we went forward
# go straight by 3 revs
'''
move_straight()
servo.release()
'''
while True:
servo.rotate(90)
print("Front: "+str(ultra.front)[:6]+" | Left: "+str(ultra.left)[:6]+"| Right: "+str(ultra.right)[:6])
'''