Table of Contents
Recently I discovered Pybricks and their announcement that Pybricks will support the Powered Up hubs in the near future.
I was able to buy a Technic hub, 2 XL motors and 1 L motor for a nice price on Bricklink and I decided that my old Lego 8094 set was the perfect candidate to test it on.
Prepare the Lego set
First I had to make some modifications such as removing the old motors and replace them with the Powered Up motors and making the pen lift/drop mechanism driven as well:
I made sure that the motors drive all axis directly because I wanted to use the sensorless load measurement option that these Powered Up motors support.
The sensorless load measurement makes it possible to move the motor until it stalls because of an obstacle and use that for homing the X, Y and Z-axis.
Pybricks
I was lucky enough to get access to the early beta testing program of Pybricks so I could get started.
All code I created for this project are in beta!!! It is still a proof of concept so the code is absolutely not optimized yet!!
The first important step was creating a homing routine to detect the limits of the axis so the machine knows its current position.
from pybricks.hubs import CPlusHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Stop
from pybricks.tools import wait
import math
TechnicHub = CPlusHub()
# theoretical XY limits
Xmax = -6000
Ymax = -6000
X_DegMm = 0
Y_DegMm = 0
Xcurr = 0
Ycurr = 0
# Define the XYZ axis motor ports
Xaxis = Motor(Port.B)
Yaxis = Motor(Port.A)
Zaxis = Motor(Port.C)
def G28_homingXYZ(Xmax, Ymax):
global Xcurr
global Ycurr
global X_DegMm
global Y_DegMm
# Move the Z-axis to its 0deg reference location
print("Homing Z-axis: ")
stallAngle = Zaxis.run_until_stalled(90,Stop.COAST,30)
Zaxis.reset_angle(90)
Zaxis.run_target(90,0,Stop.COAST)
print("finished")
# Home the Y-axis
print("Homing Y-axis: ")
stallAngle = Yaxis.run_until_stalled(+3600,Stop.COAST,20)
Yaxis.reset_angle(0)
Yaxis.run_target(3600, Ymax, Stop.COAST)
Ymax = Yaxis.run_until_stalled(-3600,Stop.COAST,20)
print("Ymax = " + str(Ymax) + "°")
print("finished")
# Home the X-axis
print("Homing X-axis: ")
stallAngle = Xaxis.run_until_stalled(3600,Stop.COAST,20)
Xaxis.reset_angle(0)
Xaxis.run_target(3600, Xmax, Stop.COAST)
Xmax = Xaxis.run_until_stalled(-3600,Stop.COAST,20)
print("Xmax = " + str(Xmax) + "°")
print("finished")
X_DegMm = Xmax / 145
Y_DegMm = Ymax / 140
Xcurr = 145
Ycurr = 140
print("Current location = X", Xcurr, " Y", Ycurr)
# Say hello :)
print("Lego 8094 says hello!")
print("Battery voltage = " + str(TechnicHub.battery.voltage()) + "mV")
G28_homingXYZ(Xmax, Ymax)
After homing was working I had to make sure the machine can move in XY along a line other than 0°, 45° and 90° as that was the drawback of the original set. To be able to make a strait line from A to B along an angled (other than 45°) the feedrate for each motor needs to be proportional.
def G1move(X,Y):
global Xcurr
global Ycurr
ERROR = False
print("G1 X", X, " Y", Y)
if X > 145:
print("X+ axis overtravel!!!")
ERROR = True
elif X < 0:
print("X- axis overtravel!!!")
ERROR = True
if Y > 140:
print("Y+ axis overtravel!!!")
ERROR = True
elif Y < 0:
print("Y- axis overtravel!!!")
ERROR = True
if ERROR == False:
PenDown()
#print(" Xcurr", Xcurr, " Ycurr", Ycurr)
Xinc = X - Xcurr
Yinc = Y - Ycurr
#print(" Xinc", Xinc, " Yinc", Yinc)
Xangle = Xaxis.angle() + (Xinc * X_DegMm)
Yangle = Yaxis.angle() + (Yinc * Y_DegMm)
#print(" X_DegMm", X_DegMm, " Y_DegMm", Y_DegMm)
#print(" Xangle", Xangle, " Yangle", Yangle)
factor = min(X,Y) / max(X,Y)
if min(X,Y) == X:
Xfeed = 500*factor
Yfeed = 500
#print("Xfeed = ", Xfeed, " Yfeed = ", Yfeed)
Xaxis.run_target(Xfeed, Xangle, Stop.COAST, False)
Yaxis.run_target(Yfeed, Yangle, Stop.COAST, False)
else:
Xfeed = 500
Yfeed = 500*factor
#print("Xfeed = ", Xfeed, " Yfeed = ", Yfeed)
Xaxis.run_target(Xfeed, Xangle, Stop.COAST, False)
Yaxis.run_target(Yfeed, Yangle, Stop.COAST, False)
wait(1000) # give the motor time to start moving
while True:
if Xaxis.speed() == 0 and Yaxis.speed() == 0:
break
#print(" Xang", Xaxis.angle(), " Yang", Yaxis.angle())
Xcurr = X
Ycurr = Y
I tried to draw the Pybricks logo and write Pybricks beneath it but I seem to run into some memory problems. I created a postprocessor in my CAM system to get the correct coordinates for the drawing but I forget to change the output from 0.001mm accuracy to something like 0.1mm accuracy. I hope that that will reduce the memory issue I’m facing.
I also really need to find some time to fine tune the used calculations.
Feel free to play around and suggest modifications 🙂
Complete code
from pybricks.hubs import CPlusHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Stop
from pybricks.tools import wait
import math
TechnicHub = CPlusHub()
# theoretical XY limits
Xmax = -6000
Ymax = -6000
X_DegMm = 0
Y_DegMm = 0
Xcurr = 0
Ycurr = 0
# Define the XYZ axis motor ports
Xaxis = Motor(Port.B)
Yaxis = Motor(Port.A)
Zaxis = Motor(Port.C)
def G28_homingXYZ(Xmax, Ymax):
global Xcurr
global Ycurr
global X_DegMm
global Y_DegMm
# Move the Z-axis to its 0deg reference location
print("Homing Z-axis: ")
stallAngle = Zaxis.run_until_stalled(90,Stop.COAST,30)
Zaxis.reset_angle(90)
Zaxis.run_target(90,0,Stop.COAST)
print("finished")
# Home the Y-axis
print("Homing Y-axis: ")
stallAngle = Yaxis.run_until_stalled(+3600,Stop.COAST,20)
Yaxis.reset_angle(0)
Yaxis.run_target(3600, Ymax, Stop.COAST)
Ymax = Yaxis.run_until_stalled(-3600,Stop.COAST,20)
print("Ymax = " + str(Ymax) + "°")
print("finished")
# Home the X-axis
print("Homing X-axis: ")
stallAngle = Xaxis.run_until_stalled(3600,Stop.COAST,20)
Xaxis.reset_angle(0)
Xaxis.run_target(3600, Xmax, Stop.COAST)
Xmax = Xaxis.run_until_stalled(-3600,Stop.COAST,20)
print("Xmax = " + str(Xmax) + "°")
print("finished")
X_DegMm = Xmax / 145
Y_DegMm = Ymax / 140
Xcurr = 145
Ycurr = 140
print("Current location = X", Xcurr, " Y", Ycurr)
# NOTE PenDown(): puts the pen down on the paper
def PenDown():
Zaxis.run_target(90,-500,Stop.COAST)
# NOTE PenUp(): retracts the pen away from the paper
def PenUp():
Zaxis.run_target(90,0,Stop.COAST)
def G0move(X,Y):
global Xcurr
global Ycurr
ERROR = False
print("G0 X", X, " Y", Y)
if X > 145:
print("X+ axis overtravel!!!")
ERROR = True
elif X < 0:
print("X- axis overtravel!!!")
ERROR = True
if Y > 140:
print("Y+ axis overtravel!!!")
ERROR = True
elif Y < 0:
print("Y- axis overtravel!!!")
ERROR = True
if ERROR == False:
PenUp()
#print(" Xcurr", Xcurr, " Ycurr", Ycurr)
Xinc = X - Xcurr
Yinc = Y - Ycurr
#print(" Xinc", Xinc, " Yinc", Yinc)
Xangle = Xaxis.angle() + (Xinc * X_DegMm)
Yangle = Yaxis.angle() + (Yinc * Y_DegMm)
#print(" X_DegMm", X_DegMm, " Y_DegMm", Y_DegMm)
#print(" Xangle", Xangle, " Yangle", Yangle)
Xfeed = 3000
Yfeed = 3000
#print(" Xang", Xaxis.angle(), " Yang", Yaxis.angle())
Xaxis.run_target(Xfeed, Xangle, Stop.COAST, False)
Yaxis.run_target(Yfeed, Yangle, Stop.COAST, False)
wait(1000) # give the motor time to start moving
while True:
if Xaxis.speed() == 0 and Yaxis.speed() == 0:
break
#print(" Xang", Xaxis.angle(), " Yang", Yaxis.angle())
Xcurr = X
Ycurr = Y
def G1move(X,Y):
global Xcurr
global Ycurr
ERROR = False
print("G1 X", X, " Y", Y)
if X > 145:
print("X+ axis overtravel!!!")
ERROR = True
elif X < 0:
print("X- axis overtravel!!!")
ERROR = True
if Y > 140:
print("Y+ axis overtravel!!!")
ERROR = True
elif Y < 0:
print("Y- axis overtravel!!!")
ERROR = True
if ERROR == False:
PenDown()
#print(" Xcurr", Xcurr, " Ycurr", Ycurr)
Xinc = X - Xcurr
Yinc = Y - Ycurr
#print(" Xinc", Xinc, " Yinc", Yinc)
Xangle = Xaxis.angle() + (Xinc * X_DegMm)
Yangle = Yaxis.angle() + (Yinc * Y_DegMm)
#print(" X_DegMm", X_DegMm, " Y_DegMm", Y_DegMm)
#print(" Xangle", Xangle, " Yangle", Yangle)
factor = min(X,Y) / max(X,Y)
if min(X,Y) == X:
Xfeed = 500*factor
Yfeed = 500
#print("Xfeed = ", Xfeed, " Yfeed = ", Yfeed)
Xaxis.run_target(Xfeed, Xangle, Stop.COAST, False)
Yaxis.run_target(Yfeed, Yangle, Stop.COAST, False)
else:
Xfeed = 500
Yfeed = 500*factor
#print("Xfeed = ", Xfeed, " Yfeed = ", Yfeed)
Xaxis.run_target(Xfeed, Xangle, Stop.COAST, False)
Yaxis.run_target(Yfeed, Yangle, Stop.COAST, False)
wait(1000) # give the motor time to start moving
while True:
if Xaxis.speed() == 0 and Yaxis.speed() == 0:
break
#print(" Xang", Xaxis.angle(), " Yang", Yaxis.angle())
Xcurr = X
Ycurr = Y
# NOTE ReturnToReferencePosition(Xmax, Ymax)
def ReturnToReferencePosition():
global Xcurr
global Ycurr
G0move(70,130)
# Say hello :)
print("Lego 8094 says hello!")
print("Battery voltage = " + str(TechnicHub.battery.voltage()) + "mV")
G28_homingXYZ(Xmax, Ymax)
G0move(95.677,67.667)
G1move(96.166,64.577)
G1move(97.587,61.789)
G1move(99.799,59.577)
G1move(102.587,58.157)
G1move(105.677,57.667)
G1move(113.659,57.667)
G1move(116.749,58.157)
G1move(119.536,59.577)
G1move(121.749,61.789)
G1move(123.169,64.577)
G1move(123.659,67.667)
G1move(123.659,104.908)
G1move(123.169,107.998)
G1move(121.749,110.785)
G1move(119.536,112.998)
G1move(116.749,114.418)
G1move(113.659,114.908)
G1move(30.194,114.908)
G1move(27.104,114.418)
G1move(24.316,112.998)
G1move(22.104,110.785)
G1move(20.684,107.998)
G1move(20.194,104.908)
G1move(20.194,67.667)
G1move(20.684,64.577)
G1move(22.104,61.789)
G1move(24.316,59.577)
G1move(27.104,58.157)
G1move(30.194,57.667)
G1move(39.095,57.667)
G1move(42.185,58.157)
G1move(44.973,59.577)
G1move(47.185,61.789)
G1move(48.606,64.577)
G1move(49.095,67.667)
G1move(95.677,67.667)
G0move(105.289,76.983)
G1move(105.289,76.983)
G1move(105.289,67.608)
G1move(114.477,67.608)
G1move(114.477,105.108)
G1move(30.289,105.108)
G1move(30.289,67.608)
G1move(39.47,67.608)
G1move(39.477,76.983)
G1move(47.539,76.983)
G1move(47.539,80.546)
G1move(50.352,80.546)
G1move(50.352,76.983)
G1move(56.727,76.983)
G1move(56.727,80.546)
G1move(59.727,80.546)
G1move(59.727,76.983)
G1move(66.102,76.983)
G1move(66.102,80.546)
G1move(69.102,80.546)
G1move(69.102,76.983)
G1move(75.477,76.983)
G1move(75.477,80.546)
G1move(78.477,80.546)
G1move(78.477,76.983)
G1move(84.852,76.983)
G1move(84.852,80.546)
G1move(87.852,80.546)
G1move(87.852,76.983)
G1move(94.227,76.983)
G1move(94.227,80.546)
G1move(97.227,80.546)
G1move(97.227,76.983)
G1move(105.289,76.983)
G0move(97.048,96.992)
G1move(97.048,96.992)
G1move(96.161,98.735)
G1move(94.778,100.117)
G1move(93.036,101.005)
G1move(91.104,101.311)
G1move(89.173,101.005)
G1move(87.431,100.117)
G1move(86.048,98.735)
G1move(85.16,96.992)
G1move(84.854,95.061)
G1move(85.16,93.13)
G1move(86.048,91.387)
G1move(87.431,90.005)
G1move(89.173,89.117)
G1move(91.104,88.811)
G1move(93.036,89.117)
G1move(94.778,90.005)
G1move(96.161,91.387)
G1move(97.048,93.13)
G1move(97.354,95.061)
G1move(97.048,96.992)
G0move(59.623,97.143)
G1move(59.623,97.143)
G1move(58.736,98.885)
G1move(57.353,100.268)
G1move(55.611,101.156)
G1move(53.679,101.462)
G1move(51.748,101.156)
G1move(50.006,100.268)
G1move(48.623,98.885)
G1move(47.735,97.143)
G1move(47.429,95.212)
G1move(47.735,93.28)
G1move(48.623,91.538)
G1move(50.006,90.155)
G1move(51.748,89.268)
G1move(53.679,88.962)
G1move(55.611,89.268)
G1move(57.353,90.155)
G1move(58.736,91.538)
G1move(59.623,93.28)
G1move(59.929,95.212)
G1move(59.623,97.143)
G0move(18.82,36.252)
G1move(18.82,36.252)
G1move(18.82,48.752)
G1move(22.57,48.752)
G1move(23.82,48.156)
G1move(24.237,47.561)
G1move(24.654,46.371)
G1move(24.654,44.585)
G1move(24.237,43.395)
G1move(23.82,42.799)
G1move(22.57,42.204)
G1move(18.82,42.204)
G0move(34.237,44.585)
G1move(34.237,44.585)
G1move(36.737,36.252)
G0move(36.737,36.252)
G0move(39.237,44.585)
G1move(39.237,44.585)
G1move(36.737,36.252)
G1move(35.904,33.871)
G1move(35.07,32.68)
G1move(34.237,32.085)
G1move(33.82,32.085)
G0move(48.82,36.252)
G1move(48.82,36.252)
G1move(48.82,48.752)
G1move(52.57,48.752)
G1move(53.82,48.156)
G1move(54.237,47.561)
G1move(54.654,46.371)
G1move(54.654,45.18)
G1move(54.237,43.99)
G1move(53.82,43.395)
G1move(52.57,42.799)
G0move(48.82,42.799)
G1move(48.82,42.799)
G1move(52.57,42.799)
G1move(53.82,42.204)
G1move(54.237,41.609)
G1move(54.654,40.418)
G1move(54.654,38.633)
G1move(54.237,37.442)
G1move(53.82,36.847)
G1move(52.57,36.252)
G1move(48.82,36.252)
G0move(63.82,36.252)
G1move(63.82,36.252)
G1move(63.82,44.585)
G0move(63.82,41.014)
G1move(63.82,41.014)
G1move(64.237,42.799)
G1move(65.07,43.99)
G1move(65.904,44.585)
G1move(67.154,44.585)
G0move(78.82,48.156)
G1move(78.82,48.156)
G1move(79.237,47.561)
G1move(79.654,48.156)
G1move(79.237,48.752)
G1move(78.82,48.156)
G0move(79.237,44.585)
G1move(79.237,44.585)
G1move(79.237,36.252)
G0move(98.82,42.799)
G1move(98.82,42.799)
G1move(97.987,43.99)
G1move(97.154,44.585)
G1move(95.904,44.585)
G1move(95.07,43.99)
G1move(94.237,42.799)
G1move(93.82,41.014)
G1move(93.82,39.823)
G1move(94.237,38.037)
G1move(95.07,36.847)
G1move(95.904,36.252)
G1move(97.154,36.252)
G1move(97.987,36.847)
G1move(98.82,38.037)
G0move(108.82,36.252)
G1move(108.82,36.252)
G1move(108.82,48.752)
G0move(112.987,44.585)
G1move(112.987,44.585)
G1move(108.82,38.633)
G0move(110.487,41.014)
G1move(110.487,41.014)
G1move(113.404,36.252)
G0move(128.404,42.799)
G1move(128.404,42.799)
G1move(127.987,43.99)
G1move(126.737,44.585)
G1move(125.487,44.585)
G1move(124.237,43.99)
G1move(123.82,42.799)
G1move(124.237,41.609)
G1move(125.07,41.014)
G1move(127.154,40.418)
G1move(127.987,39.823)
G1move(128.404,38.633)
G1move(128.404,38.037)
G1move(127.987,36.847)
G1move(126.737,36.252)
G1move(125.487,36.252)
G1move(124.237,36.847)
G1move(123.82,38.037)
G0move(123.82,38.037)
ReturnToReferencePosition()