# -*- coding: utf-8 -*
import serial
import math
import time
import RPi.GPIO as GPIO





servoPIN = 18
GPIO.setmode(GPIO.BCM)
GPIO.setup(servoPIN, GPIO.OUT)


fire_gun = GPIO.PWM(servoPIN, 50)   #GPIO 5 for PWM with 50Hz
fire_gun.start(2.5) #Initialization



"""
def gunRight():
    #fire_gun = GPIO.PWM(servoPIN, 50)  #GPIO 5 for PWM with 50Hz
    #fire_gun.start(2.5)    #Initialization

    print "Shot Fire The First Net Gun"
    fire_gun.ChangeDutyCycle(4.9)
    time.sleep(0.4)
    fire_gun.ChangeDutyCycle(7.2)
    time.sleep(0.4)
    print "Servo Motor Back To Rest Position"
    fire_gun.stop()
"""




ser = serial.Serial("/dev/ttyUSB0", 115200)

def getTFminiData():
    while True:
        count = ser.in_waiting
        if count > 8:
            recv = ser.read(9)
            ser.reset_input_buffer()
            if recv[0] == 'Y' and recv[1] == 'Y': # 0x59 is 'Y'
                low = int(recv[2].encode('hex'), 16)
                high = int(recv[3].encode('hex'), 16)
                distance = low + high * 256
                #My unit conversion starts here
                dist = float(distance) /100 
                print(distance)
                print "Distance to object is:",dist,"Meters"
                #GPIO.setup(TRIG, GPIO.OUT)
                #GPIO.setup(ECHO, GPIO.IN)

                #if dist <= 0.70:
                    #GPIO.output(5, True)   





try:
    if ser.is_open == False:
       ser.open()
       for i in range(500):
           getTFminiData()
           if dist <= 0.70:
          fire_gun.ChangeDutyCycle(4.9)
              time.sleep(0.4)
              GPIO.cleanup()


except KeyboardInterrupt:   # Ctrl+C
    if ser != None:
        #GPIO.cleanup()
    ser.close()