Skip to content
Snippets Groups Projects
Commit 3b7350d2 authored by Quentin Bolsee's avatar Quentin Bolsee
Browse files

Added code

parents
No related branches found
No related tags found
No related merge requests found
# Urumbu graph editor
## Installation
```
python -m pip install -r requirements.txt
```
## Running
```
python server.py
```
## Frontend
This project is to be used with this frontend:
https://github.com/leomcelroy/software-defined-machines-graphs
import serial
import time
class Node:
def __init__(self, name, input_names=(), output_names=(), input_init=None):
self.name = name
self.n_input = len(input_names)
self.n_output = len(output_names)
# start with default values
self.input_names = input_names
self.input_connections = [None for _ in range(self.n_input)]
self.input_values = [None for _ in range(self.n_input)]
self.output_names = output_names
self.output_connections = [set() for _ in range(self.n_output)]
self.input_lookup = {name: i for i, name in enumerate(input_names)}
self.output_lookup = {name: i for i, name in enumerate(output_names)}
if input_init is not None:
for name, value in input_init.items():
self.input_values[self.input_lookup[name]] = value
def is_input_connected(self, i):
return self.input_connections[i] is not None
def is_output_connected(self, i):
return len(self.output_connections[i]) > 0
def connect_input(self, i, other, other_i):
self.input_connections[i] = (other, other_i)
def disconnect_input(self, i):
self.input_connections[i] = None
def connect_output(self, i, other, other_i):
output_connections = self.output_connections[i] # type: set
output_connections.add((other, other_i))
def disconnect_output(self, i, other, other_i):
output_connections = self.output_connections[i] # type: set
output_connections.remove((other, other_i))
def input(self, i):
if self.input_connections[i] is not None:
# update if we can
other, other_i = self.input_connections[i]
val = other.output(other_i)
if val is not None:
self.input_values[i] = val
return self.input_values[i]
def output(self, i):
raise NotImplementedError()
class UrumbuNode(Node):
def __init__(self, ser, name, input_names=(), output_names=(), input_init=None):
self.ser = ser
super().__init__(name=name, input_names=input_names, output_names=output_names, input_init=input_init)
def close(self):
self.ser.close()
def write(self, c):
self.ser.write(c)
def read(self, n=None):
if n is None:
return self.ser.read()
else:
return self.ser.read(n)
class UrumbuNode10bitOutput(UrumbuNode):
def __init__(self, ser, name, output_name="output"):
super().__init__(ser, name=name, output_names=(output_name,))
def output(self, i):
try:
if i != 0:
return None
self.write(b'?')
buffer = self.read(2)
v = int.from_bytes(buffer, byteorder='little')
return v
except serial.SerialException as e:
return None
class UrumbuStepper(UrumbuNode):
pass
class UrumbuServo(UrumbuNode):
pass
class UrumbuCapa(UrumbuNode10bitOutput):
def __init__(self, ser):
super().__init__(ser, name=f"Capacitor at {ser.port}", output_name="capacitance")
class UrumbuPot(UrumbuNode10bitOutput):
def __init__(self, ser):
super().__init__(ser, name=f"Potentiometer at {ser.port}", output_name="level")
class UrumbuDistance(UrumbuNode10bitOutput):
def __init__(self, ser):
super().__init__(ser, name=f"Dist. sensor at {ser.port}", output_name="distance")
class UrumbuPressure(UrumbuNode10bitOutput):
def __init__(self, ser):
super().__init__(ser, name=f"Press. sensor at {ser.port}", output_name="pressure")
class Graph:
def __init__(self):
self.nodes = {}
def __getitem__(self, item):
return self.nodes[item]
def reset(self):
self.nodes = {}
def add_node(self, node_id, node):
self.nodes[node_id] = node
def delete_node(self, node_id):
if node_id not in self.nodes:
return
node = self.nodes[node_id]
for conn_info in node.input_connections:
if conn_info is None:
continue
other, other_i = conn_info
other.disconnect_output(other_i)
for conn_set in node.output_connections:
for conn_info in conn_set:
if conn_info is None:
continue
other, other_i = conn_info
other.disconnect_input(other_i)
self.nodes.pop(node_id)
def connect(self, node_out_id, node_out_i, node_in_id, node_in_i):
if node_out_id not in self.nodes or node_in_id not in self.nodes:
return
node_out = self.nodes[node_out_id]
node_in = self.nodes[node_in_id]
node_out.connect_output(node_out_i, node_in, node_in_i)
node_in.connect_input(node_in_i, node_out, node_out_i)
def disconnect(self, node_out_id, node_out_i, node_in_id, node_in_i):
if node_out_id not in self.nodes or node_in_id not in self.nodes:
return
node_out = self.nodes[node_out_id]
node_in = self.nodes[node_in_id]
node_out.disconnect_output(node_out_i, node_in, node_in_i)
node_in.disconnect_input(node_in_i)
sig_class = {
"0000": UrumbuNode,
"0001": UrumbuStepper,
"0002": UrumbuServo,
"0003": UrumbuCapa,
"0004": UrumbuPot,
"0005": UrumbuDistance,
"0006": UrumbuPressure
}
def main():
try:
# ser = serial.Serial("COM47", baudrate=115200, timeout=0.1)
# ser = serial.Serial("COM48", baudrate=115200, timeout=0.1)
ser = serial.Serial("COM49", baudrate=115200, timeout=0.1)
node = UrumbuPot(ser)
# for i in range(10)
try:
while True:
t = time.perf_counter()
val = node.output(0)
dt = time.perf_counter() - t
print(f"{val:03d}, {dt:6.5f}")
time.sleep(0.1)
finally:
node.close()
except serial.serialutil.SerialException:
pass
if __name__ == "__main__":
main()
server.py 0 → 100644
import time
import websockets
import json
import asyncio
import logging
from urllib.parse import quote, unquote
import serial
import serial.tools.list_ports
import graph_engine
import urumbu_process
logging.getLogger().setLevel(logging.INFO)
state_nodes = {}
state_ports = set()
state_connections = []
state_websocket = None
state_graph = graph_engine.Graph()
state_ports_ignore = {"COM40", "COM41", "COM4"}
def try_open(port, baudrate=115200):
ser = serial.Serial(port, baudrate=baudrate, timeout=0.5)
ser.write(b"@")
sig_bytes = ser.read(4)
if sig_bytes is None:
return None
sig = sig_bytes.decode("ascii")
if sig in graph_engine.sig_class:
cls = graph_engine.sig_class[sig]
return cls(ser)
async def handler(websocket):
global state_websocket
state_websocket = websocket
await send_current_graph()
async for message in websocket:
try:
msg_obj = json.loads(message)
logging.info(f"Received: {msg_obj}")
action = msg_obj["action"]
if action == "add_connection":
node_out_id, _, node_out_i = msg_obj["from"].split(":")
node_in_id, _, node_in_i = msg_obj["to"].split(":")
node_out_i = int(node_out_i)
node_in_i = int(node_in_i)
await add_connection(node_out_id, node_out_i, node_in_id, node_in_i, sync=False)
elif action == "delete_connection":
node_out_id, _, node_out_i = msg_obj["from"].split(":")
node_in_id, _, node_in_i = msg_obj["to"].split(":")
node_out_i = int(node_out_i)
node_in_i = int(node_in_i)
await delete_connection(node_out_id, node_out_i, node_in_id, node_in_i, sync=False)
# decode_action(msg_obj)
except json.JSONDecodeError:
logging.error(f"Error decoding msg: {message}")
state_websocket = None
async def update_ports():
global state_websocket, state_graph, state_ports_ignore
ports = serial.tools.list_ports.comports()
ports_seen = set()
ports_known = set()
for port in state_ports:
ports_known.add(port)
for port, desc, hwid in sorted(ports):
if port in state_ports_ignore:
continue
ports_seen.add(port)
ports_new = ports_seen - ports_known
ports_delete = ports_known - ports_seen
for port in ports_new:
try:
obj = try_open(port)
except serial.serialutil.SerialException as e:
continue
if obj is None:
state_ports_ignore.add(port)
continue
logging.info(f"Adding: {port}, {type(obj)}")
await add_node(port, obj)
state_ports.add(port)
for port in ports_delete:
obj = state_graph.nodes[port]
obj.close()
state_ports.remove(port)
logging.info(f"Deleting: {port}, {type(obj)}")
await delete_node(port)
async def send_reset_graph():
if state_websocket is None:
return
await state_websocket.send(json.dumps({"action": "reset"}))
async def send_current_graph():
# for good measure
await send_reset_graph()
for node_id, node in state_graph.nodes.items():
await send_add_node(node_id, node)
for node_id, node in state_graph.nodes.items():
for i, conn_info in enumerate(node.input_connections):
if conn_info is None:
continue
other, other_i = conn_info
other_id = None
for other_id_iter, other_iter in state_graph.nodes.items():
if other == other_iter:
other_id = other_id_iter
break
if other_id is None:
continue
await send_add_connection(other_id, other_i, node_id, i)
async def add_node(node_id, node, sync=True):
state_graph.add_node(node_id, node)
if sync and state_websocket is not None:
await send_add_node(node_id, node)
async def send_add_node(node_id, node):
msg = {
"action": "add_node",
"id": node_id,
"name": node.name,
"inputs": node.input_names,
"outputs": node.output_names
}
await state_websocket.send(json.dumps(msg))
async def delete_node(node_id, sync=True):
state_graph.delete_node(node_id)
if sync and state_websocket is not None:
await send_delete_node(node_id)
async def send_delete_node(node_id):
msg = {
"action": "delete_node",
"id": node_id,
}
await state_websocket.send(json.dumps(msg))
async def add_connection(node_out_id, node_out_i, node_in_id, node_in_i, sync=True):
state_graph.connect(node_out_id, node_out_i, node_in_id, node_in_i)
if sync and state_websocket is not None:
await send_add_connection(node_out_id, node_out_i, node_in_id, node_in_i)
async def send_add_connection(node_out_id, node_out_i, node_in_id, node_in_i):
msg = {
"action": "add_connection",
"from": f"{node_out_id}:out:{node_out_i}",
"to": f"{node_in_id}:in:{node_in_i}",
}
await state_websocket.send(json.dumps(msg))
async def delete_connection(node_out_id, node_out_i, node_in_id, node_in_i, sync=True):
state_graph.disconnect(node_out_id, node_out_i, node_in_id, node_in_i)
if sync and state_websocket is not None:
await send_delete_connection(node_out_id, node_out_i, node_in_id, node_in_i)
async def send_delete_connection(node_out_id, node_out_i, node_in_id, node_in_i):
msg = {
"action": "delete_connection",
"from": f"{node_out_id}:out:{node_out_i}",
"to": f"{node_in_id}:in:{node_in_i}",
}
await state_websocket.send(json.dumps(msg))
class MathNode(graph_engine.Node):
def __init__(self, name, func, name_output):
self.func = func
super().__init__(name, ("input", ), (name_output, ), input_init={"input": 0})
def output(self, i):
if i != 0:
return None
return self.func(self.input(0))
async def loop_ports():
while True:
await update_ports()
await asyncio.sleep(0.5)
class MachineNode(graph_engine.Node):
def __init__(self, name, input_names=(), input_init=None):
super().__init__(name, input_names, input_init=input_init)
async def loop_machine():
m = MachineNode("Machine", input_names=("x", "y", "z"), input_init={"x": 0, "y": 0, "z": 0})
state_graph.add_node("urumbu", m)
modules_config = {
"a": {
"type": "stepper",
"port": "COM40",
"baudrate": 115200,
"axis": 0,
"steps_per_unit": 8 * 200 / 32, # 32 mm / turn
"reverse": True
},
"b": {
"type": "stepper",
"port": "COM4",
"baudrate": 115200,
"axis": 1,
"steps_per_unit": 8 * 200 / 32, # 32 mm / turn
"reverse": True
},
"x": {
"type": "stepper",
"port": "COM41",
"baudrate": 115200,
"axis": 2,
"steps_per_unit": 8 * 200 / 32, # 32 mm / turn
"reverse": False
},
}
pos_array, pos_lock = urumbu_process.init_machine(modules_config, urumbu_process.transform_corexy)
while True:
x = 75 * m.input(0) / 1023
y = 75 * m.input(1) / 1023
z = 75 * m.input(2) / 1023
pos_lock.acquire()
pos_array[0] = x
pos_array[1] = y
pos_array[2] = z
pos_lock.release()
await asyncio.sleep(0.05)
async def launch_server():
async with websockets.serve(handler, 'localhost', 4000):
await asyncio.Future() # run forever
if __name__ == "__main__":
loop = asyncio.get_event_loop()
loop.create_task(loop_machine())
loop.create_task(launch_server())
loop.create_task(loop_ports())
loop.run_forever()
import re
import serial
import time
import multiprocessing
import logging
import argparse
import math
import os
import numpy as np
import graph_engine
BAUDRATE_DEFAULT = 115200
class Module:
def __init__(self, port, baudrate=BAUDRATE_DEFAULT):
self.port = None
self.baudrate = baudrate
try:
self.port = serial.Serial(port, baudrate)
except serial.SerialException:
logging.error(f"Cannot connect to {port}")
@property
def connected(self):
return self.port is not None
def write(self, txt):
self.port.write(txt)
def close(self):
self.port.close()
def pressed(self, nc=True):
self.write(b"?")
r = self.port.read(1)
if nc:
return r == b"1"
else:
return r == b"0"
class Stepper(Module):
def __init__(self, steps_per_unit, port, baudrate=BAUDRATE_DEFAULT, reverse=False):
super().__init__(port, baudrate)
self.steps = 0
self.reverse = reverse
self.steps_per_unit = steps_per_unit
def step(self, forward):
self.steps += 1 if forward else -1
if self.reverse:
forward = not forward
self.write(b"f" if forward else b"r")
class Servo(Module):
def __init__(self, pulse_min, pulse_max, port, baudrate=BAUDRATE_DEFAULT):
self.pulse_min = pulse_min
self.pulse_max = pulse_max
super().__init__(port, baudrate)
self.delay_us = 0
def pulse(self, delay_us):
self.write(delay_us.to_bytes(2, byteorder='little'))
def fraction(self, f):
p = int(self.pulse_min + (self.pulse_max - self.pulse_min) * f)
self.pulse(p)
def pressed(self, nc=True):
self.pulse(65535)
r = self.port.read(1)
if nc:
return r == b"1"
else:
return r == b"0"
class RGB(Module):
def __init__(self, port, baudrate=BAUDRATE_DEFAULT):
super().__init__(port, baudrate)
def write_rgb(self, red, green, blue):
rgb = (blue << 16) | (green << 8) | red
self.write(rgb.to_bytes(3, byteorder="little"))
class Spindle(Module):
def __init__(self, port, baudrate=BAUDRATE_DEFAULT):
super().__init__(port, baudrate)
def set_rpm(self, rpm):
pass
def set_on(self, on):
if on:
self.write(b"1")
else:
self.write(b"0")
class Action:
def __iter__(self):
return [self].__iter__()
class HomingAction(Action):
def __init__(self, axis, name, pos, feedrate, nc=True):
self.axis = axis
self.name = name
self.pos = np.array(pos)
self.feedrate = feedrate
self.nc = nc
def transform_corexy(pos, pos_transform):
pos_transform[:] = pos[:]
pos_transform[0] = (pos[0] + pos[1])/2
pos_transform[1] = (pos[0] - pos[1])/2
def modules_manager(modules_config, target_pos, pos_lock, pos_transformer=None):
logging.info("start loop")
modules = {}
modules_axis = {}
for name, config in modules_config.items():
if config["type"] == "stepper":
obj = Stepper(config["steps_per_unit"],
config["port"],
config["baudrate"],
reverse=config.get("reverse", False))
modules[name] = obj
if "axis" in config:
modules_axis[config["axis"]] = obj
elif config["type"] == "servo":
modules[name] = Servo(config["pulse_min"],
config["pulse_max"],
config["port"],
config["baudrate"])
elif config["type"] == "rgb":
modules[name] = RGB(config["port"], config["baudrate"])
elif config["type"] == "spindle":
modules[name] = Spindle(config["port"], config["baudrate"])
class MotorTicker:
def __init__(self, n_axis, max_speed, max_acc):
self.n_axis = n_axis
self.motors_steps = np.zeros((self.n_axis,))
self.min_speed = 0.2
self.max_speed = max_speed
self.max_acc = max_acc
self.state_pos = np.zeros((self.n_axis,))
self.state_speed = np.zeros((self.n_axis,))
# self.count = 0
# self.count2 = 0
# self.count_max = 50
def tick_motor(self, dt):
if dt == 0:
return
# self.count += 1
pos_lock.acquire()
target_pos_np = np.array(target_pos)
pos_lock.release()
diff = target_pos_np - self.state_pos
for i in range(self.n_axis):
dist = abs(diff[i])
norm_speed = abs(self.state_speed[i])
if norm_speed < 1e-6:
norm_speed = 0
dist_brake = (norm_speed * norm_speed) / (2*self.max_acc)
if dist < 1e-6 or dist < norm_speed * dt:
# reached destination right now
self.state_pos[i] = target_pos_np[i]
self.state_speed[i] = 0
else:
# print(self.state_speed, self.state_pos)
vec = np.sign(diff)[i]
if (vec * self.state_speed[i]) < 0:
# flip to the right direction
# print("flip")
self.state_speed[i] += vec * self.max_acc * dt
else:
if dist < dist_brake:
# decelerate
# if self.count == self.count_max:
# print(f"decel {self.state_speed[i]} {self.count2:05d}")
if norm_speed != 0:
if norm_speed <= self.max_acc * dt:
# minimal speed reached right now
self.state_speed[i] = 0
else:
self.state_speed[i] -= vec * self.max_acc * dt
else:
if norm_speed < self.max_speed:
# accelerate
# if self.count == self.count_max:
# print(f"accel {self.state_speed[i]} {self.count2:05d}")
if self.max_speed - norm_speed <= self.max_acc * dt:
# max speed reached right now
self.state_speed[i] = vec * self.max_speed
else:
self.state_speed[i] += vec * self.max_acc * dt
self.state_pos[i] += self.state_speed[i] * dt
# tick those motors!
if pos_transformer is None:
self.motors_steps[:] = self.state_pos[:]
else:
pos_transformer(self.state_pos[:], self.motors_steps)
for j in range(self.n_axis):
m = modules_axis[j]
s = int(self.motors_steps[j] * m.steps_per_unit)
if m.steps < s:
m.step(True)
elif m.steps > s:
m.step(False)
m = MotorTicker(len(modules_axis), 65, 350)
dt = 0
while True:
t = time.perf_counter()
m.tick_motor(dt)
dt = time.perf_counter() - t
def init_machine(modules_config, pos_transform=None):
multiprocessing.set_start_method('spawn')
n = len(modules_config)
pos_array = multiprocessing.Array("f", [0 for i in range(n)])
pos_lock = multiprocessing.Lock()
p1 = multiprocessing.Process(target=modules_manager, args=(modules_config, pos_array, pos_lock, pos_transform))
p1.start()
return pos_array, pos_lock
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment