forked from asigdel29/RobotRadar
-
Notifications
You must be signed in to change notification settings - Fork 0
/
radar.py
167 lines (134 loc) · 5.35 KB
/
radar.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
import pygame
import picamera
import io
import math
import time
import colors
import sys
from target import *
from display import*
from gopigo import *
from collections import Counter
# initialize the program
x = pygame.init()
pygame.font.init()
defaultFont = pygame.font.get_default_font()
fontRenderer = pygame.font.Font(defaultFont, 20)
radarDisplay = pygame.display.set_mode((1400, 800))
pygame.display.set_caption('Radar Screen')
def us_map():
delay=.02
debug =0 #True to print all raw values
num_of_readings=45 #Number of readings to take
incr=180/num_of_readings #increment of ang in servo
ang_l=[0]*(num_of_readings+1) #list to hold the ang's of readings
dist_l=[0]*(num_of_readings+1) #list to hold the distance at each ang
x=[0]*(num_of_readings+1) #list to hold the x coordinate of each point
y=[0]*(num_of_readings+1) #list to hold the y coordinate of each point
buf=[0]*40
ang=15
lim=1000 #maximum limit of distance measurement (any value over this which be initialized to the limit value)
index=0
sample=2
targets = {}
enable_servo()
servo_pos=160
running = 1
turnTime = .305
driveTime = .305
j = 0
while True:
for i in range(sample):
dist=us_dist(15)
if dist<lim and dist>=0:
buf[i]=dist
else:
buf[i]=lim
if dist < 2:
stop()
running = 0
max=Counter(buf).most_common()
rm=-1
for i in range (len(max)):
if max[i][0] <> lim and max[i][0] <> 0:
rm=max[i][0]
break
if rm==-1:
rm=lim
if debug==1:
print index,ang,rm
if running == 1:
servo(ang)
time.sleep(delay)
ang+=incr
if dist != -1 and dist <= 50:
targets[ang] = Target(ang, dist)
draw(radarDisplay, targets, ang, dist, fontRenderer)
if ang>165:
ang =25
servo(90)
running = 0
for ev in pygame.event.get():
if ev.type == pygame.KEYDOWN:
if ev.key == pygame.K_RIGHT:
right()
time.sleep(turnTime)
for angle in targets.keys():
if targets[angle].angle >= 330:
targets[angle].angle = targets[angle].angle - 330
print targets[angle].angle
elif targets[angle].angle < 330:
targets[angle].angle = targets[angle].angle + 30
print targets[angle].angle
if ev.key == pygame.K_LEFT:
left()
time.sleep(turnTime)
for angle in targets.keys():
if targets[angle].angle >= 30:
targets[angle].angle = targets[angle].angle - 30
print targets[angle].angle
elif targets[angle].angle < 30:
targets[angle].angle = targets[angle].angle + 330
print targets[angle].angle
if ev.key == pygame.K_UP:
fwd()
time.sleep(driveTime)
for angle in targets.keys():
targets[angle].distance = targets[angle].distance - 10
if ev.key == pygame.K_DOWN:
bwd()
time.sleep(driveTime)
for angle in targets.keys():
targets[angle].distance = targets[angle].distance + 10
if ev.key == pygame.K_1:
exit()
if ev.key == pygame.K_0:
running = 1
if ev.key == pygame.K_2:
for angle in targets.keys():
targets[angle].distance = None
if ev.type == pygame.KEYUP:
if ev.key == pygame.K_LEFT or ev.key == pygame.K_RIGHT or ev.key == pygame.K_UP or ev.key == pygame.K_DOWN:
stop()
while True:
enable_com_timeout(2000)
enc_tgt(1,1,18) #Set encoder targetting. Stop after 4 rotations of both the wheels
time.sleep(.2)
while True:
enc=read_enc_status()
ts=read_timeout_status()
time.sleep(.05)
if enc == 0: #Stop when target is reached
break
if ts==0:
break
if us_map() <20: #If any obstacle is closer than 20 cm, stop
break
disable_servo()
# detect if close is pressed to stop the program
for event in pygame.event.get():
if event.type == pygame.QUIT:
raise KeyboardInterrupt
disable_servo()
pygame.quit()
sys.exit()