CoreXY Machine based on FAB25 COBS-USB Serial¶
Boards and Pins¶
FAB25(Moduler Things) | BootCamp Leon2024 | BootCamp Waag2023 Ver0 |
---|---|---|
![]() |
![]() |
![]() |
![]() |
![]() |
|
![]() OA2, OA1, OB1, OB2 |
![]() OA1, OA2, OB1, OB2 |
|
stepperDriver.cpp | ||
#define AIN1_PIN 6 // on D4 | #define AIN1_PIN 7 // on D5 | #define AIN1_PIN 0 // on D6 |
#define AIN2_PIN 7 // on D5 | #define AIN2_PIN 0 // on D6 | #define AIN2_PIN 7 // on D5 |
#define BIN1_PIN 28 // on D2 | #define BIN1_PIN 2 // on D8 | #define BIN1_PIN 29 // on D3 |
#define BIN2_PIN 4 // on D9 | #define BIN2_PIN 4 // on D9 | #define BIN2_PIN 28 // on D2 |
#define APWM_PIN 27 // on D1 | #define APWM_PIN 6 // on D4 | #define APWM_PIN 6 // on D4 |
#define BPWM_PIN 29 // on D3 | #define BPWM_PIN 3 // on D10 | #define BPWM_PIN 27 // on D1 |
simple_stepper.ino | ||
#define PIN_LIMIT 26 // on D0 | #define PIN_LIMIT 29 // on D3 | #define PIN_LIMIT 26 // on D0 |
More info about:
- Moduler Things board
- BootCamp Leon2024 board
Firmware¶
Repo FAB25 simple stepper
% cd path/to/simple_stepper
% tree
.
├── cobs.cpp
├── cobs.h
├── COBSUSBSerial.cpp
├── COBSUSBSerial.h
├── fixedPointUtes.cpp
├── fixedPointUtes.h
├── lutgen.js
├── motionStateMachine.cpp
├── motionStateMachine.h
├── serializationUtes.cpp
├── serializationUtes.h
├── simple_stepper.ino
├── stepperDriver.cpp
├── stepperDriver.h
└── stepperLUT.h
stepperDriver.cpp
// FAB25 Board
/*
#define AIN1_PIN 6 // on D4
#define AIN2_PIN 7 // on D5
#define BIN1_PIN 28 // on D2
#define BIN2_PIN 4 // on D9
#define APWM_PIN 27 // on D1
#define BPWM_PIN 29 // on D3
*/
// BootCamp Leon2024 Board
#define AIN1_PIN 7 // on D5
#define AIN2_PIN 0 // on D6
#define BIN1_PIN 2 // on D8
#define BIN2_PIN 4 // on D9
#define APWM_PIN 6 // on D4
#define BPWM_PIN 3 // on D10
// BootCamp Waag2023 Ver0 Board
/*
#define AIN1_PIN 0 // on D6
#define AIN2_PIN 7 // on D5
#define BIN1_PIN 29 // on D3
#define BIN2_PIN 28 // on D2
#define APWM_PIN 6 // on D4
#define BPWM_PIN 27 // on D1
*/
simple_stepper.ino
// FAB25 Board
/*
#define PIN_LIMIT 26 // on D0
*/
// BootCamp Leon2024 Board
#define PIN_LIMIT 29 // on D3
motionStateMachine.cpp | |
---|---|
143 144 145 146 |
|
Firmware Download
Note
simple_stepper.ino
// using the RP2040 at 200MHz
Python¶
Python code
The code is designed for Python >= 3.10
Dependencies:
pyserial >= 3.4
cobs >= 1.1
install python
% python3 -V
Python 3.13.5
% pip install pyserial
Defaulting to user installation because normal site-packages is not writeable
Requirement already satisfied: pyserial in ./Library/Python/3.9/lib/python/site-packages (3.5)
% pyserial --version
zsh: command not found: pyserial
% python3.13 -m pip install --user pyserial
Successfully installed pyserial-3.5
% python3 -m pip install cobs --user
Successfully installed cobs-1.2.1
% ls /dev/tty.*
/dev/tty.usbmodem101
import motor
import time
import random
def goto(m: motor.Motor, x, vel, accel):
# print(m.get_states())
m.set_target_position(x, vel, accel)
# print(m.get_states())
# ... wait while not at point:
while True:
states = m.get_states()
# print(states["position"])
# if states['is_moving'] == 0:
if not states["is_moving"]:
break
# if abs(states["position"] - x) <= 0.01:
# break
def goto_all(m1: motor.Motor, m2: motor.Motor, x1, x2, vel, accel):
m1.set_target_position(x1, vel, accel)
m2.set_target_position(x2, vel, accel)
# ... wait while not at point:
done = False
while not done:
states1 = m1.get_states()
states2 = m2.get_states()
done = states1['is_moving'] == 0 and states2['is_moving'] == 0
def main():
# change to the correct serial port
# m = motor.Motor("COM47")
m = motor.Motor("/dev/tty.usbmodem101") # =============================== here
# turns/sec
vel = 3 #5 in my CoreXY
# turns/sec²
acc = 30 #30
# 200 steps per turn
steps_per_unit = 200
# 30% current
current = 0.3 #0.3
m.set_steps_per_unit(steps_per_unit)
m.set_current(current)
goto(m, 2.0, vel, acc)
time.sleep(0.2)
goto(m, 0.0, vel, acc)
m.set_current(0)
if __name__ == "__main__":
main()
from cobs_usb_serial import CobsUsbSerial
import struct
MSG_GET_ID = 7
MSG_ECHO_TEST = 11
MSG_SET_POWER = 20
MSG_SET_TARG_VEL = 21
MSG_SET_TARG_POS = 22
MSG_SET_POSITION = 23
MSG_SET_CURRENT = 24
MSG_GET_STATES = 25
MSG_ACK = 31
MSG_NACK = 32
class Motor:
def __init__(self, port: str):
self.cobs = CobsUsbSerial(port)
self.spu = 200
def get_id(self):
msg = struct.pack('=B', MSG_GET_ID)
self.cobs.write(msg)
ack, device_id = struct.unpack('=BB', self.cobs.read())
return device_id
def set_steps_per_unit(self, spu):
self.spu = spu
def test(self) -> None:
test_bytes = bytes([MSG_ECHO_TEST, 14, 16])
print("testing w/ an echo-packet: ", [byte for byte in test_bytes])
self.cobs.write(test_bytes)
reply = self.cobs.read()
print("test echo returns: ", [byte for byte in reply])
def set_target_velocity(self, velocity: float, accel: float):
msg = struct.pack('=Bff', MSG_SET_TARG_VEL, velocity * self.spu, accel * self.spu)
self.cobs.write(msg)
return struct.unpack('=B', self.cobs.read())
def set_target_position_no_wait(self, position: float, velocity: float, accel: float):
msg = struct.pack('=Bfff', MSG_SET_TARG_POS, position * self.spu, velocity * self.spu, accel * self.spu)
self.cobs.write(msg)
return
def set_target_position(self, position: float, velocity: float, accel: float):
self.set_target_position_no_wait(position, velocity, accel)
return struct.unpack('=B', self.cobs.read())
def set_position(self, position: float):
msg = struct.pack('=Bf', MSG_SET_POSITION, position * self.spu)
self.cobs.write(msg)
return struct.unpack('=B', self.cobs.read())
def set_current(self, current: float):
msg = struct.pack('=Bf', MSG_SET_CURRENT, current)
self.cobs.write(msg)
return struct.unpack('=B', self.cobs.read())
def get_states(self):
self.cobs.write(bytes([MSG_GET_STATES]))
reply = self.cobs.read()
# print("states returns: ", [byte for byte in reply])
# print(reply)
ack, position, velocity, acceleration, is_moving, limit = struct.unpack('=BfffBB', reply)
return {
'ack': ack,
'position': position / self.spu,
'velocity': velocity / self.spu,
'acceleration': acceleration / self.spu,
'is_moving': is_moving,
'limit': limit
}
class HBridge:
def __init__(self, port: str):
self.cobs = CobsUsbSerial(port)
def get_id(self):
msg = struct.pack('=B', MSG_GET_ID)
self.cobs.write(msg)
ack, device_id = struct.unpack('=BB', self.cobs.read())
return device_id
def test(self) -> None:
test_bytes = bytes([MSG_ECHO_TEST, 14, 16])
print("testing w/ an echo-packet: ", [byte for byte in test_bytes])
self.cobs.write(test_bytes)
reply = self.cobs.read()
print("test echo returns: ", [byte for byte in reply])
def set_power(self, current: float, index: int):
msg = struct.pack('=BfB', MSG_SET_POWER, current, index)
self.cobs.write(msg)
return struct.unpack('=B', self.cobs.read())
def get_states(self):
self.cobs.write(bytes([MSG_GET_STATES]))
reply = self.cobs.read()
# print("states returns: ", [byte for byte in reply])
# print(reply)
ack, power_a, power_b = struct.unpack('=Bff', reply)
return {
'ack': ack,
'power_a': power_a,
'power_b': power_b,
}
from cobs import cobs
import serial
class CobsUsbSerial:
def __init__(self, port, baudrate=115200):
self.port = port
self.baudrate = baudrate
self.ser = serial.Serial(port, baudrate=baudrate)
def write(self, data: bytes):
data_enc = cobs.encode(data) + b"\x00"
self.ser.write(data_enc)
def read(self):
data_enc = self.ser.read_until(b"\x00")
data = cobs.decode(data_enc[:-1])
return data
Run¶
From Terminal¶
% cd path/to/modular-printable-axis-main/code/python
% tree
.
├── __pycache__
│ ├── cobs_usb_serial.cpython-312.pyc
│ ├── cobs_usb_serial.cpython-313.pyc
│ ├── motor.cpython-312.pyc
│ └── motor.cpython-313.pyc
├── cobs_usb_serial.py
├── main_test.py
├── motor.py
└── two_axis.py
% python3 main_test.py
From Thonny¶
CoreXY¶
Add CoreXY function and replace main() in main_test.py¶
# Add CoreXY coordinate movement function to main_test.py
def goto_xy(m1: motor.Motor, m2: motor.Motor, x: float, y: float, vel: float, accel: float):
# CoreXY algorithm: A = X + Y, B = X - Y
a = x + y
b = x - y
goto_all(m1, m2, a, b, vel, accel)
# Replace the main() function in main_test.py with this CoreXY movement test sequence
def main():
m1 = motor.Motor("/dev/tty.usbmodem1101")
m2 = motor.Motor("/dev/tty.usbmodem1201")
vel = 3 #5
acc = 30
steps_per_unit = 200
current = 0.3
m1.set_steps_per_unit(steps_per_unit)
m2.set_steps_per_unit(steps_per_unit)
m1.set_current(current)
m2.set_current(current)
# Move to position (1.0, 1.0) from the origin (0.0, 0.0)
goto_xy(m1, m2, 1.0, 1.0, vel, acc) # Move diagonally to top-right
time.sleep(0.2)
# Return to the origin (0.0, 0.0)
goto_xy(m1, m2, 0.0, 0.0, vel, acc) # Move diagonally back to origin
time.sleep(0.2)
# Move along X axis to (1.0, 0.0)
goto_xy(m1, m2, 1.0, 0.0, vel, acc) # Move right
time.sleep(0.2)
# Move along Y axis to (1.0, 1.0)
goto_xy(m1, m2, 1.0, 1.0, vel, acc) # Move up
time.sleep(0.2)
# Move along X axis to (0.0, 1.0)
goto_xy(m1, m2, 0.0, 1.0, vel, acc) # Move left
time.sleep(0.2)
# Return to origin (0.0, 0.0)
goto_xy(m1, m2, 0.0, 0.0, vel, acc) # Move down
time.sleep(0.2)
# Turn off motor current
m1.set_current(0)
m2.set_current(0)
CoreXY code: Draw a pattern¶
main_test.py
import motor
import time
import random
def goto_xy(m1: motor.Motor, m2: motor.Motor, x: float, y: float, vel: float, accel: float):
# CoreXY algorithm: A = X + Y, B = X - Y
a = x + y
b = x - y
m1.set_target_position(a, vel, accel)
m2.set_target_position(b, vel, accel)
# Wait until both motors finish moving
while True:
states1 = m1.get_states()
states2 = m2.get_states()
if states1['is_moving'] == 0 and states2['is_moving'] == 0:
break
def main():
m1 = motor.Motor("/dev/tty.usbmodem1101")
m2 = motor.Motor("/dev/tty.usbmodem1201")
vel = 3 #5
acc = 30
steps_per_unit = 200
current = 0.3
m1.set_steps_per_unit(steps_per_unit)
m2.set_steps_per_unit(steps_per_unit)
m1.set_current(current)
m2.set_current(current)
# Move to position (1.0, 1.0) from the origin (0.0, 0.0)
goto_xy(m1, m2, 1.0, 1.0, vel, acc) # Move diagonally to top-right
time.sleep(0.2)
# Return to the origin (0.0, 0.0)
goto_xy(m1, m2, 0.0, 0.0, vel, acc) # Move diagonally back to origin
time.sleep(0.2)
# Move along X axis to (1.0, 0.0)
goto_xy(m1, m2, 1.0, 0.0, vel, acc) # Move right
time.sleep(0.2)
# Move along Y axis to (1.0, 1.0)
goto_xy(m1, m2, 1.0, 1.0, vel, acc) # Move up
time.sleep(0.2)
# Move along X axis to (0.0, 1.0)
goto_xy(m1, m2, 0.0, 1.0, vel, acc) # Move left
time.sleep(0.2)
# Return to origin (0.0, 0.0)
goto_xy(m1, m2, 0.0, 0.0, vel, acc) # Move down
time.sleep(0.2)
# Turn off motor current
m1.set_current(0)
m2.set_current(0)
if __name__ == "__main__":
main()
CoreXY code: Control from GUI¶
main_test.py
import motor
import time
import tkinter as tk
def goto_xy(m1: motor.Motor, m2: motor.Motor, x: float, y: float, vel: float, accel: float):
a = x + y
b = x - y
m1.set_target_position(a, vel, accel)
m2.set_target_position(b, vel, accel)
while True:
states1 = m1.get_states()
states2 = m2.get_states()
if states1['is_moving'] == 0 and states2['is_moving'] == 0:
break
def run_gui():
m1 = motor.Motor("/dev/tty.usbmodem1101")
m2 = motor.Motor("/dev/tty.usbmodem1201")
vel = 3
acc = 30
steps_per_unit = 200
current = 0.3
m1.set_steps_per_unit(steps_per_unit)
m2.set_steps_per_unit(steps_per_unit)
m1.set_current(current)
m2.set_current(current)
current_x = 0.0
current_y = 0.0
def move_by(dx, dy):
nonlocal current_x, current_y
current_x += dx
current_y += dy
goto_xy(m1, m2, current_x, current_y, vel, acc)
def move_home():
nonlocal current_x, current_y
current_x = 0.0
current_y = 0.0
goto_xy(m1, m2, current_x, current_y, vel, acc)
def shutdown():
m1.set_current(0)
m2.set_current(0)
root.destroy()
# --- GUI ---
root = tk.Tk()
root.title("CoreXY Motor GUI")
frame = tk.Frame(root)
frame.pack(padx=10, pady=10)
tk.Button(frame, text="↑", width=5, command=lambda: move_by(0, 1.0)).grid(row=0, column=1)
tk.Button(frame, text="←", width=5, command=lambda: move_by(-1.0, 0)).grid(row=1, column=0)
tk.Button(frame, text="→", width=5, command=lambda: move_by(1.0, 0)).grid(row=1, column=2)
tk.Button(frame, text="↓", width=5, command=lambda: move_by(0, -1.0)).grid(row=2, column=1)
tk.Button(frame, text="Home", width=5, command=move_home).grid(row=1, column=1)
tk.Button(root, text="Exit", width=10, command=shutdown).pack(pady=5)
root.mainloop()
if __name__ == "__main__":
run_gui()
main_test.py
import motor
import time
import tkinter as tk
def goto_xy(m1: motor.Motor, m2: motor.Motor, x: float, y: float, vel: float, accel: float):
a = x + y
b = x - y
m1.set_target_position(a, vel, accel)
m2.set_target_position(b, vel, accel)
while True:
states1 = m1.get_states()
states2 = m2.get_states()
if states1['is_moving'] == 0 and states2['is_moving'] == 0:
break
def run_gui():
m1 = motor.Motor("/dev/tty.usbmodem1101")
m2 = motor.Motor("/dev/tty.usbmodem1201")
vel = 3
acc = 30
steps_per_unit = 200
current = 0.3
m1.set_steps_per_unit(steps_per_unit)
m2.set_steps_per_unit(steps_per_unit)
m1.set_current(current)
m2.set_current(current)
current_x = 0.0
current_y = 0.0
def move_by(dx, dy):
nonlocal current_x, current_y
current_x += dx
current_y += dy
goto_xy(m1, m2, current_x, current_y, vel, acc)
pos_label.config(text=f"Current Position: ({current_x:.1f}, {current_y:.1f})")
def move_home():
nonlocal current_x, current_y
current_x = 0.0
current_y = 0.0
goto_xy(m1, m2, current_x, current_y, vel, acc)
pos_label.config(text=f"Current Position: ({current_x:.1f}, {current_y:.1f})")
def shutdown():
m1.set_current(0)
m2.set_current(0)
root.destroy()
def move_to_xy():
nonlocal current_x, current_y
try:
x = float(entry_x.get())
y = float(entry_y.get())
current_x = x
current_y = y
goto_xy(m1, m2, current_x, current_y, vel, acc)
pos_label.config(text=f"Current Position: ({current_x:.1f}, {current_y:.1f})")
except ValueError:
pass
def run_pattern(): # ← Run Pattern追加
nonlocal current_x, current_y
square_size = 1.0
points = [
(0, 0),
(square_size, 0),
(square_size, square_size),
(0, square_size),
(0, 0),
]
for x, y in points:
current_x = x
current_y = y
goto_xy(m1, m2, x, y, vel, acc)
pos_label.config(text=f"Current Position: ({current_x:.1f}, {current_y:.1f})")
time.sleep(0.5)
# --- GUI ---
root = tk.Tk()
root.title("CoreXY Motor GUI")
frame = tk.Frame(root)
frame.pack(padx=10, pady=10)
tk.Button(frame, text="↑", width=5, command=lambda: move_by(0, 1.0)).grid(row=0, column=1)
tk.Button(frame, text="←", width=5, command=lambda: move_by(-1.0, 0)).grid(row=1, column=0)
tk.Button(frame, text="→", width=5, command=lambda: move_by(1.0, 0)).grid(row=1, column=2)
tk.Button(frame, text="↓", width=5, command=lambda: move_by(0, -1.0)).grid(row=2, column=1)
tk.Button(frame, text="Home", width=5, command=move_home).grid(row=1, column=1)
pos_label = tk.Label(root, text=f"Current Position: ({current_x:.1f}, {current_y:.1f})")
pos_label.pack(pady=5)
coord_frame = tk.Frame(root)
coord_frame.pack(pady=5)
tk.Label(coord_frame, text="X:").grid(row=0, column=0)
entry_x = tk.Entry(coord_frame, width=5)
entry_x.grid(row=0, column=1)
tk.Label(coord_frame, text="Y:").grid(row=0, column=2)
entry_y = tk.Entry(coord_frame, width=5)
entry_y.grid(row=0, column=3)
tk.Button(coord_frame, text="Go", command=move_to_xy).grid(row=0, column=4, padx=5)
tk.Button(root, text="Run Pattern", width=15, command=run_pattern).pack(pady=5)
tk.Button(root, text="Exit", width=10, command=shutdown).pack(pady=5)
root.mainloop()
if __name__ == "__main__":
run_gui()
To Do¶
Limit switch