import math
import serial
import RPi.GPIO as gpio
import time

ser = serial.Serial('/dev/ttyUSB0', 115200)
gpio.setmode(gpio.BCM)
gpio.setup(13, gpio.OUT)
pwm = gpio.PWM(13, 100)
pwm.start(5)
verbose = True

print("Initializing Grbl...")
ser.write("\r\n\r\n")
time.sleep(2)
ser.write(b'$RST=#\n')
time.sleep(1)
ser.flushInput()

def goToBin(bin):
    adjustedBin = math.floor(bin / 2)
    distance = adjustedBin * 18
    delay = 0.5 + 0.93 * adjustedBin
    command = 'G0 -X'
    command += distance
    command += '\n'
    ser.write(command.encode('utf-8'))
    # s.write("$C\n")
    while True:
        grbl_out = s.readline().strip() # Wait for grbl response with carriage return
        if grbl_out.find('error') >= 0 :
            print "REC:",grbl_out
            print "  Grbl reported error!"
            quit()
        elif grbl_out.find('ok') >= 0 :
            if verbose: print 'REC:',grbl_out
            break
    # time.sleep(delay)
    if bin % 2 == 0: # tilt to right
        pwm.ChangeDutyCycle(0)
        time.sleep(1)
        pwm.ChangeDutyCycle(20)
    else:
        pwm.ChangeDutyCycle(40)
        time.sleep(1)
        pwm.ChangeDutyCycle(20)
    ser.write(b'G0 X0\n')
    while True:
        grbl_out = s.readline().strip() # Wait for grbl response with carriage return
        if grbl_out.find('error') >= 0 :
            print "REC:",grbl_out
            print "  Grbl reported error!"
            quit()
        elif grbl_out.find('ok') >= 0 :
            if verbose: print 'REC:',grbl_out
            break
    #time.sleep(delay)