From 52ef77e389ce2b610f3df6ea56523dfec2158192 Mon Sep 17 00:00:00 2001
From: Cole Deck <shark@noreply.example.org>
Date: Tue, 3 Mar 2020 01:11:00 +0000
Subject: [PATCH] Use grbl's recommended command streaming setup

---
 control_motor.py | 38 +++++++++++++++++++++++++++++++++-----
 1 file changed, 33 insertions(+), 5 deletions(-)

diff --git a/control_motor.py b/control_motor.py
index d0a253c..9a6794f 100644
--- a/control_motor.py
+++ b/control_motor.py
@@ -8,15 +8,34 @@ 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 = 'G0 -X'
     command += distance
-    ser.write(command)
-    time.sleep(delay)
+    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)
@@ -25,5 +44,14 @@ def goToBin(bin):
         pwm.ChangeDutyCycle(40)
         time.sleep(1)
         pwm.ChangeDutyCycle(20)
-    ser.write(b'G0 X0')
-    time.sleep(delay)
\ No newline at end of file
+    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)
\ No newline at end of file