diff --git a/axi/__init__.py b/axi/__init__.py index 46e5950..f737287 100644 --- a/axi/__init__.py +++ b/axi/__init__.py @@ -1 +1,3 @@ +from device import Device + from planner import Planner diff --git a/axi/device.py b/axi/device.py new file mode 100644 index 0000000..688bb2f --- /dev/null +++ b/axi/device.py @@ -0,0 +1,74 @@ +from math import modf +from serial import Serial +from serial.tools.list_ports import comports +import time + +STEPS_PER_INCH = 2032 +STEPS_PER_MM = 80 + +VID_PID = '04D8:FD92' + +def find_port(): + for port in comports(): + if VID_PID in port[2]: + return port[0] + return None + +class Device(object): + def __init__(self, steps_per_unit=STEPS_PER_INCH): + port = find_port() + if port is None: + raise Exception('cannot find axidraw device') + self.serial = Serial(port, timeout=1) + self.steps_per_unit = steps_per_unit + + def readline(self): + return self.serial.readline().strip() + + def command(self, *args): + line = ','.join(map(str, args)) + self.serial.write(line + '\r') + return self.readline() + + def version(self): + return self.command('V') + + # motor functions + def enable_motors(self): + return self.command('EM', 1, 1) + + def disable_motors(self): + return self.command('EM', 0, 0) + + def motor_status(self): + return self.command('QM') + + def move(self, duration, a, b): + return self.command('XM', duration, a, b) + + def wait(self): + while '1' in self.motor_status(): + time.sleep(0.1) + + def run_plan(self, plan): + step_ms = 30 + step_s = step_ms / 1000.0 + t = 0 + ex = 0 + ey = 0 + while t < plan.t: + i1 = plan.instant(t) + i2 = plan.instant(t + step_s) + d = i2.p.sub(i1.p) + ex, sx = modf(d.x * self.steps_per_unit + ex) + ey, sy = modf(d.y * self.steps_per_unit + ey) + self.move(step_ms, int(sx), int(sy)) + t += step_s + self.wait() + + # pen functions + def pen_up(self, delay=0): + return self.command('SP', 1, delay) + + def pen_down(self, delay=0): + return self.command('SP', 0, delay) diff --git a/device_test.py b/device_test.py new file mode 100644 index 0000000..b1d04d9 --- /dev/null +++ b/device_test.py @@ -0,0 +1,32 @@ +from axi import Device, Planner +from math import sin, cos, pi +import time + +def circle(cx, cy, r, n): + points = [] + for i in range(n + 1): + a = 2 * pi * i / n + x = cx + cos(a) * r + y = cy + sin(a) * r + points.append((x, y)) + return points + +def main(): + planner = Planner(acceleration=10, max_velocity=5, corner_factor=0.01) + path = [] + path.append((0, 0)) + for i in range(10): + path.extend(circle(4, 4, (i + 1) * 0.2, 72)) + path.append((0, 0)) + plan = planner.plan(path) + + d = Device() + d.pen_up() + d.enable_motors() + time.sleep(0.2) + d.run_plan(plan) + d.wait() + d.disable_motors() + +if __name__ == '__main__': + main()