import RPi.GPIO as GPIO
import time
import sys

TRIGER = 23
ECHO = 24
SERVO = 18
MAX_DISTANCE = 200.0

data = [[0 for j in range(2)] for i in range(13)]

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(TRIGER, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(SERVO, GPIO.OUT)

Servo = GPIO.PWM(SERVO, 50)
Servo.start(0)

x = -60

def read_distance():
    GPIO.output(TRIGER, GPIO.HIGH)
    time.sleep(0.00001)
    GPIO.output(TRIGER, GPIO.LOW)
    
    while GPIO.input(ECHO) == GPIO.LOW:
        sig_off = time.time()
    while GPIO.input(ECHO) == GPIO.HIGH:
        sig_on = time.time()
    
    duration = (sig_on - sig_off)
    distance = duration * 34000 / 2
    return distance

def servo_angle(angle):
    duty = 2.5 + (12.0 - 2.5) * (angle + 90) / 180
    Servo.ChangeDutyCycle(duty)
    time.sleep(0.1)

def get_min_data(dt):
    minDt = 1000
    dig = 0
    for i in range(13):
        if dt[i][1] != -1 and dt[i][1] < minDt:
            minDt = dt[i][1]
            dig = dt[i][0]
    if minDt == 1000:
        minDt = -1
    return dig, minDt

try:
    while True:
        for i in range(13):
            servo_angle(x)
            cm = read_distance()
            if cm >= 2.0 and cm <= MAX_DISTANCE:
                dist = cm
            else:
                dist = -1
            data[i][0] = x
            data[i][1] = dist
            x += 10
        x = -60
        servo_angle(x)
        time.sleep(0.3)
        dx = get_min_data(data)
        if dx[1] == -1:
            print('NO DETECT')
        else:
            print('digree =', dx[0])
            print('distance =', '{:.2f}'.format(dx[1]))

except KeyboardInterrupt:
    Servo.stop()
    GPIO.cleanup()
    sys.exit()