You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
102 lines
3.2 KiB
Python
102 lines
3.2 KiB
Python
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(13)
|
|
verbose = True
|
|
|
|
print("[ INFO ] Initializing Grbl...")
|
|
ser.write(b'\r\n\r\n')
|
|
time.sleep(2)
|
|
ser.write(b'$RST=#\n')
|
|
time.sleep(1)
|
|
ser.flushInput()
|
|
time.sleep(0.25)
|
|
ser.write(b'$X\n')
|
|
print("[ INFO ] Grbl is ready.")
|
|
|
|
def goToBin(bin):
|
|
print("[ INFO ] Delivering item to bin: " + str(bin))
|
|
adjustedBin = math.floor(bin / 2)
|
|
if adjustedBin > 11:
|
|
print("[ INFO ] All bins full! Using overflow bin.")
|
|
bin = 0;
|
|
adjustedBin = 0;
|
|
distance = adjustedBin * 18
|
|
delay = 0.5 + 0.93 * adjustedBin
|
|
command = '$J=X-'
|
|
command += str(distance)
|
|
command += ' F2000'
|
|
print("[ INFO ] Sending command to Grbl: " + command)
|
|
command += '\n'
|
|
time.sleep(0.25)
|
|
ser.write(command.encode('utf-8'))
|
|
# s.write("$C\n")
|
|
while True:
|
|
grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
|
|
print(grbl_out)
|
|
if int(grbl_out.find('error')) >= 0 :
|
|
print("[ EXIT ] Grbl reported an error.")
|
|
quit()
|
|
elif int(grbl_out.find('ok')) >= 0 :
|
|
if verbose: print('[ INFO ] Grbl message: ',grbl_out)
|
|
break
|
|
print("[ INFO ] Waiting for " + str(delay) + " seconds.")
|
|
time.sleep(delay)
|
|
if bin % 2 == 0: # tilt to left
|
|
print("[ INFO ] Titling motor to left side.")
|
|
pwm.ChangeDutyCycle(5)
|
|
time.sleep(1)
|
|
pwm.ChangeDutyCycle(14)
|
|
else:
|
|
print("[ INFO ] Titling motor to right side.")
|
|
pwm.ChangeDutyCycle(25)
|
|
time.sleep(1)
|
|
pwm.ChangeDutyCycle(14)
|
|
time.sleep(1)
|
|
print("[ INFO ] Sending command to Grbl: G0 X0")
|
|
ser.write(b'$j=X0 F2000\n')
|
|
while True:
|
|
grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
|
|
if int(grbl_out.find('error')) >= 0 :
|
|
print("[ EXIT ] Grbl reported an error.")
|
|
quit()
|
|
elif int(grbl_out.find('ok')) >= 0 :
|
|
if verbose: print('[ INFO ] Grbl message: ',grbl_out)
|
|
break
|
|
print("[ INFO ] Waiting for " + str(delay) + " seconds.")
|
|
time.sleep(delay)
|
|
def stopInput():
|
|
command = '!'
|
|
command += '\n'
|
|
ser.write(command.encode('utf-8'))
|
|
print(command)
|
|
#command2 = str(0x85)
|
|
#ser.write(command2.encode('utf-8'))
|
|
#while True:
|
|
# grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
|
|
# if int(grbl_out.find('error')) >= 0 :
|
|
# print("[ EXIT ] Grbl reported an error.")
|
|
# quit()
|
|
# elif int(grbl_out.find('ok')) >= 0 :
|
|
# if verbose: print('[ INFO ] Grbl message: ',grbl_out)
|
|
# break
|
|
def startInput():
|
|
ser.write(b'$J=G91 Y-5000 F400\n')
|
|
print("intake")
|
|
#x = 0
|
|
#while True and x < 10:
|
|
#x += 1
|
|
#grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
|
|
#if int(grbl_out.find('error')) >= 0 :
|
|
# print("[ EXIT ] Grbl reported an error.")
|
|
# quit()
|
|
#elif int(grbl_out.find('ok')) >= 0 :
|
|
# if verbose: print('[ INFO ] Grbl message: ',grbl_out)
|
|
# break
|