diff --git a/python_motion/__pycache__/cobs_usb_serial.cpython-311.pyc b/python_motion/__pycache__/cobs_usb_serial.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1ea2180ad5c1e908a82022d5565407cb9eeff271
Binary files /dev/null and b/python_motion/__pycache__/cobs_usb_serial.cpython-311.pyc differ
diff --git a/python_motion/__pycache__/motor.cpython-311.pyc b/python_motion/__pycache__/motor.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..78819395db5c7a0421901aa096cb43df08c7b035
Binary files /dev/null and b/python_motion/__pycache__/motor.cpython-311.pyc differ
diff --git a/python_motion/cobs_usb_serial.py b/python_motion/cobs_usb_serial.py
new file mode 100644
index 0000000000000000000000000000000000000000..e98b83b5b08daa87e4d966a0bb4cfd2327c9630f
--- /dev/null
+++ b/python_motion/cobs_usb_serial.py
@@ -0,0 +1,18 @@
+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, timeout=1)
+
+ 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
diff --git a/python_motion/motion.py b/python_motion/motion.py
new file mode 100644
index 0000000000000000000000000000000000000000..f393d5d3cfa708481549393957e6fcffee380b87
--- /dev/null
+++ b/python_motion/motion.py
@@ -0,0 +1,92 @@
+from motor import Motor
+import random
+import time
+
+home_rate = 100
+home_accel = 2000
+home_backoff_time = 0.5
+
+go_rate = 150
+go_accel = 2000
+
+use_current = 0.2
+
+y_motor = Motor("COM25")
+print("y_motor: (1): ", y_motor.get_id())
+
+x_bottom_motor = Motor("COM7")
+print("x_bottom: (2): ", x_bottom_motor.get_id())
+x_top_motor = Motor("COM28")
+print("x_top: (3): ", x_top_motor.get_id())
+
+
+# steps / unit,
+y_motor.set_steps_per_unit(200 / 40)
+x_bottom_motor.set_steps_per_unit(-200 /40)
+x_top_motor.set_steps_per_unit(200 / 40)
+
+# power up
+y_motor.set_current(use_current)
+x_bottom_motor.set_current(use_current)
+x_top_motor.set_current(use_current)
+
+
+# home Y ....
+# if we're on the switch, backoff:
+if y_motor.get_states()['limit'] > 0:
+ y_motor.set_target_velocity(home_rate, home_accel)
+ time.sleep(home_backoff_time)
+ y_motor.set_target_velocity(0, home_accel)
+ time.sleep(home_backoff_time)
+
+# let's home it,
+y_motor.set_target_velocity(-home_rate, home_accel)
+
+# then check-wait on states,
+while True:
+ states = y_motor.get_states()
+ if states['limit'] > 0:
+ break;
+
+y_motor.set_target_velocity(0, home_accel)
+
+
+# home X ...
+if x_top_motor.get_states()['limit'] > 0:
+ x_bottom_motor.set_target_velocity(home_rate, home_accel)
+ x_top_motor.set_target_velocity(home_rate, home_accel)
+ time.sleep(home_backoff_time)
+ x_bottom_motor.set_target_velocity(0, home_accel)
+ x_top_motor.set_target_velocity(0, home_accel)
+ time.sleep(home_backoff_time)
+
+
+x_bottom_motor.set_target_velocity(-home_rate, home_accel)
+x_top_motor.set_target_velocity(-home_rate, home_accel)
+
+while True:
+ states = x_top_motor.get_states()
+ if states['limit'] > 0:
+ break;
+
+x_bottom_motor.set_target_velocity(0, home_accel)
+x_top_motor.set_target_velocity(0, home_accel)
+time.sleep(home_backoff_time)
+
+# set zeroes,
+x_bottom_motor.set_position(0)
+x_top_motor.set_position(0)
+y_motor.set_position(0)
+
+
+# goto points
+def goto(x, y, vel, accel):
+ x_bottom_motor.set_target_position_no_wait(x, vel, accel)
+ x_top_motor.set_target_position_no_wait(x, vel, accel)
+ y_motor.set_target_position_no_wait(y, vel, accel)
+ # # ... wait while not at point:
+ # while True:
+ # x_bot_state = x_bottom_motor.get_states()
+
+
+goto(200, 100, go_rate, go_accel)
\ No newline at end of file
diff --git a/python_motion/motor.py b/python_motion/motor.py
new file mode 100644
index 0000000000000000000000000000000000000000..57f348f05029061e09ba8fdd74e697a59984de3a
--- /dev/null
+++ b/python_motion/motor.py
@@ -0,0 +1,85 @@
+from cobs_usb_serial import CobsUsbSerial
+import struct
+
+
+MSG_GET_ID = 7
+MSG_ECHO_TEST = 11
+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])
+
+ ack, position, velocity, acceleration, limit = struct.unpack('=BfffB', reply)
+
+ return {
+ 'ack': ack,
+ 'position': position / self.spu,
+ 'velocity': velocity / self.spu,
+ 'acceleration': acceleration / self.spu,
+ 'limit': limit
+ }
+
+
\ No newline at end of file
diff --git a/python_motion/serial_list.py b/python_motion/serial_list.py
new file mode 100644
index 0000000000000000000000000000000000000000..5f25377227f77c79dc2db925a16f5a5a280f412b
--- /dev/null
+++ b/python_motion/serial_list.py
@@ -0,0 +1,20 @@
+import serial.tools.list_ports
+
+def list_serial_ports():
+ ports = serial.tools.list_ports.comports()
+ for port in ports:
+ print(f"Port: {port.device}")
+ print(f" - Description: {port.description}")
+ if port.serial_number:
+ print(f" - Serial Number: {port.serial_number}")
+ if port.manufacturer:
+ print(f" - Manufacturer: {port.manufacturer}")
+ if port.product:
+ print(f" - Product: {port.product}")
+ if port.vid is not None:
+ print(f" - VID: {port.vid:04X}")
+ if port.pid is not None:
+ print(f" - PID: {port.pid:04X}")
+ print()
+
+list_serial_ports()
diff --git a/simple-stepper/.vscode/arduino.json b/simple-stepper/.vscode/arduino.json
new file mode 100644
index 0000000000000000000000000000000000000000..a8e46da7bb42e76975165be17f7b7aacfba4b512
--- /dev/null
+++ b/simple-stepper/.vscode/arduino.json
@@ -0,0 +1,6 @@
+{
+ "port": "COM7",
+ "configuration": "flash=2097152_0,freq=200,opt=Small,rtti=Disabled,stackprotect=Disabled,exceptions=Disabled,dbgport=Disabled,dbglvl=None,usbstack=picosdk,ipbtstack=ipv4only,uploadmethod=default",
+ "board": "rp2040:rp2040:seeed_xiao_rp2040",
+ "sketch": "simple-stepper.ino"
+}
\ No newline at end of file
diff --git a/simple-stepper/.vscode/c_cpp_properties.json b/simple-stepper/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..17cdc59d4fa403e1d1d0d9c7993a114b7bd618aa
--- /dev/null
+++ b/simple-stepper/.vscode/c_cpp_properties.json
@@ -0,0 +1,524 @@
+{
+ "version": 4,
+ "configurations": [
+ {
+ "name": "Arduino",
+ "compilerPath": "C:\\Users\\jaker\\AppData\\Local\\Arduino15\\packages\\rp2040\\tools\\pqt-gcc\\2.1.0-a-d3d2e6b\\bin\\arm-none-eabi-g++",
+ "compilerArgs": [
+ "-I",
+ "-Werror=return-type",
+ "-Wno-psabi",
+ "-march=armv6-m",
+ "-mcpu=cortex-m0plus",
+ "-mthumb",
+ "-ffunction-sections",
+ "-fdata-sections",
+ "-fno-exceptions"
+ ],
+ "intelliSenseMode": "gcc-x64",
+ "includePath": [
+ "C:\\Users\\jaker\\AppData\\Local\\Arduino15\\packages\\rp2040\\hardware\\rp2040\\3.5.0\\include",
+ "C:\\Users\\jaker\\AppData\\Local\\Arduino15\\packages\\rp2040\\hardware\\rp2040\\3.5.0\\cores\\rp2040",
+ "C:\\Users\\jaker\\AppData\\Local\\Arduino15\\packages\\rp2040\\hardware\\rp2040\\3.5.0\\variants\\seeed_xiao_rp2040",
+ "c:\\users\\jaker\\appdata\\local\\arduino15\\packages\\rp2040\\tools\\pqt-gcc\\2.1.0-a-d3d2e6b\\arm-none-eabi\\include\\c++\\12.3.0",
+ "c:\\users\\jaker\\appdata\\local\\arduino15\\packages\\rp2040\\tools\\pqt-gcc\\2.1.0-a-d3d2e6b\\arm-none-eabi\\include\\c++\\12.3.0\\arm-none-eabi\\thumb",
+ "c:\\users\\jaker\\appdata\\local\\arduino15\\packages\\rp2040\\tools\\pqt-gcc\\2.1.0-a-d3d2e6b\\arm-none-eabi\\include\\c++\\12.3.0\\backward",
+ "c:\\users\\jaker\\appdata\\local\\arduino15\\packages\\rp2040\\tools\\pqt-gcc\\2.1.0-a-d3d2e6b\\lib\\gcc\\arm-none-eabi\\12.3.0\\include",
+ "c:\\users\\jaker\\appdata\\local\\arduino15\\packages\\rp2040\\tools\\pqt-gcc\\2.1.0-a-d3d2e6b\\lib\\gcc\\arm-none-eabi\\12.3.0\\include-fixed",
+ "c:\\users\\jaker\\appdata\\local\\arduino15\\packages\\rp2040\\tools\\pqt-gcc\\2.1.0-a-d3d2e6b\\arm-none-eabi\\include"
+ ],
+ "forcedInclude": [
+ "C:\\Users\\jaker\\AppData\\Local\\Arduino15\\packages\\rp2040\\hardware\\rp2040\\3.5.0\\cores\\rp2040\\Arduino.h"
+ ],
+ "cStandard": "c11",
+ "cppStandard": "c++17",
+ "defines": [
+ "CFG_TUSB_MCU=OPT_MCU_RP2040",
+ "USBD_PID=0x000a",
+ "USBD_VID=0x2e8a",
+ "USBD_MAX_POWER_MA=250",
+ "USB_MANUFACTURER=\"Seeed\"",
+ "USB_PRODUCT=\"XIAO RP2040\"",
+ "PICO_CYW43_ARCH_THREADSAFE_BACKGROUND=1",
+ "CYW43_LWIP=1",
+ "LWIP_IPV6=0",
+ "LWIP_IPV4=1",
+ "LWIP_IGMP=1",
+ "LWIP_CHECKSUM_CTRL_PER_NETIF=1",
+ "ARDUINO_VARIANT=\"seeed_xiao_rp2040\"",
+ "TARGET_RP2040",
+ "PICO_FLASH_SIZE_BYTES=2097152",
+ "ARM_MATH_CM0_FAMILY",
+ "ARM_MATH_CM0_PLUS",
+ "F_CPU=200000000L",
+ "ARDUINO=10607",
+ "ARDUINO_SEEED_XIAO_RP2040",
+ "BOARD_NAME=\"SEEED_XIAO_RP2040\"",
+ "ARDUINO_ARCH_RP2040",
+ "WIFICC=CYW43_COUNTRY_WORLDWIDE",
+ "__DBL_MIN_EXP__=(-1021)",
+ "__HQ_FBIT__=15",
+ "__cpp_attributes=200809L",
+ "__cpp_nontype_template_parameter_auto=201606L",
+ "__UINT_LEAST16_MAX__=0xffff",
+ "__ARM_SIZEOF_WCHAR_T=4",
+ "__ATOMIC_ACQUIRE=2",
+ "__SFRACT_IBIT__=0",
+ "__FLT_MIN__=1.1754943508222875e-38F",
+ "__GCC_IEC_559_COMPLEX=0",
+ "__cpp_aggregate_nsdmi=201304L",
+ "__UFRACT_MAX__=0XFFFFP-16UR",
+ "__UINT_LEAST8_TYPE__=unsigned char",
+ "__DQ_FBIT__=63",
+ "__INTMAX_C(c)=c ## LL",
+ "__ULFRACT_FBIT__=32",
+ "__CHAR_BIT__=8",
+ "__USQ_IBIT__=0",
+ "__UINT8_MAX__=0xff",
+ "__ACCUM_FBIT__=15",
+ "__WINT_MAX__=0xffffffffU",
+ "__FLT32_MIN_EXP__=(-125)",
+ "__cpp_static_assert=201411L",
+ "__USFRACT_FBIT__=8",
+ "__ORDER_LITTLE_ENDIAN__=1234",
+ "__SIZE_MAX__=0xffffffffU",
+ "__WCHAR_MAX__=0xffffffffU",
+ "__LACCUM_IBIT__=32",
+ "__DBL_DENORM_MIN__=double(4.9406564584124654e-324L)",
+ "__GCC_ATOMIC_CHAR_LOCK_FREE=1",
+ "__GCC_IEC_559=0",
+ "__FLT32X_DECIMAL_DIG__=17",
+ "__FLT_EVAL_METHOD__=0",
+ "__TQ_IBIT__=0",
+ "__cpp_binary_literals=201304L",
+ "__LLACCUM_MAX__=0X7FFFFFFFFFFFFFFFP-31LLK",
+ "__FLT64_DECIMAL_DIG__=17",
+ "__cpp_noexcept_function_type=201510L",
+ "__GCC_ATOMIC_CHAR32_T_LOCK_FREE=1",
+ "__cpp_variadic_templates=200704L",
+ "__UINT_FAST64_MAX__=0xffffffffffffffffULL",
+ "__SIG_ATOMIC_TYPE__=int",
+ "__DBL_MIN_10_EXP__=(-307)",
+ "__FINITE_MATH_ONLY__=0",
+ "__ARMEL__=1",
+ "__cpp_variable_templates=201304L",
+ "__FLT32X_MAX_EXP__=1024",
+ "__LFRACT_IBIT__=0",
+ "__GNUC_PATCHLEVEL__=0",
+ "__FLT32_HAS_DENORM__=1",
+ "__LFRACT_MAX__=0X7FFFFFFFP-31LR",
+ "__USA_FBIT__=16",
+ "__UINT_FAST8_MAX__=0xffffffffU",
+ "__cpp_rvalue_reference=200610L",
+ "__cpp_nested_namespace_definitions=201411L",
+ "__INT8_C(c)=c",
+ "__INT_LEAST8_WIDTH__=8",
+ "__cpp_variadic_using=201611L",
+ "__UINT_LEAST64_MAX__=0xffffffffffffffffULL",
+ "__INT_LEAST8_MAX__=0x7f",
+ "__SA_FBIT__=15",
+ "__cpp_capture_star_this=201603L",
+ "__SHRT_MAX__=0x7fff",
+ "__LDBL_MAX__=1.7976931348623157e+308L",
+ "__FRACT_MAX__=0X7FFFP-15R",
+ "__cpp_if_constexpr=201606L",
+ "__LDBL_IS_IEC_60559__=2",
+ "__UFRACT_FBIT__=16",
+ "__UFRACT_MIN__=0.0UR",
+ "__UINT_LEAST8_MAX__=0xff",
+ "__GCC_ATOMIC_BOOL_LOCK_FREE=1",
+ "__UINTMAX_TYPE__=long long unsigned int",
+ "__LLFRACT_EPSILON__=0x1P-63LLR",
+ "__FLT_EVAL_METHOD_TS_18661_3__=0",
+ "__CHAR_UNSIGNED__=1",
+ "__UINT32_MAX__=0xffffffffUL",
+ "__GXX_EXPERIMENTAL_CXX0X__=1",
+ "__ULFRACT_MAX__=0XFFFFFFFFP-32ULR",
+ "__TA_IBIT__=64",
+ "__LDBL_MAX_EXP__=1024",
+ "__WINT_MIN__=0U",
+ "__FLT32X_IS_IEC_60559__=2",
+ "__INT_LEAST16_WIDTH__=16",
+ "__ULLFRACT_MIN__=0.0ULLR",
+ "__SCHAR_MAX__=0x7f",
+ "__WCHAR_MIN__=0U",
+ "__INT64_C(c)=c ## LL",
+ "__GCC_ATOMIC_POINTER_LOCK_FREE=1",
+ "__LLACCUM_MIN__=(-0X1P31LLK-0X1P31LLK)",
+ "__SIZEOF_INT__=4",
+ "__FLT32X_MANT_DIG__=53",
+ "__GCC_ATOMIC_CHAR16_T_LOCK_FREE=1",
+ "__USACCUM_IBIT__=8",
+ "__cpp_aligned_new=201606L",
+ "__FLT32_MAX_10_EXP__=38",
+ "__STDC_HOSTED__=1",
+ "__LFRACT_MIN__=(-0.5LR-0.5LR)",
+ "__HA_IBIT__=8",
+ "__cpp_decltype_auto=201304L",
+ "__DBL_DIG__=15",
+ "__FLT32_DIG__=6",
+ "__FLT_EPSILON__=1.1920928955078125e-7F",
+ "__APCS_32__=1",
+ "__GXX_WEAK__=1",
+ "__SHRT_WIDTH__=16",
+ "__FLT32_IS_IEC_60559__=2",
+ "__USFRACT_IBIT__=0",
+ "__LDBL_MIN__=2.2250738585072014e-308L",
+ "__DBL_IS_IEC_60559__=2",
+ "__FRACT_MIN__=(-0.5R-0.5R)",
+ "__cpp_threadsafe_static_init=200806L",
+ "__DA_IBIT__=32",
+ "__ARM_SIZEOF_MINIMAL_ENUM=1",
+ "__FLT32X_HAS_INFINITY__=1",
+ "__INT32_MAX__=0x7fffffffL",
+ "__UQQ_FBIT__=8",
+ "__INT_WIDTH__=32",
+ "__SIZEOF_LONG__=4",
+ "__UACCUM_MAX__=0XFFFFFFFFP-16UK",
+ "__UINT16_C(c)=c",
+ "__DECIMAL_DIG__=17",
+ "__LFRACT_EPSILON__=0x1P-31LR",
+ "__FLT64_EPSILON__=2.2204460492503131e-16F64",
+ "__ULFRACT_MIN__=0.0ULR",
+ "__INT16_MAX__=0x7fff",
+ "__FLT64_MIN_EXP__=(-1021)",
+ "__LDBL_HAS_QUIET_NAN__=1",
+ "__ULACCUM_IBIT__=32",
+ "__FLT64_MANT_DIG__=53",
+ "__UACCUM_EPSILON__=0x1P-16UK",
+ "__GNUC__=12",
+ "__ULLACCUM_MAX__=0XFFFFFFFFFFFFFFFFP-32ULLK",
+ "__GXX_RTTI=1",
+ "__HQ_IBIT__=0",
+ "__FLT_HAS_DENORM__=1",
+ "__SIZEOF_LONG_DOUBLE__=8",
+ "__SA_IBIT__=16",
+ "__BIGGEST_ALIGNMENT__=8",
+ "__STDC_UTF_16__=1",
+ "__FLT64_MAX_10_EXP__=308",
+ "__GNUC_STDC_INLINE__=1",
+ "__DQ_IBIT__=0",
+ "__cpp_delegating_constructors=200604L",
+ "__FLT32_HAS_INFINITY__=1",
+ "__DBL_MAX__=double(1.7976931348623157e+308L)",
+ "__ULFRACT_IBIT__=0",
+ "__cpp_raw_strings=200710L",
+ "__INT_FAST32_MAX__=0x7fffffff",
+ "__DBL_HAS_INFINITY__=1",
+ "__cpp_deduction_guides=201703L",
+ "__HAVE_SPECULATION_SAFE_VALUE=1",
+ "__cpp_fold_expressions=201603L",
+ "__ACCUM_IBIT__=16",
+ "__THUMB_INTERWORK__=1",
+ "__INTPTR_WIDTH__=32",
+ "__UINT_LEAST32_MAX__=0xffffffffUL",
+ "__ULLACCUM_IBIT__=32",
+ "__LACCUM_MAX__=0X7FFFFFFFFFFFFFFFP-31LK",
+ "__FLT32X_HAS_DENORM__=1",
+ "__INT_FAST16_TYPE__=int",
+ "__LDBL_HAS_DENORM__=1",
+ "__cplusplus=201703L",
+ "__cpp_ref_qualifiers=200710L",
+ "__INT_LEAST32_MAX__=0x7fffffffL",
+ "__ARM_PCS=1",
+ "__ACCUM_MAX__=0X7FFFFFFFP-15K",
+ "__DEPRECATED=1",
+ "__ARM_ARCH_6M__=1",
+ "__cpp_rvalue_references=200610L",
+ "__DBL_MAX_EXP__=1024",
+ "__USACCUM_EPSILON__=0x1P-8UHK",
+ "__WCHAR_WIDTH__=32",
+ "__FLT32_MAX__=3.4028234663852886e+38F32",
+ "__GCC_ATOMIC_LONG_LOCK_FREE=1",
+ "__SFRACT_MAX__=0X7FP-7HR",
+ "__FRACT_IBIT__=0",
+ "__PTRDIFF_MAX__=0x7fffffff",
+ "__UACCUM_MIN__=0.0UK",
+ "__UACCUM_IBIT__=16",
+ "__FLT32_HAS_QUIET_NAN__=1",
+ "__GNUG__=12",
+ "__LONG_LONG_MAX__=0x7fffffffffffffffLL",
+ "__ULACCUM_MAX__=0XFFFFFFFFFFFFFFFFP-32ULK",
+ "__cpp_nsdmi=200809L",
+ "__SIZEOF_WINT_T__=4",
+ "__LONG_LONG_WIDTH__=64",
+ "__cpp_initializer_lists=200806L",
+ "__FLT32_MAX_EXP__=128",
+ "__ULLACCUM_MIN__=0.0ULLK",
+ "__cpp_hex_float=201603L",
+ "__GXX_ABI_VERSION=1017",
+ "__UTA_FBIT__=64",
+ "__FLT_MIN_EXP__=(-125)",
+ "__UFRACT_IBIT__=0",
+ "__cpp_enumerator_attributes=201411L",
+ "__cpp_lambdas=200907L",
+ "__INT_FAST64_TYPE__=long long int",
+ "__FLT64_DENORM_MIN__=4.9406564584124654e-324F64",
+ "__DBL_MIN__=double(2.2250738585072014e-308L)",
+ "__SIZEOF_POINTER__=4",
+ "__DBL_HAS_QUIET_NAN__=1",
+ "__FLT32X_EPSILON__=2.2204460492503131e-16F32x",
+ "__LACCUM_MIN__=(-0X1P31LK-0X1P31LK)",
+ "__FRACT_FBIT__=15",
+ "__ULLACCUM_FBIT__=32",
+ "__GXX_TYPEINFO_EQUALITY_INLINE=0",
+ "__FLT64_MIN_10_EXP__=(-307)",
+ "__ULLFRACT_EPSILON__=0x1P-64ULLR",
+ "__USES_INITFINI__=1",
+ "__REGISTER_PREFIX__",
+ "__UINT16_MAX__=0xffff",
+ "__ACCUM_MIN__=(-0X1P15K-0X1P15K)",
+ "__SQ_IBIT__=0",
+ "__FLT32_MIN__=1.1754943508222875e-38F32",
+ "__UINT8_TYPE__=unsigned char",
+ "__UHA_FBIT__=8",
+ "__FLT_DIG__=6",
+ "__NO_INLINE__=1",
+ "__SFRACT_MIN__=(-0.5HR-0.5HR)",
+ "__UTQ_FBIT__=128",
+ "__DEC_EVAL_METHOD__=2",
+ "__FLT_MANT_DIG__=24",
+ "__LDBL_DECIMAL_DIG__=17",
+ "__VERSION__=\"12.3.0\"",
+ "__UINT64_C(c)=c ## ULL",
+ "__ULLFRACT_FBIT__=64",
+ "__cpp_unicode_characters=201411L",
+ "__SOFTFP__=1",
+ "__FRACT_EPSILON__=0x1P-15R",
+ "__ULACCUM_MIN__=0.0ULK",
+ "__UDA_FBIT__=32",
+ "__LLACCUM_EPSILON__=0x1P-31LLK",
+ "__GCC_ATOMIC_INT_LOCK_FREE=1",
+ "__FLOAT_WORD_ORDER__=__ORDER_LITTLE_ENDIAN__",
+ "__USFRACT_MIN__=0.0UHR",
+ "__FLT32_MANT_DIG__=24",
+ "__cpp_aggregate_bases=201603L",
+ "__UQQ_IBIT__=0",
+ "__USFRACT_MAX__=0XFFP-8UHR",
+ "__SCHAR_WIDTH__=8",
+ "__INT32_C(c)=c ## L",
+ "__ORDER_PDP_ENDIAN__=3412",
+ "__UHQ_FBIT__=16",
+ "__LLACCUM_FBIT__=31",
+ "__INT_FAST32_TYPE__=int",
+ "__UINT_LEAST16_TYPE__=short unsigned int",
+ "__DBL_HAS_DENORM__=1",
+ "__cpp_rtti=199711L",
+ "__SIZE_TYPE__=unsigned int",
+ "__UINT64_MAX__=0xffffffffffffffffULL",
+ "__FLT_IS_IEC_60559__=2",
+ "__UDQ_FBIT__=64",
+ "__GNUC_WIDE_EXECUTION_CHARSET_NAME=\"UTF-32LE\"",
+ "__INT8_TYPE__=signed char",
+ "__thumb__=1",
+ "__cpp_digit_separators=201309L",
+ "__ELF__=1",
+ "__SACCUM_EPSILON__=0x1P-7HK",
+ "__ULFRACT_EPSILON__=0x1P-32ULR",
+ "__LLFRACT_FBIT__=63",
+ "__FLT_RADIX__=2",
+ "__INT_LEAST16_TYPE__=short int",
+ "__ARM_ARCH_PROFILE=77",
+ "__LDBL_EPSILON__=2.2204460492503131e-16L",
+ "__UINTMAX_C(c)=c ## ULL",
+ "__SACCUM_MAX__=0X7FFFP-7HK",
+ "__FLT32X_MIN__=2.2250738585072014e-308F32x",
+ "__SIG_ATOMIC_MAX__=0x7fffffff",
+ "__UACCUM_FBIT__=16",
+ "__GCC_ATOMIC_WCHAR_T_LOCK_FREE=1",
+ "__USER_LABEL_PREFIX__",
+ "__VFP_FP__=1",
+ "__SIZEOF_PTRDIFF_T__=4",
+ "__LACCUM_EPSILON__=0x1P-31LK",
+ "__LDBL_DIG__=15",
+ "__FLT64_IS_IEC_60559__=2",
+ "__FLT32X_MIN_EXP__=(-1021)",
+ "__INT_FAST16_MAX__=0x7fffffff",
+ "__GCC_CONSTRUCTIVE_SIZE=64",
+ "__FLT64_DIG__=15",
+ "__UINT_FAST32_MAX__=0xffffffffU",
+ "__UINT_LEAST64_TYPE__=long long unsigned int",
+ "__SFRACT_EPSILON__=0x1P-7HR",
+ "__FLT_HAS_QUIET_NAN__=1",
+ "__FLT_MAX_10_EXP__=38",
+ "__LONG_MAX__=0x7fffffffL",
+ "__SIZEOF_SIZE_T__=4",
+ "__FLT_HAS_INFINITY__=1",
+ "__GNUC_EXECUTION_CHARSET_NAME=\"UTF-8\"",
+ "__cpp_unicode_literals=200710L",
+ "__UINT_FAST16_TYPE__=unsigned int",
+ "__INT_FAST32_WIDTH__=32",
+ "__CHAR16_TYPE__=short unsigned int",
+ "__PRAGMA_REDEFINE_EXTNAME=1",
+ "__SIZE_WIDTH__=32",
+ "__INT_LEAST16_MAX__=0x7fff",
+ "__INT64_MAX__=0x7fffffffffffffffLL",
+ "__SACCUM_FBIT__=7",
+ "__FLT32_DENORM_MIN__=1.4012984643248171e-45F32",
+ "__SIG_ATOMIC_WIDTH__=32",
+ "__INT_LEAST64_TYPE__=long long int",
+ "__INT16_TYPE__=short int",
+ "__INT_LEAST8_TYPE__=signed char",
+ "__cpp_structured_bindings=201606L",
+ "__SQ_FBIT__=31",
+ "__ARM_ARCH_ISA_THUMB=1",
+ "__INT_FAST8_MAX__=0x7fffffff",
+ "__ARM_ARCH=6",
+ "__INTPTR_MAX__=0x7fffffff",
+ "__cpp_sized_deallocation=201309L",
+ "__cpp_guaranteed_copy_elision=201606L",
+ "__QQ_FBIT__=7",
+ "__UTA_IBIT__=64",
+ "__FLT64_HAS_QUIET_NAN__=1",
+ "__FLT32_MIN_10_EXP__=(-37)",
+ "__EXCEPTIONS=1",
+ "__PTRDIFF_WIDTH__=32",
+ "__LDBL_MANT_DIG__=53",
+ "__SFRACT_FBIT__=7",
+ "__cpp_range_based_for=201603L",
+ "__SACCUM_MIN__=(-0X1P7HK-0X1P7HK)",
+ "__FLT64_HAS_INFINITY__=1",
+ "__STDCPP_DEFAULT_NEW_ALIGNMENT__=8",
+ "__SIG_ATOMIC_MIN__=(-__SIG_ATOMIC_MAX__ - 1)",
+ "__cpp_nontype_template_args=201411L",
+ "__cpp_return_type_deduction=201304L",
+ "__INTPTR_TYPE__=int",
+ "__UINT16_TYPE__=short unsigned int",
+ "__WCHAR_TYPE__=unsigned int",
+ "__SIZEOF_FLOAT__=4",
+ "__THUMBEL__=1",
+ "__TQ_FBIT__=127",
+ "__USQ_FBIT__=32",
+ "__UINTPTR_MAX__=0xffffffffU",
+ "__INT_FAST64_WIDTH__=64",
+ "__cpp_decltype=200707L",
+ "__FLT32_DECIMAL_DIG__=9",
+ "__INT_FAST64_MAX__=0x7fffffffffffffffLL",
+ "__GCC_ATOMIC_TEST_AND_SET_TRUEVAL=1",
+ "__FLT_NORM_MAX__=3.4028234663852886e+38F",
+ "__UINT_FAST64_TYPE__=long long unsigned int",
+ "__cpp_inline_variables=201606L",
+ "__INT_MAX__=0x7fffffff",
+ "__LACCUM_FBIT__=31",
+ "__USACCUM_MIN__=0.0UHK",
+ "__UHA_IBIT__=8",
+ "__INT64_TYPE__=long long int",
+ "__FLT_MAX_EXP__=128",
+ "__UTQ_IBIT__=0",
+ "__DBL_MANT_DIG__=53",
+ "__cpp_inheriting_constructors=201511L",
+ "__INT_LEAST64_MAX__=0x7fffffffffffffffLL",
+ "__WINT_TYPE__=unsigned int",
+ "__UINT_LEAST32_TYPE__=long unsigned int",
+ "__SIZEOF_SHORT__=2",
+ "__ULLFRACT_IBIT__=0",
+ "__FLT32_NORM_MAX__=3.4028234663852886e+38F32",
+ "__LDBL_MIN_EXP__=(-1021)",
+ "__arm__=1",
+ "__FLT64_MAX__=1.7976931348623157e+308F64",
+ "__UDA_IBIT__=32",
+ "__WINT_WIDTH__=32",
+ "__cpp_template_auto=201606L",
+ "__INT_LEAST64_WIDTH__=64",
+ "__FLT32X_MAX_10_EXP__=308",
+ "__LFRACT_FBIT__=31",
+ "__WCHAR_UNSIGNED__=1",
+ "__LDBL_MAX_10_EXP__=308",
+ "__ATOMIC_RELAXED=0",
+ "__DBL_EPSILON__=double(2.2204460492503131e-16L)",
+ "__UINT8_C(c)=c",
+ "__FLT64_MAX_EXP__=1024",
+ "__INT_LEAST32_TYPE__=long int",
+ "__SIZEOF_WCHAR_T__=4",
+ "__LLFRACT_MAX__=0X7FFFFFFFFFFFFFFFP-63LLR",
+ "__FLT64_NORM_MAX__=1.7976931348623157e+308F64",
+ "__INTMAX_MAX__=0x7fffffffffffffffLL",
+ "__INT_FAST8_TYPE__=int",
+ "__cpp_namespace_attributes=201411L",
+ "__ULLACCUM_EPSILON__=0x1P-32ULLK",
+ "__USACCUM_MAX__=0XFFFFP-8UHK",
+ "__LDBL_HAS_INFINITY__=1",
+ "__UHQ_IBIT__=0",
+ "__LLACCUM_IBIT__=32",
+ "__FLT64_HAS_DENORM__=1",
+ "__FLT32_EPSILON__=1.1920928955078125e-7F32",
+ "__DBL_DECIMAL_DIG__=17",
+ "__STDC_UTF_32__=1",
+ "__INT_FAST8_WIDTH__=32",
+ "__FLT32X_MAX__=1.7976931348623157e+308F32x",
+ "__TA_FBIT__=63",
+ "__DBL_NORM_MAX__=double(1.7976931348623157e+308L)",
+ "__BYTE_ORDER__=__ORDER_LITTLE_ENDIAN__",
+ "__GCC_DESTRUCTIVE_SIZE=64",
+ "__UDQ_IBIT__=0",
+ "__INTMAX_WIDTH__=64",
+ "__ORDER_BIG_ENDIAN__=4321",
+ "__cpp_runtime_arrays=198712L",
+ "__UINT64_TYPE__=long long unsigned int",
+ "__ACCUM_EPSILON__=0x1P-15K",
+ "__UINT32_C(c)=c ## UL",
+ "__cpp_alias_templates=200704L",
+ "__FLT_DENORM_MIN__=1.4012984643248171e-45F",
+ "__LLFRACT_IBIT__=0",
+ "__INT8_MAX__=0x7f",
+ "__LONG_WIDTH__=32",
+ "__UINT_FAST32_TYPE__=unsigned int",
+ "__FLT32X_NORM_MAX__=1.7976931348623157e+308F32x",
+ "__CHAR32_TYPE__=long unsigned int",
+ "__FLT_MAX__=3.4028234663852886e+38F",
+ "__cpp_constexpr=201603L",
+ "__USACCUM_FBIT__=8",
+ "__INT32_TYPE__=long int",
+ "__SIZEOF_DOUBLE__=8",
+ "__cpp_exceptions=199711L",
+ "__FLT_MIN_10_EXP__=(-37)",
+ "__UFRACT_EPSILON__=0x1P-16UR",
+ "__FLT64_MIN__=2.2250738585072014e-308F64",
+ "__INT_LEAST32_WIDTH__=32",
+ "__INTMAX_TYPE__=long long int",
+ "__FLT32X_HAS_QUIET_NAN__=1",
+ "__ATOMIC_CONSUME=1",
+ "__GNUC_MINOR__=3",
+ "__INT_FAST16_WIDTH__=32",
+ "__UINTMAX_MAX__=0xffffffffffffffffULL",
+ "__FLT32X_DENORM_MIN__=4.9406564584124654e-324F32x",
+ "__HA_FBIT__=7",
+ "__cpp_template_template_args=201611L",
+ "__DBL_MAX_10_EXP__=308",
+ "__LDBL_DENORM_MIN__=4.9406564584124654e-324L",
+ "__INT16_C(c)=c",
+ "__STDC__=1",
+ "__FLT32X_DIG__=15",
+ "__PTRDIFF_TYPE__=int",
+ "__LLFRACT_MIN__=(-0.5LLR-0.5LLR)",
+ "__ATOMIC_SEQ_CST=5",
+ "__DA_FBIT__=31",
+ "__UINT32_TYPE__=long unsigned int",
+ "__FLT32X_MIN_10_EXP__=(-307)",
+ "__UINTPTR_TYPE__=unsigned int",
+ "__USA_IBIT__=16",
+ "__ARM_EABI__=1",
+ "__LDBL_MIN_10_EXP__=(-307)",
+ "__cpp_generic_lambdas=201304L",
+ "__SIZEOF_LONG_LONG__=8",
+ "__ULACCUM_EPSILON__=0x1P-32ULK",
+ "__cpp_user_defined_literals=200809L",
+ "__SACCUM_IBIT__=8",
+ "__GCC_ATOMIC_LLONG_LOCK_FREE=1",
+ "__FLT_DECIMAL_DIG__=9",
+ "__UINT_FAST16_MAX__=0xffffffffU",
+ "__LDBL_NORM_MAX__=1.7976931348623157e+308L",
+ "__GCC_ATOMIC_SHORT_LOCK_FREE=1",
+ "__ULLFRACT_MAX__=0XFFFFFFFFFFFFFFFFP-64ULLR",
+ "__UINT_FAST8_TYPE__=unsigned int",
+ "__USFRACT_EPSILON__=0x1P-8UHR",
+ "__ULACCUM_FBIT__=32",
+ "__QQ_IBIT__=0",
+ "__cpp_init_captures=201304L",
+ "__ATOMIC_ACQ_REL=4",
+ "__ATOMIC_RELEASE=3",
+ "USBCON"
+ ]
+ }
+ ]
+}
\ No newline at end of file
diff --git a/simple-stepper/COBSUSBSerial.cpp b/simple-stepper/COBSUSBSerial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..03939cc7b407f9f9d5e891a19bf39b513886e3ff
--- /dev/null
+++ b/simple-stepper/COBSUSBSerial.cpp
@@ -0,0 +1,89 @@
+// link !
+// TODO: pls use nanocobs, for encode- and decode-in-place, to save big (!) memory
+// on new link layer... to fit into D11s...
+
+#include "COBSUSBSerial.h"
+#include "cobs.h"
+
+
+#if defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_RP2040)
+COBSUSBSerial::COBSUSBSerial(SerialUSB* _usbcdc){
+ usbcdc = _usbcdc;
+}
+#else
+COBSUSBSerial::COBSUSBSerial(Serial_* _usbcdc){
+ usbcdc = _usbcdc;
+}
+#endif
+
+void COBSUSBSerial::begin(void){
+ usbcdc->begin(9600);
+}
+
+void COBSUSBSerial::loop(void){
+ // check RX side:
+ // while data & not-full,
+ while(usbcdc->available() && rxBufferLen == 0){
+ rxBuffer[rxBufferWp ++] = usbcdc->read();
+ if(rxBuffer[rxBufferWp - 1] == 0){
+ // decoding in place should always work: COBS doesn't revisit bytes
+ // encoding in place would be a different trick, and would require the use of
+ // nanocobs from this lad: https://github.com/charlesnicholson/nanocobs
+ size_t len = cobsDecode(rxBuffer, 255, rxBuffer);
+ // now we are with-packet, set length and reset write pointer
+ // len includes the trailing 0, rm that...
+ rxBufferLen = len - 1;
+ rxBufferWp = 0;
+ }
+ }
+
+ // check tx side,
+ while(txBufferLen && usbcdc->availableForWrite()){
+ // ship a byte,
+ usbcdc->write(txBuffer[txBufferRp ++]);
+ // if done, mark empty
+ if(txBufferRp >= txBufferLen){
+ txBufferLen = 0;
+ txBufferRp = 0;
+ }
+ }
+}
+
+size_t COBSUSBSerial::getPacket(uint8_t* dest){
+ if(rxBufferLen > 0){
+ memcpy(dest, rxBuffer, rxBufferLen);
+ size_t len = rxBufferLen;
+ rxBufferLen = 0;
+ return len;
+ } else {
+ return 0;
+ }
+}
+
+boolean COBSUSBSerial::clearToRead(void){
+ return (rxBufferLen > 0);
+}
+
+void COBSUSBSerial::send(uint8_t* packet, size_t len){
+ // ship it! blind!
+ size_t encodedLen = cobsEncode(packet, len, txBuffer);
+ // stuff 0 byte,
+ txBuffer[encodedLen] = 0;
+ txBufferLen = encodedLen + 1;
+ txBufferRp = 0;
+}
+
+boolean COBSUSBSerial::clearToSend(void){
+ // we're CTS if we have nothing in the outbuffer,
+ return (txBufferLen == 0);
+}
+
+// we should do some... work with this, i.e.
+// keepalives, to detect if other-end is open or not...
+boolean COBSUSBSerial::isOpen(void){
+ if(usbcdc){
+ return true;
+ } else {
+ return false;
+ }
+}
\ No newline at end of file
diff --git a/simple-stepper/COBSUSBSerial.h b/simple-stepper/COBSUSBSerial.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c87ab9653062e5e00d983256ac2fb61b41ae575
--- /dev/null
+++ b/simple-stepper/COBSUSBSerial.h
@@ -0,0 +1,37 @@
+// example cobs-encoded usb-serial link
+
+#include <Arduino.h>
+
+class COBSUSBSerial {
+ public:
+ #if defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_RP2040)
+ COBSUSBSerial(SerialUSB* _usbcdc);
+ #else
+ COBSUSBSerial(Serial_* _usbcdc);
+ #endif
+ void begin(void);
+ void loop(void);
+ // check & read,
+ boolean clearToRead(void);
+ size_t getPacket(uint8_t* dest);
+ // clear ahead?
+ boolean clearToSend(void);
+ // open at all?
+ boolean isOpen(void);
+ // transmit a packet of this length
+ void send(uint8_t* packet, size_t len);
+ private:
+ #if defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_RP2040)
+ SerialUSB* usbcdc = nullptr;
+ #else
+ Serial_* usbcdc = nullptr;
+ #endif
+ // buffer, write pointer, length,
+ uint8_t rxBuffer[255];
+ uint8_t rxBufferWp = 0;
+ uint8_t rxBufferLen = 0;
+ // ibid,
+ uint8_t txBuffer[255];
+ uint8_t txBufferRp = 0;
+ uint8_t txBufferLen = 0;
+};
\ No newline at end of file
diff --git a/simple-stepper/cobs.cpp b/simple-stepper/cobs.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f243ad4a2618360ac2acd8ee80a4f48d023513b
--- /dev/null
+++ b/simple-stepper/cobs.cpp
@@ -0,0 +1,72 @@
+/*
+utils/cobs.cpp
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2019
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the osap project.
+Copyright is retained and must be preserved. The work is provided as is;
+no warranty is provided, and users accept all liability.
+*/
+
+#include "cobs.h"
+// str8 crib from
+// https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
+
+/** COBS encode data to buffer
+ @param data Pointer to input data to encode
+ @param length Number of bytes to encode
+ @param buffer Pointer to encoded output buffer
+ @return Encoded buffer length in bytes
+ @note doesn't write stop delimiter
+*/
+
+size_t cobsEncode(const void *data, size_t length, uint8_t *buffer){
+
+ uint8_t *encode = buffer; // Encoded byte pointer
+ uint8_t *codep = encode++; // Output code pointer
+ uint8_t code = 1; // Code value
+
+ for (const uint8_t *byte = (const uint8_t *)data; length--; ++byte){
+ if (*byte) // Byte not zero, write it
+ *encode++ = *byte, ++code;
+
+ if (!*byte || code == 0xff){ // Input is zero or block completed, restart
+ *codep = code, code = 1, codep = encode;
+ if (!*byte || length)
+ ++encode;
+ }
+ }
+ *codep = code; // Write final code value
+ return encode - buffer;
+}
+
+/** COBS decode data from buffer
+ @param buffer Pointer to encoded input bytes
+ @param length Number of bytes to decode
+ @param data Pointer to decoded output data
+ @return Number of bytes successfully decoded
+ @note Stops decoding if delimiter byte is found
+*/
+
+size_t cobsDecode(const uint8_t *buffer, size_t length, void *data){
+
+ const uint8_t *byte = buffer; // Encoded input byte pointer
+ uint8_t *decode = (uint8_t *)data; // Decoded output byte pointer
+
+ for (uint8_t code = 0xff, block = 0; byte < buffer + length; --block){
+ if (block) // Decode block byte
+ *decode++ = *byte++;
+ else
+ {
+ if (code != 0xff) // Encoded zero, write it
+ *decode++ = 0;
+ block = code = *byte++; // Next block length
+ if (code == 0x00) // Delimiter code found
+ break;
+ }
+ }
+
+ return decode - (uint8_t *)data;
+}
diff --git a/simple-stepper/cobs.h b/simple-stepper/cobs.h
new file mode 100644
index 0000000000000000000000000000000000000000..b47070ca26d021f113da680a6835df65712d4007
--- /dev/null
+++ b/simple-stepper/cobs.h
@@ -0,0 +1,24 @@
+/*
+utils/cobs.h
+
+consistent overhead byte stuffing implementation
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2019
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the osap project.
+Copyright is retained and must be preserved. The work is provided as is;
+no warranty is provided, and users accept all liability.
+*/
+
+#ifndef UTIL_COBS_H_
+#define UTIL_COBS_H_
+
+#include <Arduino.h>
+
+size_t cobsEncode(const void *data, size_t length, uint8_t *buffer);
+
+size_t cobsDecode(const uint8_t *buffer, size_t length, void *data);
+
+#endif
diff --git a/simple-stepper/fixedPointUtes.cpp b/simple-stepper/fixedPointUtes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..af6568d3a8338ac20fc51f62622fa55c00464617
--- /dev/null
+++ b/simple-stepper/fixedPointUtes.cpp
@@ -0,0 +1,80 @@
+#include "fixedPointUtes.h"
+
+// ---------------------------------------------- Fixed Point Maths
+
+// FP 16.16 puts 65k ticks in front and in behind,
+// for i.e. max rate of 32k 'units' (it's signed)
+// which correlates to 9830 RPM for a 200-full-step stepper
+const int32_t fp_scale = 16;
+const float fp_float_max = 32768.0F;
+const float fp_float_min = -32768.0F;
+const int32_t fp_int_max = 32768;
+const int32_t fp_int_min = -32768;
+const int64_t fp_32b_max = 2147483648;
+const int64_t fp_32b_min = -2147483648;
+
+// hmmm https://www.youtube.com/watch?v=S12qx1DwjVk& at ~ 18:00
+float fp_fixed32ToFloat(fpint32_t fixed){
+ return ((float)fixed / (float)(1 << fp_scale));
+}
+
+float fp_fixed64ToFloat(fpint64_t fixed){
+ return ((float)fixed / (float)(1 << fp_scale));
+}
+
+// actually this is unclear to me... https://www.youtube.com/watch?v=S12qx1DwjVk& at 16:57
+fpint32_t fp_floatToFixed32(float flt){
+ if(flt > fp_float_max) flt = fp_float_max;
+ if(flt < fp_float_min) flt = fp_float_min;
+ return (flt * (float)(1 << fp_scale));
+}
+
+fpint64_t fp_floatToFixed64(float flt){
+ return (flt * (float)(1 << fp_scale));
+}
+
+int32_t fp_fixed32ToInt32(fpint32_t fixed){
+ return (fixed >> fp_scale);
+}
+
+fpint32_t fp_int32ToFixed32(int32_t inty){
+ if(inty > fp_int_max) inty = fp_int_max;
+ if(inty < fp_int_min) inty = fp_int_min;
+ return (inty << fp_scale);
+}
+
+fpint32_t fp_mult32x32(fpint32_t a, fpint32_t b){
+ // the result of this mult can be > our max possible 32b-wide fixedp,
+ int64_t res = ((int64_t)(a) * (int64_t)(b)) >> fp_scale;
+ // so we guard against that, since it is cast back to 32b on exit,
+ if(res > fp_32b_max) res = fp_32b_max;
+ if(res < fp_32b_min) res = fp_32b_min;
+ return res;
+}
+
+// we could instead do mult w/ some fancy shifting, for speed (no 64b at all)
+// but it also makes for tricky overflow / etc; I'm not going to get into this yet
+// https://www.youtube.com/watch?v=npQF28g6s_k& 7:40
+// fp_int32_t fp_mult(fpint32_t a, fpint32_t b){
+// return ((a >> 6) * (b >> 6)) >> 4;
+// }
+
+fpint32_t fp_div32x32(fpint32_t num, fpint32_t denum){
+ return ((int64_t)(num) << fp_scale) / denum;
+}
+
+fpint64_t fp_mult32x32_64(fpint32_t a, fpint32_t b){
+ return ((fpint64_t)(a) * (fpint64_t)(b)) >> fp_scale;
+}
+
+fpint64_t fp_div64x64(fpint64_t num, fpint64_t denum){
+ return (num << 16) / denum;
+}
+
+// // big-div,
+// fpint64_t fp_calcStopDistance(fpint32_t _vel, fpint32_t _maxAccel){
+// // return 0;
+// int64_t _velSq = ((int64_t)(_vel) * (int64_t)(_vel)) >> fp_scale;
+// int64_t _accelTwo = ((int64_t)(_maxAccel) * (int64_t)(fp_int32ToFixed32(2))) >> fp_scale;
+// return (_velSq << fp_scale) / _accelTwo;
+// }
\ No newline at end of file
diff --git a/simple-stepper/fixedPointUtes.h b/simple-stepper/fixedPointUtes.h
new file mode 100644
index 0000000000000000000000000000000000000000..20c0c6cac09edd403a2a105ccf02afe61b31ef59
--- /dev/null
+++ b/simple-stepper/fixedPointUtes.h
@@ -0,0 +1,36 @@
+#ifndef FIXEDPOINT_UTES_H_
+#define FIXEDPOINT_UTES_H_
+
+#include <Arduino.h>
+
+// ---------------------------------------------- structs and interfaes
+
+// we get explicit about fixed point
+typedef int32_t fpint32_t;
+typedef int64_t fpint64_t;
+
+// ---------------------------------------------- fixedp conversions
+
+float fp_fixed32ToFloat(fpint32_t fixed);
+float fp_fixed64ToFloat(fpint64_t fixed);
+
+fpint32_t fp_floatToFixed32(float flt);
+fpint64_t fp_floatToFixed64(float flt);
+
+int32_t fp_fixed32ToInt32(fpint32_t fixed);
+
+fpint32_t fp_int32ToFixed32(int32_t inty);
+
+// ---------------------------------------------- fixedp maths
+
+fpint32_t fp_mult32x32(fpint32_t a, fpint32_t b);
+
+fpint32_t fp_div32x32(fpint32_t num, fpint32_t denum);
+
+// ---------------------------------------------- overflow fixedp maths
+
+fpint64_t fp_mult32x32_64(fpint32_t a, fpint32_t b);
+
+fpint64_t fp_div64x64(fpint64_t num, fpint64_t denum);
+
+#endif
\ No newline at end of file
diff --git a/simple-stepper/lutgen.js b/simple-stepper/lutgen.js
new file mode 100644
index 0000000000000000000000000000000000000000..07a6d1499e9af17bb5b3f8981f89b679044dadec
--- /dev/null
+++ b/simple-stepper/lutgen.js
@@ -0,0 +1,60 @@
+import fs from 'fs';
+import process from 'process';
+
+// Function to generate the sinusoid values
+function generateSinusoid(length, amplitude) {
+ let values = [];
+ for (let i = 0; i < length; i++) {
+ let angle = (Math.PI * i) / (length - 1);
+ let value = Math.round(amplitude * Math.sin(angle));
+ values.push(value);
+ }
+ return values;
+}
+
+// Main function to create the lookup table
+function createLookupTable(length, amplitude) {
+ const values = generateSinusoid(length, amplitude);
+ let lutString = `#ifndef STEPPER_LUT_H_\n#define STEPPER_LUT_H_\n\n`
+ lutString += `#include <Arduino.h>\n\n`;
+ lutString += `#define LUT_LENGTH ${length} \n`;
+ lutString += `#define PWM_PERIOD ${amplitude}\n\n`;
+ lutString += `const uint16_t LUT[LUT_LENGTH] = {\n`;
+ // do 'em row by row
+ let i = 0;
+ while (i < values.length) {
+ lutString += ` `;
+ for (let j = 0; j < 16; j++) {
+ lutString += `${values[i]}, `;
+ i++;
+ if (i >= values.length) break;
+ }
+ lutString += `\n`;
+ }
+ // lutString += values.join(',') + ',\n';
+ lutString += '};\n\n';
+ lutString += `#endif\n`
+ return lutString;
+}
+
+// Read command-line arguments
+const length = parseInt(process.argv[2]);
+const amplitude = parseInt(process.argv[3]);
+
+// Validate arguments
+if (isNaN(length) || isNaN(amplitude)) {
+ console.error('Usage: node script.js <length> <amplitude>');
+ process.exit(1);
+}
+
+// Generate the lookup table string
+const lutString = createLookupTable(length, amplitude);
+
+// Write to file
+fs.writeFile('stepperLUT.h', lutString, (err) => {
+ if (err) {
+ console.error('Error writing to file:', err);
+ } else {
+ console.log('Lookup table written to stepperLUT.h');
+ }
+});
diff --git a/simple-stepper/motionStateMachine.cpp b/simple-stepper/motionStateMachine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c66f4fafcc8ba0dbeee6353d375ed685479d1674
--- /dev/null
+++ b/simple-stepper/motionStateMachine.cpp
@@ -0,0 +1,215 @@
+#include "motionStateMachine.h"
+#include "fixedPointUtes.h"
+#include "stepperDriver.h"
+
+// overlapping with the limit pin !
+#define PIN_DEBUG 26
+
+#define ALARM_DT_NUM 1
+#define ALARM_DT_IRQ TIMER_IRQ_1
+
+// _delT is re-calculated when we init w/ a new microsecondsPerIntegration
+fpint32_t _delT = 0;
+uint32_t _delT_us = 0;
+
+// core states (units are full steps)
+volatile uint8_t _mode = MOTION_MODE_VEL; // operative _mode
+volatile fpint64_t _pos = 0; // current position (big 64)
+volatile fpint32_t _vel = 0; // current velocity
+volatile fpint32_t _accel = 0; // current acceleration
+
+// init-once values we'll use in the integrator
+volatile fpint32_t _delta = 0;
+volatile fpint64_t _posTarget = 0; // for position control,
+volatile fpint64_t _dist = 0; // for position control,
+volatile fpint64_t _stopDistance = 0; // for position control,
+
+// and settings
+fpint32_t _maxAccel = 0; // maximum acceleration
+fpint32_t _maxVelocity = 0;
+
+// nasty !
+fpint32_t _velEpsilon = 0;
+
+void motion_init(uint32_t microsecondsPerIntegration){
+ // here's our delta-tee for each integration step
+ _delT = fp_floatToFixed32((float)(microsecondsPerIntegration) / 1000000.0F);
+ // and (for resetting our timer) the _delta directly in us
+ _delT_us = microsecondsPerIntegration;
+
+ // setup the hardware timer
+ hw_set_bits(&timer_hw->inte, 1u << ALARM_DT_NUM);
+ irq_set_exclusive_handler(ALARM_DT_IRQ, alarm_dt_handler);
+ irq_set_enabled(ALARM_DT_IRQ, true);
+ timer_hw->alarm[ALARM_DT_NUM] = (uint32_t) (timer_hw->timerawl + _delT_us);
+
+ // (optionally) use a debug pin to perf test
+ // pinMode(PIN_DEBUG, OUTPUT);
+ _velEpsilon = fp_floatToFixed32(1.0F);
+}
+
+void alarm_dt_handler(void){
+ // setup next call right away
+ hw_clear_bits(&timer_hw->intr, 1u << ALARM_DT_NUM);
+ timer_hw->alarm[ALARM_DT_NUM] = (uint32_t) (timer_hw->timerawl + _delT_us);
+ // do the motion system integration,
+ // sio_hw->gpio_set = (uint32_t)(1 << PIN_DEBUG);
+ motion_integrate();
+ // sio_hw->gpio_clr = (uint32_t)(1 << PIN_DEBUG);
+}
+
+// ------------------------------------ integrator codes
+
+fpint64_t motion_calc_stopping_distance(fpint32_t vel, fpint32_t accel){
+ // our num / denum,
+ fpint64_t velSqrd = fp_mult32x32_64(vel, vel);
+ fpint64_t twoAccel = fp_mult32x32_64(fp_int32ToFixed32(2), accel);
+ // now we div that out,
+ return fp_div64x64(velSqrd, twoAccel);
+}
+
+void motion_calc_mode_velocity(void){
+ // go fast, or go slo;
+ if(_vel < _maxVelocity){
+ _accel = _maxAccel;
+ } else if (_vel > _maxVelocity){
+ _accel = -_maxAccel;
+ }
+
+ // using our chosen accel, integrate velocity from previous:
+ _vel += fp_mult32x32(_accel, _delT);
+
+ // and check against targets
+ // TODO: hack-ey velocity-at-target thing (1st if here)
+ // should dead-reckon step intervals, checking a delta,
+ // but requires that we check which direction we are going (towards velocity)
+ if( _maxVelocity < _velEpsilon && _maxVelocity > -_velEpsilon && // if we are near-zero target velocity,
+ _vel < _velEpsilon && _vel > -_velEpsilon // and we are also ... near that velocity,
+ ){
+ _accel = 0;
+ _vel = 0;
+ } else if(_vel > 0 && _maxVelocity > 0 && _vel > _maxVelocity){
+ _accel = 0;
+ _vel = _maxVelocity;
+ } else if(_vel < 0 && _maxVelocity < 0 && _vel < _maxVelocity){
+ _accel = 0;
+ _vel = _maxVelocity;
+ }
+}
+
+void motion_calc_mode_position(void){
+ // figure whence we would need to start stopping, and our current dist
+ _stopDistance = motion_calc_stopping_distance(_vel, _maxAccel);
+ _dist = _posTarget - _pos;
+
+ // check if we're about to make it... bonk if so,
+ // units are steps, so epsilon is tiny !
+ if(abs(_dist - _delta) < fp_floatToFixed32(0.001F)){
+ _vel = 0;
+ _accel = 0;
+ return;
+ }
+
+ // now we do a buncha cheques
+ if(_stopDistance >= abs(_dist)){ // we're going to overshoot,
+ if(_vel <= 0){ // when -ve vel,
+ _accel = _maxAccel; // do +ve accel
+ } else { // when +ve vel,
+ _accel = -_maxAccel; // do -ve accel
+ }
+ } else { // we're not overshooting,
+ if(_dist > 0){ // if delta is positive,
+ _accel = _maxAccel; // go forwards,
+ } else { // if it's negative
+ _accel = -_maxAccel; // go backwards...
+ }
+ }
+
+ // using our chosen accel, integrate velocity from previous:
+ _vel += fp_mult32x32(_accel, _delT);
+
+ // cap our _vel based on maximum rates:
+ if(_vel >= _maxVelocity){
+ _accel = 0;
+ _vel = _maxVelocity;
+ } else if (_vel <= -_maxVelocity){
+ _accel = 0;
+ _vel = -_maxVelocity;
+ }
+}
+
+void motion_integrate(void){
+ // set our _accel based on modal requests,
+ switch(_mode){
+ case MOTION_MODE_POS:
+ motion_calc_mode_position();
+ break;
+ case MOTION_MODE_VEL:
+ motion_calc_mode_velocity();
+ break;
+ }
+
+ // grab a delta and integrate,
+ _delta = fp_mult32x32(_vel, _delT);
+ _pos += _delta;
+
+ // position is base 16, we can just shift our way to relative electrical phase
+ // 0-2048 (11 bits) per electrical phase, is
+ // 0-512 (9 bits) per step,
+ // 0b XXXXXXXXXXXXXXXX.XXXXXXXXXXXXXXXX
+ // 0b XX.XXXXXXXXX
+ // FS.MicroStep
+ uint16_t phaseAngle = (_pos >> 7) & 0b11111111111;
+ stepper_point(phaseAngle);
+}
+
+// ------------------------------------ end integrator
+
+void motion_setVelocityTarget(float target, float maxAccel){
+ noInterrupts();
+ _maxAccel = fp_floatToFixed32(abs(maxAccel));
+ _maxVelocity = fp_floatToFixed32(target);
+ _mode = MOTION_MODE_VEL;
+ interrupts();
+}
+
+void motion_setPosition(float pos){
+ noInterrupts();
+ _pos = fp_floatToFixed64(pos);
+ interrupts();
+}
+
+void motion_getCurrentStates(motionState_t* statePtr){
+ noInterrupts();
+ statePtr->pos = fp_fixed64ToFloat(_pos);
+ statePtr->vel = fp_fixed32ToFloat(_vel);
+ statePtr->accel = fp_fixed32ToFloat(_accel);
+ interrupts();
+}
+
+void motion_setPositionTarget(float target, float maxVel, float maxAccel){
+ noInterrupts();
+ _posTarget = fp_floatToFixed64(target);
+ _maxVelocity = fp_floatToFixed32(abs(maxVel));
+ _maxAccel = fp_floatToFixed32(abs(maxAccel));
+ _mode = MOTION_MODE_POS;
+ interrupts();
+}
+
+// void motion_debug(void){
+// // ... we could / should snapshot, if this is chunky ?
+// noInterrupts();
+// Serial.println(String(millis())
+// // + "\t_delT: \t" + String(fp_fixed32ToFloat(_delT), 6)
+// + "\tm: " + String(_mode)
+// + "\tptrg: " + String(fp_fixed64ToFloat(_posTarget), 1)
+// + "\tvtrg: " + String(fp_fixed64ToFloat(_maxVelocity), 1)
+// + "\tpos: " + String(fp_fixed64ToFloat(_pos), 1)
+// + "\tdst: " + String(fp_fixed64ToFloat(_dist), 1)
+// + "\tstp: " + String(fp_fixed64ToFloat(_stopDistance), 1)
+// + "\tacc: " + String(fp_fixed32ToFloat(_accel), 1)
+// + "\tvel: " + String(fp_fixed32ToFloat(_vel), 1)
+// // + "\tvtrg: \t" + String(fp_fixed32ToFloat(_maxVelocity), 4)
+// );
+// interrupts();
+// }
diff --git a/simple-stepper/motionStateMachine.h b/simple-stepper/motionStateMachine.h
new file mode 100644
index 0000000000000000000000000000000000000000..678abf78815698e08d855211b7701267ce29a522
--- /dev/null
+++ b/simple-stepper/motionStateMachine.h
@@ -0,0 +1,28 @@
+#ifndef MOTION_STATE_MACHINE_H_
+#define MOTION_STATE_MACHINE_H_
+
+#include <Arduino.h>
+#include <hardware/timer.h>
+#include <hardware/irq.h>
+
+#define MOTION_MODE_POS 0
+#define MOTION_MODE_VEL 1
+
+typedef struct motionState_t {
+ float pos;
+ float vel;
+ float accel;
+} motionState_t;
+
+void motion_init(uint32_t microsecondsPerIntegration);
+
+void motion_integrate(void);
+void alarm_dt_handler(void);
+
+void motion_setPositionTarget(float target, float maxVel, float maxAccel);
+void motion_setVelocityTarget(float target, float maxAccel);
+void motion_setPosition(float pos);
+
+void motion_getCurrentStates(motionState_t* statePtr);
+
+#endif
diff --git a/simple-stepper/serializationUtes.cpp b/simple-stepper/serializationUtes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d40cd2193b351ac1d15e27e3cba674648737dcbf
--- /dev/null
+++ b/simple-stepper/serializationUtes.cpp
@@ -0,0 +1,14 @@
+#include "serializationUtes.h"
+
+float ts_readFloat32(unsigned char* buf, uint16_t* ptr){
+ chunk_float32 chunk = { .bytes = { buf[(*ptr)], buf[(*ptr) + 1], buf[(*ptr) + 2], buf[(*ptr) + 3] } };
+ (*ptr) += 4;
+ return chunk.f;
+}
+
+void ts_writeFloat32(float val, volatile unsigned char* buf, uint16_t* ptr){
+ chunk_float32 chunk;
+ chunk.f = val;
+ buf[(*ptr)] = chunk.bytes[0]; buf[(*ptr) + 1] = chunk.bytes[1]; buf[(*ptr) + 2] = chunk.bytes[2]; buf[(*ptr) + 3] = chunk.bytes[3];
+ (*ptr) += 4;
+}
\ No newline at end of file
diff --git a/simple-stepper/serializationUtes.h b/simple-stepper/serializationUtes.h
new file mode 100644
index 0000000000000000000000000000000000000000..5549f09e32458c55014cfd8b9880cf028e965383
--- /dev/null
+++ b/simple-stepper/serializationUtes.h
@@ -0,0 +1,15 @@
+#ifndef SERIALIZATION_UTES_H_
+#define SERIALIZATION_UTES_H_
+
+#include <Arduino.h>
+
+union chunk_float32 {
+ uint8_t bytes[4];
+ float f;
+};
+
+float ts_readFloat32(unsigned char* buf, uint16_t* ptr);
+
+void ts_writeFloat32(float val, volatile unsigned char* buf, uint16_t* ptr);
+
+#endif
\ No newline at end of file
diff --git a/simple-stepper/simple-stepper.ino b/simple-stepper/simple-stepper.ino
new file mode 100644
index 0000000000000000000000000000000000000000..613158e0acbc3f146c48631807317175795bca8f
--- /dev/null
+++ b/simple-stepper/simple-stepper.ino
@@ -0,0 +1,154 @@
+#include "COBSUSBSerial.h"
+#include "serializationUtes.h"
+#include "motionStateMachine.h"
+#include "stepperDriver.h"
+
+// no-transport-layer stepper motor
+// using the RP2040 at 200MHz
+// using https://modular-things.github.io/modular-things/things/stepper-hbridge-xiao/
+
+#define PIN_LIMIT 26
+
+#define MSG_GET_ID 7
+#define MSG_ECHO_TEST 11
+#define MSG_SET_TARG_VEL 21
+#define MSG_SET_TARG_POS 22
+#define MSG_SET_POSITION 23
+#define MSG_SET_CURRENT 24
+#define MSG_GET_STATES 25
+#define MSG_ACK 31
+#define MSG_NACK 32
+
+// y_motor: 1, x_bottom_motor: 2, x_top_motor: 3
+#define MOTOR_ID 3
+
+COBSUSBSerial cobs(&Serial);
+
+
+void setTargetVelocity(uint8_t* data, size_t len){
+ uint16_t wptr = 0;
+ float targ = ts_readFloat32(data, &wptr);
+ float maxAccel = ts_readFloat32(data, &wptr);
+ motion_setVelocityTarget(targ, maxAccel);
+}
+
+
+void setTargetPosition(uint8_t* data, size_t len){
+ uint16_t wptr = 0;
+ float targ = ts_readFloat32(data, &wptr);
+ float maxVel = ts_readFloat32(data, &wptr);
+ float maxAccel = ts_readFloat32(data, &wptr);
+ motion_setPositionTarget(targ, maxVel, maxAccel);
+}
+
+
+void setPosition(uint8_t* data, size_t len){
+ // should do maxAccel, maxVel, and (optionally) setPosition
+ // upstream should've though of this, so,
+ uint16_t rptr = 0;
+ float pos = ts_readFloat32(data, &rptr);
+ motion_setPosition(pos);
+}
+
+
+void setCurrent(uint8_t* data, size_t len){
+ // it's just <cscale> for the time being,
+ uint16_t rptr = 0;
+ float cscale = ts_readFloat32(data, &rptr);
+ // we get that as a floating p, 0-1,
+ // driver wants integers 0-1024:
+ uint32_t amp = cscale * 1024;
+ stepper_setAmplitude(amp);
+}
+
+
+boolean lastButtonState = false;
+
+size_t getStates(uint8_t* data, size_t len, uint8_t* reply){
+ motionState_t state;
+ motion_getCurrentStates(&state);
+ uint16_t wptr = 0;
+ // in-fill current posn, velocity, and acceleration
+ ts_writeFloat32(state.pos, reply, &wptr);
+ ts_writeFloat32(state.vel, reply, &wptr);
+ ts_writeFloat32(state.accel, reply, &wptr);
+ // and limit state,
+ reply[wptr ++] = (lastButtonState ? 1 : 0);
+ // return the data length
+ return wptr;
+}
+
+
+void setup() {
+ // startup the stepper hardware
+ stepper_init();
+ // setup motion, pick an integration interval (us)
+ motion_init(64);
+ // and our limit pin, is wired (to spec)
+ // to a normally-closed switch, from SIG to GND,
+ // meaning that when the switch is "clicked" - it will open,
+ // and the pullup will win, we will have logic high
+ pinMode(PIN_LIMIT, INPUT_PULLUP);
+ // and our comms
+ cobs.begin();
+}
+
+
+uint32_t debounceDelay = 1;
+uint32_t lastButtonCheck = 0;
+
+motionState_t states;
+
+uint8_t msg[256];
+uint8_t reply[256];
+
+void loop() {
+ // run comms
+ cobs.loop();
+ // and check for paquiats: condition is if we can
+ // get a packet, and tx the reply
+ if(cobs.clearToRead() && cobs.clearToSend()){
+ size_t len = cobs.getPacket(msg);
+ // start a reply,
+ size_t wptr = 0;
+ reply[wptr ++] = MSG_ACK;
+ // switch on grams,
+ switch(msg[0]){
+ case MSG_GET_ID:
+ reply[wptr ++] = MOTOR_ID;
+ break;
+ case MSG_ECHO_TEST:
+ memcpy(reply, msg, len);
+ wptr = len;
+ break;
+ case MSG_SET_TARG_VEL:
+ setTargetVelocity(msg + 1, len - 1);
+ break;
+ case MSG_SET_TARG_POS:
+ setTargetPosition(msg + 1, len - 1);
+ break;
+ case MSG_SET_POSITION:
+ setPosition(msg + 1, len - 1);
+ break;
+ case MSG_SET_CURRENT:
+ setCurrent(msg + 1, len - 1);
+ break;
+ case MSG_GET_STATES:
+ wptr += getStates(msg + 1, len - 1, reply + 1);
+ break;
+ default:
+ reply[0] = MSG_NACK;
+ break;
+ }
+ // send the reply,
+ cobs.send(reply, wptr);
+ }
+ // debounce and set button states,
+ if(lastButtonCheck + debounceDelay < millis()){
+ lastButtonCheck = millis();
+ boolean newState = digitalRead(PIN_LIMIT);
+ if(newState != lastButtonState){
+ lastButtonState = newState;
+ }
+ }
+}
diff --git a/simple-stepper/stepperDriver.cpp b/simple-stepper/stepperDriver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4dfdff3ddea8defb89355514d73505e4ea8e7344
--- /dev/null
+++ b/simple-stepper/stepperDriver.cpp
@@ -0,0 +1,143 @@
+/*
+stepperDriver.cpp
+
+stepper code for two A4950s or TB67... w/ VREF via TC -> RC Filters
+for the RP2040 XIAO !
+
+Jake Read at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2022
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the squidworks and ponyo
+projects. Copyright is retained and must be preserved. The work is provided as
+is; no warranty is provided, and users accept all liability.
+*/
+
+#include "stepperDriver.h"
+#include "stepperLUT.h"
+
+#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
+
+// macros to set h-bridge dir pins
+
+#define AIN1_HI sio_hw->gpio_set = (uint32_t)(1 << AIN1_PIN)
+#define AIN1_LO sio_hw->gpio_clr = (uint32_t)(1 << AIN1_PIN)
+
+#define AIN2_HI sio_hw->gpio_set = (uint32_t)(1 << AIN2_PIN)
+#define AIN2_LO sio_hw->gpio_clr = (uint32_t)(1 << AIN2_PIN)
+
+#define BIN1_HI sio_hw->gpio_set = (uint32_t)(1 << BIN1_PIN)
+#define BIN1_LO sio_hw->gpio_clr = (uint32_t)(1 << BIN1_PIN)
+
+#define BIN2_HI sio_hw->gpio_set = (uint32_t)(1 << BIN2_PIN)
+#define BIN2_LO sio_hw->gpio_clr = (uint32_t)(1 << BIN2_PIN)
+
+// macros to set h-bridge dir states
+
+#define A_UP AIN2_LO; AIN1_HI
+#define B_UP BIN2_LO; BIN1_HI
+
+#define A_OFF AIN2_LO; AIN1_LO
+#define B_OFF BIN2_LO; BIN1_LO
+
+#define A_DOWN AIN1_LO; AIN2_HI
+#define B_DOWN BIN1_LO; BIN2_HI
+
+// hardware pwm vals
+
+uint16_t sliceNumA;
+uint16_t sliceNumB;
+uint16_t channelA;
+uint16_t channelB;
+
+void stepper_init(void){
+ // -------------------------------------------- DIR PINS
+ // all of 'em, outputs
+ pinMode(AIN1_PIN, OUTPUT);
+ pinMode(AIN2_PIN, OUTPUT);
+ pinMode(BIN1_PIN, OUTPUT);
+ pinMode(BIN2_PIN, OUTPUT);
+
+ gpio_set_function(APWM_PIN, GPIO_FUNC_PWM);
+ gpio_set_function(BPWM_PIN, GPIO_FUNC_PWM);
+ sliceNumA = pwm_gpio_to_slice_num(APWM_PIN);
+ sliceNumB = pwm_gpio_to_slice_num(BPWM_PIN);
+ channelA = pwm_gpio_to_channel(APWM_PIN);
+ channelB = pwm_gpio_to_channel(BPWM_PIN);
+
+ // TODO: check about the old code, with Q ?
+ // or fk it, go full beans always ?
+ // uint32_t f_sys = clock_get_hz(clk_sys);
+ float divider = 1.0F; //(float)f_sys / (PWM_PERIOD * 10000UL);
+
+ pwm_set_clkdiv(sliceNumA, divider);
+ pwm_set_clkdiv(sliceNumB, divider);
+
+ // pwm period
+ pwm_set_wrap(sliceNumA, PWM_PERIOD);
+ pwm_set_wrap(sliceNumB, PWM_PERIOD);
+
+ // set a start-up value of 1 / PWM_PERIOD
+ pwm_set_chan_level(sliceNumA, channelA, 1);
+ pwm_set_chan_level(sliceNumB, channelB, 1);
+
+ // Set the PWM running
+ pwm_set_enabled(sliceNumA, true);
+ pwm_set_enabled(sliceNumB, true);
+}
+
+void stepper_point(uint16_t phaseAngle, uint16_t amplitude){
+ // wrap phaseAngle to 2048, and get a / b components
+ uint16_t coilAPhase = phaseAngle & 0b0000011111111111;
+ uint16_t coilBPhase = (phaseAngle + LUT_LENGTH / 2) & 0b0000011111111111;
+
+ // clamp amplitude,
+ amplitude = amplitude & 0b0000001111111111;
+
+ // a coil dir
+ if (coilAPhase > LUT_LENGTH){
+ A_DOWN;
+ } else if (coilAPhase < LUT_LENGTH){
+ A_UP;
+ } else {
+ A_OFF;
+ }
+ // b coil dir
+ if (coilBPhase > LUT_LENGTH){
+ B_DOWN;
+ } else if (coilBPhase < LUT_LENGTH){
+ B_UP;
+ } else {
+ B_OFF;
+ }
+
+ // now we rectify each to the positive half wave,
+ coilAPhase = coilAPhase & 0b0000001111111111;
+ coilBPhase = coilBPhase & 0b0000001111111111;
+
+ // expand to 32 for multiply overflow,
+ // then do fixed-point where 0-1.0 == 0-1024, using 2^10 bit divide
+ uint32_t coilAMag = (LUT[coilAPhase] * amplitude) >> 10;
+ uint32_t coilBMag = (LUT[coilBPhase] * amplitude) >> 10;
+
+ // and set amplitudes...
+ pwm_set_chan_level(sliceNumA, channelA, coilAMag);
+ pwm_set_chan_level(sliceNumB, channelB, coilBMag);
+}
+
+uint16_t _amplitude = 0;
+
+void stepper_point(uint16_t phaseAngle){
+ stepper_point(phaseAngle, _amplitude);
+}
+
+void stepper_setAmplitude(uint16_t amplitude){
+ if(amplitude > 1024) amplitude = 1024;
+ if(amplitude < 0) amplitude = 0;
+ _amplitude = amplitude;
+}
\ No newline at end of file
diff --git a/simple-stepper/stepperDriver.h b/simple-stepper/stepperDriver.h
new file mode 100644
index 0000000000000000000000000000000000000000..dfc2186ddf8b348bcba049f87e3ab80fb59a7e8d
--- /dev/null
+++ b/simple-stepper/stepperDriver.h
@@ -0,0 +1,34 @@
+/*
+stepperDriver.h
+
+stepper code for two A4950s w/ VREF via TC -> RC Filters
+with RP2040 support
+
+Jake Read & Quentin Bolsee at the Center for Bits and Atoms
+(c) Massachusetts Institute of Technology 2023
+
+This work may be reproduced, modified, distributed, performed, and
+displayed for any purpose, but must acknowledge the squidworks and ponyo
+projects. Copyright is retained and must be preserved. The work is provided as
+is; no warranty is provided, and users accept all liability.
+*/
+
+#ifndef STEPPER_DRIVER_H_
+#define STEPPER_DRIVER_H_
+
+#include <Arduino.h>
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+
+// startup stepper hardware
+void stepper_init(void);
+
+// mapping 0-2PI is 0-2048 and 0-1 is 0-1024
+void stepper_point(uint16_t phaseAngle, uint16_t amplitude);
+
+// or, omitting amplitude...
+void stepper_point(uint16_t phaseAngle);
+
+void stepper_setAmplitude(uint16_t amplitude);
+
+#endif
diff --git a/simple-stepper/stepperLUT.h b/simple-stepper/stepperLUT.h
new file mode 100644
index 0000000000000000000000000000000000000000..82b338c4c81cdf3117f49d34715b6729807df7eb
--- /dev/null
+++ b/simple-stepper/stepperLUT.h
@@ -0,0 +1,76 @@
+#ifndef STEPPER_LUT_H_
+#define STEPPER_LUT_H_
+
+#include <Arduino.h>
+
+#define LUT_LENGTH 1024
+#define PWM_PERIOD 2048
+
+const uint16_t LUT[LUT_LENGTH] = {
+ 0, 6, 13, 19, 25, 31, 38, 44, 50, 57, 63, 69, 75, 82, 88, 94,
+ 101, 107, 113, 119, 126, 132, 138, 145, 151, 157, 163, 170, 176, 182, 188, 195,
+ 201, 207, 213, 220, 226, 232, 238, 245, 251, 257, 263, 270, 276, 282, 288, 295,
+ 301, 307, 313, 319, 326, 332, 338, 344, 350, 357, 363, 369, 375, 381, 388, 394,
+ 400, 406, 412, 418, 425, 431, 437, 443, 449, 455, 461, 468, 474, 480, 486, 492,
+ 498, 504, 510, 516, 522, 529, 535, 541, 547, 553, 559, 565, 571, 577, 583, 589,
+ 595, 601, 607, 613, 619, 625, 631, 637, 643, 649, 655, 661, 667, 673, 679, 685,
+ 691, 697, 702, 708, 714, 720, 726, 732, 738, 744, 749, 755, 761, 767, 773, 779,
+ 784, 790, 796, 802, 808, 813, 819, 825, 831, 836, 842, 848, 854, 859, 865, 871,
+ 876, 882, 888, 893, 899, 905, 910, 916, 922, 927, 933, 938, 944, 950, 955, 961,
+ 966, 972, 977, 983, 988, 994, 999, 1005, 1010, 1016, 1021, 1027, 1032, 1038, 1043, 1048,
+ 1054, 1059, 1065, 1070, 1075, 1081, 1086, 1091, 1097, 1102, 1107, 1113, 1118, 1123, 1128, 1134,
+ 1139, 1144, 1149, 1154, 1160, 1165, 1170, 1175, 1180, 1185, 1191, 1196, 1201, 1206, 1211, 1216,
+ 1221, 1226, 1231, 1236, 1241, 1246, 1251, 1256, 1261, 1266, 1271, 1276, 1281, 1286, 1291, 1295,
+ 1300, 1305, 1310, 1315, 1320, 1324, 1329, 1334, 1339, 1344, 1348, 1353, 1358, 1362, 1367, 1372,
+ 1376, 1381, 1386, 1390, 1395, 1400, 1404, 1409, 1413, 1418, 1422, 1427, 1431, 1436, 1440, 1445,
+ 1449, 1454, 1458, 1463, 1467, 1471, 1476, 1480, 1484, 1489, 1493, 1497, 1502, 1506, 1510, 1514,
+ 1519, 1523, 1527, 1531, 1535, 1540, 1544, 1548, 1552, 1556, 1560, 1564, 1568, 1572, 1576, 1580,
+ 1584, 1588, 1592, 1596, 1600, 1604, 1608, 1612, 1616, 1620, 1623, 1627, 1631, 1635, 1639, 1642,
+ 1646, 1650, 1654, 1657, 1661, 1665, 1668, 1672, 1676, 1679, 1683, 1686, 1690, 1693, 1697, 1700,
+ 1704, 1707, 1711, 1714, 1718, 1721, 1725, 1728, 1731, 1735, 1738, 1741, 1745, 1748, 1751, 1754,
+ 1758, 1761, 1764, 1767, 1770, 1774, 1777, 1780, 1783, 1786, 1789, 1792, 1795, 1798, 1801, 1804,
+ 1807, 1810, 1813, 1816, 1819, 1822, 1825, 1827, 1830, 1833, 1836, 1839, 1841, 1844, 1847, 1850,
+ 1852, 1855, 1858, 1860, 1863, 1866, 1868, 1871, 1873, 1876, 1878, 1881, 1883, 1886, 1888, 1891,
+ 1893, 1895, 1898, 1900, 1902, 1905, 1907, 1909, 1912, 1914, 1916, 1918, 1921, 1923, 1925, 1927,
+ 1929, 1931, 1933, 1935, 1937, 1939, 1941, 1943, 1945, 1947, 1949, 1951, 1953, 1955, 1957, 1959,
+ 1961, 1962, 1964, 1966, 1968, 1969, 1971, 1973, 1975, 1976, 1978, 1979, 1981, 1983, 1984, 1986,
+ 1987, 1989, 1990, 1992, 1993, 1995, 1996, 1997, 1999, 2000, 2002, 2003, 2004, 2005, 2007, 2008,
+ 2009, 2010, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, 2020, 2021, 2022, 2023, 2024, 2025,
+ 2026, 2027, 2028, 2029, 2030, 2031, 2031, 2032, 2033, 2034, 2034, 2035, 2036, 2037, 2037, 2038,
+ 2038, 2039, 2040, 2040, 2041, 2041, 2042, 2042, 2043, 2043, 2044, 2044, 2044, 2045, 2045, 2045,
+ 2046, 2046, 2046, 2046, 2047, 2047, 2047, 2047, 2047, 2048, 2048, 2048, 2048, 2048, 2048, 2048,
+ 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2047, 2047, 2047, 2047, 2047, 2046, 2046, 2046, 2046,
+ 2045, 2045, 2045, 2044, 2044, 2044, 2043, 2043, 2042, 2042, 2041, 2041, 2040, 2040, 2039, 2038,
+ 2038, 2037, 2037, 2036, 2035, 2034, 2034, 2033, 2032, 2031, 2031, 2030, 2029, 2028, 2027, 2026,
+ 2025, 2024, 2023, 2022, 2021, 2020, 2019, 2018, 2017, 2016, 2015, 2014, 2013, 2012, 2010, 2009,
+ 2008, 2007, 2005, 2004, 2003, 2002, 2000, 1999, 1997, 1996, 1995, 1993, 1992, 1990, 1989, 1987,
+ 1986, 1984, 1983, 1981, 1979, 1978, 1976, 1975, 1973, 1971, 1969, 1968, 1966, 1964, 1962, 1961,
+ 1959, 1957, 1955, 1953, 1951, 1949, 1947, 1945, 1943, 1941, 1939, 1937, 1935, 1933, 1931, 1929,
+ 1927, 1925, 1923, 1921, 1918, 1916, 1914, 1912, 1909, 1907, 1905, 1902, 1900, 1898, 1895, 1893,
+ 1891, 1888, 1886, 1883, 1881, 1878, 1876, 1873, 1871, 1868, 1866, 1863, 1860, 1858, 1855, 1852,
+ 1850, 1847, 1844, 1841, 1839, 1836, 1833, 1830, 1827, 1825, 1822, 1819, 1816, 1813, 1810, 1807,
+ 1804, 1801, 1798, 1795, 1792, 1789, 1786, 1783, 1780, 1777, 1774, 1770, 1767, 1764, 1761, 1758,
+ 1754, 1751, 1748, 1745, 1741, 1738, 1735, 1731, 1728, 1725, 1721, 1718, 1714, 1711, 1707, 1704,
+ 1700, 1697, 1693, 1690, 1686, 1683, 1679, 1676, 1672, 1668, 1665, 1661, 1657, 1654, 1650, 1646,
+ 1642, 1639, 1635, 1631, 1627, 1623, 1620, 1616, 1612, 1608, 1604, 1600, 1596, 1592, 1588, 1584,
+ 1580, 1576, 1572, 1568, 1564, 1560, 1556, 1552, 1548, 1544, 1540, 1535, 1531, 1527, 1523, 1519,
+ 1514, 1510, 1506, 1502, 1497, 1493, 1489, 1484, 1480, 1476, 1471, 1467, 1463, 1458, 1454, 1449,
+ 1445, 1440, 1436, 1431, 1427, 1422, 1418, 1413, 1409, 1404, 1400, 1395, 1390, 1386, 1381, 1376,
+ 1372, 1367, 1362, 1358, 1353, 1348, 1344, 1339, 1334, 1329, 1324, 1320, 1315, 1310, 1305, 1300,
+ 1295, 1291, 1286, 1281, 1276, 1271, 1266, 1261, 1256, 1251, 1246, 1241, 1236, 1231, 1226, 1221,
+ 1216, 1211, 1206, 1201, 1196, 1191, 1185, 1180, 1175, 1170, 1165, 1160, 1154, 1149, 1144, 1139,
+ 1134, 1128, 1123, 1118, 1113, 1107, 1102, 1097, 1091, 1086, 1081, 1075, 1070, 1065, 1059, 1054,
+ 1048, 1043, 1038, 1032, 1027, 1021, 1016, 1010, 1005, 999, 994, 988, 983, 977, 972, 966,
+ 961, 955, 950, 944, 938, 933, 927, 922, 916, 910, 905, 899, 893, 888, 882, 876,
+ 871, 865, 859, 854, 848, 842, 836, 831, 825, 819, 813, 808, 802, 796, 790, 784,
+ 779, 773, 767, 761, 755, 749, 744, 738, 732, 726, 720, 714, 708, 702, 697, 691,
+ 685, 679, 673, 667, 661, 655, 649, 643, 637, 631, 625, 619, 613, 607, 601, 595,
+ 589, 583, 577, 571, 565, 559, 553, 547, 541, 535, 529, 522, 516, 510, 504, 498,
+ 492, 486, 480, 474, 468, 461, 455, 449, 443, 437, 431, 425, 418, 412, 406, 400,
+ 394, 388, 381, 375, 369, 363, 357, 350, 344, 338, 332, 326, 319, 313, 307, 301,
+ 295, 288, 282, 276, 270, 263, 257, 251, 245, 238, 232, 226, 220, 213, 207, 201,
+ 195, 188, 182, 176, 170, 163, 157, 151, 145, 138, 132, 126, 119, 113, 107, 101,
+ 94, 88, 82, 75, 69, 63, 57, 50, 44, 38, 31, 25, 19, 13, 6, 0,
+};
+
+#endif
\ No newline at end of file