import time
import RPi.GPIO as GPIO

start_time = time.monotonic()

# GPIO Variables
pin = [18, 27]
freq= 50

GPIO.setmode(GPIO.BCM)

# Setup PWM pin
GPIO.setup(pin[0], GPIO.OUT)
p = GPIO.PWM(pin[0], freq)


GPIO.setup(pin[1], GPIO.IN, pull_up_down=GPIO.PUD_UP) #Escape button

def angle_write(angle, pwm_pin):
    #1ms -90
    #2ms 90
    write_duty = -(0.0625/180)*(angle + 90) + 0.125
    pwm_pin.ChangeDutyCycle( 100 * write_duty )
    return write_duty

escape = False; 

p.start(9.375) # Start pwm at 9.375 duty cycle
while(not escape):
    # Incorrect user input results in undefined behavior
    angle = input("Type in an motor angle between [-90 90]:\n") 
    freq = angle_write(float(angle), p) # Change frequency to user input
    print(freq)
    # Escape button logic
    if ( not GPIO.input(pin[1]) ):
        print("Quit")
        escape = True

    time.sleep(1) # Do PWM for a few seconds and poll again
    
    if( input("QUIT? n/y\n") == 'y' ):
      escape = True
        
p.stop() # Stop PWM
GPIO.cleanup() # Cleanup GPIO pins