from pybricks.pupdevices import Motor
from pybricks.hubs import InventorHub
from pybricks.parameters import Color, Direction, Port
from pybricks.tools import multitask, run_task, wait
from umath import floor, sqrt
# ---------------- CONFIGURATION ---------------- #
class MotorConfig:
def __init__(self, port, direction=Direction.CLOCKWISE, gear=None, speed=200, duty_limit=50):
self.port = port
self.direction = direction
self.gear = gear
self.speed = speed
self.duty_limit = duty_limit
CONFIG = {
"horizontal": MotorConfig(
port=Port.D, direction=Direction.CLOCKWISE, gear=[20, 24], speed=300, duty_limit=25
),
"vertical-main": MotorConfig(
port=Port.A, direction=Direction.CLOCKWISE, gear=[36, 20], speed=300, duty_limit=50
),
"vertical-aux": MotorConfig(
port=Port.F, direction=Direction.COUNTERCLOCKWISE, gear=[36, 20], speed=300, duty_limit=50
),
"pen": MotorConfig(
port=Port.B, direction=Direction.CLOCKWISE, speed=200, duty_limit=23
),
}
STEP_SIZE_IN_DEG = 2 # space between "coords"
LEFT_MARGIN_IN_DEG = 0
TOP_MARGIN_IN_DEG = 0
LEFT_MARGIN_IN_STEPS = 25 # int(floor(LEFT_MARGIN_IN_DEG / STEP_SIZE_IN_DEG))
TOP_MARGIN_IN_STEPS = int(floor(TOP_MARGIN_IN_DEG / STEP_SIZE_IN_DEG))
WHILE_NOT_DONE_WAIT_IN_MS = 10
J_BRICK_ISH_COORDS = [
[52, 6, 11],
[114, False, False],
[68, 129, False],
[162, 375, False],
[98, False, False],
[False, False, False], ## Detail starts
[92, 342, False],
[136, False, False],
[False, 336, False],
[138, 339, False],
[136, 327, False],
[130, 318, False],
[128, False, False],
[124, 312, False],
[84, False, False],
[80, 294, False],
[102, False, False],
[False, False, False], ## Omitted details
[92, 261, False],
[70, False, False],
[62, 243, False],
[94, False, False],
[92, 228, False],
[82, 210, False],
[48, False, False],
[40, 192, False],
[56, False, False],
[False, 186, False],
[60, False, False],
[62, 180, False],
[64, 177, False],
[70, False, False],
[64, False, False],
[False, 174, False],
[60, False, False],
[58, 177, False],
[52, 174, False],
[48, 168, False],
[44, 162, False],
[22, False, False],
[False, False, False], ## Detail ends
[4, 129, False],
[52, 6, 10],
]
COORDS = J_BRICK_ISH_COORDS
# ---------------- CORE CLASSES ---------------- #
class Axis:
"""Handles a single motor axis and its calibration routine."""
def __init__(self, config: MotorConfig, config2: MotorConfig=None):
self.motor = Motor(config.port, config.direction, config.gear)
self.speed = config.speed
self.duty_limit = config.duty_limit
self.min_angle = 0
self.max_angle = 0
self.motor2 = Motor(config2.port, config2.direction, config2.gear) if config2 is not None else None
async def calibrate(self, direction_positive=True):
"""Run motor to both ends and measure full range."""
neg_speed = -self.speed if direction_positive else self.speed
pos_speed = self.speed if direction_positive else -self.speed
if self.motor2 is not None:
await multitask(
self.motor.run_until_stalled(neg_speed, duty_limit=self.duty_limit),
self.motor2.run_until_stalled(neg_speed, duty_limit=self.duty_limit),
race=False,
)
while not self.motor.done() or not self.motor2.done():
await wait(WHILE_NOT_DONE_WAIT_IN_MS)
min_end_main, min_end_aux = self.motor.angle(), self.motor2.angle()
print(f" [MIN END] main: {min_end_main}; aux: {min_end_aux}") ## DEBUG!!
min_end = min(min_end_main, min_end_aux)
print(f" [MIN END] --> {min_end}") ## DEBUG!!
await multitask(
self.motor.run_until_stalled(pos_speed, duty_limit=self.duty_limit),
self.motor2.run_until_stalled(pos_speed, duty_limit=self.duty_limit),
race=False,
)
while not self.motor.done() or not self.motor2.done():
await wait(WHILE_NOT_DONE_WAIT_IN_MS)
max_end_main, max_end_aux = self.motor.angle(), self.motor2.angle()
print(f" [MAX END] main: {max_end_main}; aux: {max_end_aux}") ## DEBUG!!
max_end = min(max_end_main, max_end_aux)
print(f" [MAX END] --> {max_end}") ## DEBUG!!
self.motor.reset_angle(max_end)
self.motor2.reset_angle(max_end)
await multitask(
self.motor.run_target(self.speed, max_end),
self.motor2.run_target(self.speed, max_end),
race=False,
)
while not self.motor.done() or not self.motor2.done():
await wait(WHILE_NOT_DONE_WAIT_IN_MS)
self.min_angle, self.max_angle = min_end, max_end
print(f"Axis range: {self.max_angle - self.min_angle} deg") ## DEBUG!!
else:
min_end = await self.motor.run_until_stalled(neg_speed, duty_limit=self.duty_limit)
max_end = await self.motor.run_until_stalled(pos_speed, duty_limit=self.duty_limit)
self.motor.reset_angle(max_end)
await self.motor.run_target(self.speed, max_end)
self.min_angle, self.max_angle = min_end, max_end
print(f"Axis range: {self.max_angle - self.min_angle} deg") ## DEBUG!!
async def move_to(self, num_steps = 1, direction_positive=True, axis='X'):
speed = self.speed if direction_positive else -self.speed
end_angle = STEP_SIZE_IN_DEG * num_steps
if self.motor2 is not None:
await multitask(
self.motor.run_angle(speed, end_angle),
self.motor2.run_angle(speed, end_angle),
race=False,
)
while not self.motor.done() or not self.motor2.done():
await wait(WHILE_NOT_DONE_WAIT_IN_MS)
else:
await self.motor.run_angle(speed, end_angle)
class Pen:
"""Handles pen up/down motion."""
def __init__(self, config: MotorConfig):
self.motor = Motor(config.port)
self.speed = config.speed
self.duty_limit = config.duty_limit
self.up_end = 0
self.down_end = 0
async def calibrate(self):
"""Find mechanical endpoints for pen up/down."""
self.up_end = await self.motor.run_until_stalled(-self.speed, duty_limit=self.duty_limit)
self.down_end = await self.motor.run_until_stalled(self.speed, duty_limit=self.duty_limit)
self.motor.reset_angle(self.down_end)
await self.motor.run_target(self.speed, self.up_end)
print(f"Pen range: {self.down_end - self.up_end} deg") ## DEBUG!!
async def up(self):
await self.motor.run_target(-self.speed, self.up_end)
async def down(self):
await self.motor.run_target(self.speed, self.down_end)
def get_move_to_config_x(prev, curr):
num_steps = abs(prev - curr)
direction_positive = prev < curr
return num_steps, direction_positive
def get_move_to_config_y(prev, curr):
num_steps = abs(prev - curr)
direction_positive = prev > curr
return num_steps, direction_positive
SLOW_SPEED = 300
FAST_SPEED = 300
def get_distance_from_time_and_speed_in_steps (time_in_ms, speed_in_deg_per_sec):
# speed_in_steps_per_sec = float(int((speed_in_deg_per_sec / STEP_SIZE_IN_DEG) * 10) / 10)
speed_in_steps_per_sec = speed_in_deg_per_sec / STEP_SIZE_IN_DEG
time_in_sec = time_in_ms / 1000
return time_in_sec * speed_in_steps_per_sec
def get_distance_from_coords (prev, curr):
return abs(prev - curr)
def get_deg_from_steps (steps):
return steps * STEP_SIZE_IN_DEG
def get_target_speed (target_distance_in_steps, reference_distance_in_steps, reference_speed_in_deg_per_sec):
target_distance = get_deg_from_steps(target_distance_in_steps)
reference_distance = get_deg_from_steps(reference_distance_in_steps)
return target_distance / (reference_distance / reference_speed_in_deg_per_sec)
async def move_xy(x_axis, prev_x, curr_x, y_axis, prev_y, curr_y):
distance_x = get_distance_from_coords(prev_x, curr_x)
distance_y = get_distance_from_coords(prev_y, curr_y)
direction_x = -1 if prev_x > curr_x else 1
direction_y = -1 if prev_y < curr_y else 1
# We set the "slower" speed to the motor with the longer run time/distance
if distance_x < distance_y:
speed_y = SLOW_SPEED
speed_x = get_target_speed(distance_x, distance_y, speed_y)
elif distance_y < distance_x:
speed_x = SLOW_SPEED
speed_y = get_target_speed(distance_y, distance_x, speed_x)
else:
speed_x = FAST_SPEED
speed_y = FAST_SPEED
x_target_speed = direction_x * speed_x
y_target_speed = direction_y * speed_y
x_target_angle = direction_x * get_deg_from_steps(distance_x)
y_target_angle = direction_y * get_deg_from_steps(distance_y)
await multitask(
x_axis.motor.run_angle(speed_x, x_target_angle),
y_axis.motor.run_angle(speed_y, y_target_angle),
y_axis.motor2.run_angle(speed_y, y_target_angle),
race=False,
)
return curr_x, curr_y
# ---------------- MAIN ROUTINE ---------------- #
print("Configuring pen...")
pen = Pen(CONFIG["pen"])
print("Configuring axes...")
x_axis = Axis(CONFIG["horizontal"])
y_axis = Axis(CONFIG["vertical-main"], CONFIG["vertical-aux"])
async def main():
print("Calibrating pen...")
await pen.calibrate()
print("Calibrating axes...")
await x_axis.calibrate(direction_positive=False) # horizontal starts left
await y_axis.calibrate(direction_positive=True) # vertical starts top
prev_x = LEFT_MARGIN_IN_STEPS
print(f'[X] Go to: {prev_x}') ## DEBUG!!
await x_axis.move_to(prev_x, True, 'X') # move right
prev_y = TOP_MARGIN_IN_STEPS
print(f'[Y] Go to: {prev_y}') ## DEBUG!!
await y_axis.move_to(prev_y, False, 'Y') # move down
await wait(500)
for i, (X, Y, Z) in enumerate(COORDS):
if (X != False) and (Y != False): # moving at an angle
prev_x, prev_y = await move_xy(x_axis, prev_x, X, y_axis, prev_y, Y)
elif (X != False) and (Y == False): # moving along x axis
num_steps, direction_positive = get_move_to_config_x(prev_x, X)
await x_axis.move_to(num_steps, direction_positive, 'X')
prev_x = X
elif (Y != False) and (X == False): # moving along y axis
num_steps, direction_positive = get_move_to_config_y(prev_y, Y)
await y_axis.move_to(num_steps, direction_positive, 'Y')
prev_y = Y
if Z != False: # pen up or down
if Z == 10:
print('Pen Up')
await pen.up()
elif Z == 11:
print('Pen Down')
await pen.down()
print("Done!")
run_task(main())
To embed this program on your website, copy the following code and paste it into your website's HTML: