def fix_front_dist(x):
i = 0
bias_itg = 0
bias_div = 0
bias_d_last = 0
while i <= 50:
a = sonic_distance.sonic_detect()
if abs(x - a) >= 3:
bias_d = a - x
bias_itg += bias_d
bias_div = bias_d_last - bias_d
output = bias_d * 1.5 #+bias_itg*0.01+bias_div*0.01 (-75,75)
if output >= 0:
if output >= 75:
output = 75
back(25 + output)
else:
if output <= -75:
output = -75
front(25 + (-output))
time.sleep(0.04)
bias_d_last = bias_d
else:
i += 2
stop()
time.sleep(0.04)
print("Move to front distance:%dcm done" % x)
def sonic_scan():
scan = []
scan_b = []
for i in range(25, 120, 3):
p.ChangeDutyCycle(i / 10)
time.sleep(0.08)
p.ChangeDutyCycle(0)
a = sonic_distance.sonic_detect()
scan.append(a)
for i in range(120, 25, -3):
p.ChangeDutyCycle(i / 10)
time.sleep(0.08)
p.ChangeDutyCycle(0)
a = sonic_distance.sonic_detect()
scan_b.append(a)
return [scan, scan_b]
def rada():
scanner = []
for i in range(1, 180, 3):
p.ChangeDutyCycle(i * 0.05 + 3)
time.sleep(1)
a = sonic_distance.sonic_detect()
scanner.append(a)
print(scanner)
scanner = []
def sonic_scan_r():
scan_r=[]
#
for i in range(120,25,-3):
p.ChangeDutyCycle(i/10)
time.sleep(0.08)
p.ChangeDutyCycle(0)
a=sonic_distance.sonic_detect()
scan_r.append(a)
return scan_r
def sonic_scan_l():
scan_l=[]
#range(25,120,3) means PWM duty cycle*10
#PWM duty cycle 2.5 to 12 is servo angle start from 0 end in about 180 degree
for i in range(25,120,3):
p.ChangeDutyCycle(i/10)
time.sleep(0.08)
p.ChangeDutyCycle(0)
a=sonic_distance.sonic_detect()
scan_l.append(a)
return scan_l
Red = [255, 0, 0]
Green = [0, 255, 0]
Yellow = [200, 140, 0]
pygame.display.init()
while 1:
#update screen to black
screen.fill([0, 0, 0])
pygame.display.flip()
start_angle = 0
iter_angle = 0
for i in range(25, 120, 3):
angle = start_angle + iter_angle * 6.2
p.ChangeDutyCycle(i / 10)
time.sleep(0.08)
p.ChangeDutyCycle(0)
dis = sonic_distance.sonic_detect()
coordinate = get_cordinate(dis, angle)
if dis <= 30:
pygame.draw.line(screen, Red, [320, 480], coordinate, 2)
pygame.display.flip()
elif dis >= 60:
pygame.draw.line(screen, Green, [320, 480], coordinate, 2)
pygame.display.flip()
else:
pygame.draw.line(screen, Yellow, [320, 480], coordinate, 2)
pygame.display.flip()
iter_angle += 1
iter_angle = 0
screen.fill([0, 0, 0])
pygame.display.flip()