-
Notifications
You must be signed in to change notification settings - Fork 1
/
dist_sens.py
127 lines (111 loc) · 3.7 KB
/
dist_sens.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
from hardware import Distance_Sensor
from hardware import Wheel_Controller
import easygopigo3
import random
import time
import math
gpg = easygopigo3.EasyGoPiGo3()
ds = Distance_Sensor(gpg)
wc = Wheel_Controller(gpg)
def move_and_correct(distance):
print("--running move_and_correct--")
ds.set_angle(180)
time.sleep(1)
firstReading = ds.get_distance()
print("first reading:", firstReading)
ds.set_angle(90)
time.sleep(1)
forwardDistance = ds.get_distance()
if forwardDistance < distance:
forwardDistance = forwardDistance - 2
else:
forwardDistance = distance
print("forward distance:", forwardDistance)
wc.move_cm(forwardDistance)
ds.set_angle(180)
time.sleep(1)
secondReading = ds.get_distance()
print("second reading:", secondReading)
if abs(secondReading - firstReading) > .25 and abs(secondReading - firstReading) < 7 and forwardDistance != 0 and (firstReading < 14 or secondReading < 14):
print("forwardDistance:", forwardDistance)
angleInRadians = math.atan(float(secondReading) / (float(forwardDistance) + (float(-forwardDistance) * float(firstReading))/(float(firstReading) - float(secondReading))))
print("radians:", angleInRadians)
angleInDegrees = angleInRadians * 180 / math.pi
print("angle is:", angleInDegrees)
wc.move_cm(-forwardDistance)
if secondReading > firstReading:
wc.rotate_left(angleInDegrees)
else:
wc.rotate_left(angleInDegrees)
wc.move_cm(forwardDistance)
ds.set_angle(180)
time.sleep(1)
afterMoveReadingLeft = ds.get_distance()
print("afterMoveReadingLeft", afterMoveReadingLeft)
print("----")
if afterMoveReadingLeft < 11.5:
offset = 14 - 2.5 - afterMoveReadingLeft
print("off center left:", offset)
wc.rotate_left(30)
time.sleep(1)
wc.move_cm(-(offset / math.sin(math.pi/6)))
time.sleep(1)
wc.rotate_right(30)
time.sleep(1)
wc.move_cm(offset / math.tan(math.pi/6))
if afterMoveReadingLeft > 13 and afterMoveReadingLeft < 28:
offset = afterMoveReadingLeft - (14 - 2.5)
print("off center left:", offset)
wc.rotate_right(30)
time.sleep(1)
wc.move_cm(-(offset / math.sin(math.pi/6)))
time.sleep(1)
wc.rotate_left(30)
time.sleep(1)
wc.move_cm(offset / math.tan(math.pi/6))
def move():
move_and_correct(14)
ar = ds.sweep(0, 180, 90)
print("Distances: " + str(ar))
valid_choices = []
if(ar[0] > 17):
print("Left is valid")
valid_choices.append('left')
if(ar[1] > 17):
print("Straight is valid")
valid_choices.append('straight')
if(ar[2] > 17):
print("Right is valid")
valid_choices.append('right')
time.sleep(1)
if(len(valid_choices) == 0):
print("Turning around")
move_and_correct(14)
time.sleep(.25)
#ds.set_angle(90)
#time.sleep(1)
#if(ds.get_distance() < 14):
# wc.move_cm(ds.get_distance() - 2)
#else:
# wc.move_cm(14)
wc.rotateLeft(180)
else:
choice = valid_choices[random.randint(0, len(valid_choices) - 1)]
print("Chose ", choice)
#ds.set_angle(90)
#time.sleep(1)
#if(ds.get_distance() < 14):
# wc.move_cm(ds.get_distance() - 2)
#else:
# wc.move_cm(14)
move_and_correct(14)
time.sleep(.25)
if(choice == 'left'):
wc.rotate_left(90)
elif(choice == 'right'):
wc.rotate_right(90)
def correct():
ar = ds.sweep(0, 180, 15)
print(ar)
while True:
move()