2025-01-13 18:09:02 +01:00
|
|
|
import socket
|
|
|
|
import os
|
2025-01-13 19:50:47 +01:00
|
|
|
import typing
|
2025-01-13 19:53:15 +01:00
|
|
|
import time
|
2025-01-13 19:50:47 +01:00
|
|
|
import numpy as np
|
2025-01-14 11:24:37 +01:00
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
from matplotlib.colors import LinearSegmentedColormap
|
2025-01-13 19:50:47 +01:00
|
|
|
|
|
|
|
|
|
|
|
def parse_speed(text):
|
2025-01-14 23:07:23 +01:00
|
|
|
tl = text.split(b' ')
|
2025-01-14 18:23:55 +01:00
|
|
|
return (2 * int(tl[0]) - 1) * float(tl[1])
|
2025-01-13 17:42:12 +01:00
|
|
|
|
|
|
|
|
|
|
|
class Simulator:
|
2025-01-13 19:50:47 +01:00
|
|
|
# X_STEPS_PER_INCH = 4800
|
|
|
|
X_STEPS_PER_MM: float = 188.97
|
|
|
|
X_MOTOR_STEPS: float = 200
|
|
|
|
|
|
|
|
# Y_STEPS_PER_INCH = 4800
|
|
|
|
Y_STEPS_PER_MM: int = 188.97
|
|
|
|
Y_MOTOR_STEPS: int = 200
|
|
|
|
|
|
|
|
# Z_STEPS_PER_INCH = 4800
|
|
|
|
Z_STEPS_PER_MM: float = 188.97
|
|
|
|
Z_MOTOR_STEPS: int = 200
|
|
|
|
|
2025-01-14 11:24:37 +01:00
|
|
|
pos = np.array([0., 0., 0.])
|
|
|
|
speed = np.array([0., 0., 0.])
|
|
|
|
|
2025-01-13 19:50:47 +01:00
|
|
|
def __init__(self, bind):
|
2025-01-14 11:24:37 +01:00
|
|
|
self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
|
|
|
try:
|
|
|
|
os.remove(bind)
|
|
|
|
except FileNotFoundError:
|
|
|
|
pass
|
|
|
|
self.s.bind(bind)
|
2025-01-14 23:07:23 +01:00
|
|
|
self.s.setblocking(False)
|
2025-01-14 11:24:37 +01:00
|
|
|
self.s.listen()
|
2025-01-13 18:09:02 +01:00
|
|
|
while True:
|
2025-01-14 11:24:37 +01:00
|
|
|
try:
|
2025-01-14 23:07:23 +01:00
|
|
|
self.s, _ = self.s.accept()
|
|
|
|
break
|
|
|
|
except BlockingIOError:
|
|
|
|
print("Waiting for connection")
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
try:
|
|
|
|
self.loop()
|
|
|
|
except KeyboardInterrupt: # Just so everything is fine when quitting
|
|
|
|
self.s.close()
|
|
|
|
plt.ioff()
|
|
|
|
try:
|
|
|
|
os.remove(bind)
|
|
|
|
except FileNotFoundError:
|
|
|
|
pass
|
2025-01-13 18:09:02 +01:00
|
|
|
|
2025-01-13 19:50:47 +01:00
|
|
|
def loop(self):
|
2025-01-14 23:07:23 +01:00
|
|
|
while(True):
|
|
|
|
with socket.SocketIO(self.s, 'r') as buffer:
|
|
|
|
try:
|
|
|
|
time.sleep(1e-5)
|
2025-01-14 23:34:07 +01:00
|
|
|
self.simulate()
|
2025-01-14 23:07:23 +01:00
|
|
|
req = buffer.readline()
|
|
|
|
self.s.setblocking(True)
|
|
|
|
match req:
|
|
|
|
case b'request\n':
|
|
|
|
print("At your service")
|
|
|
|
self.request() # These are the functions that need to be implemented
|
|
|
|
case b'realize\n':
|
|
|
|
print("Contemplating Life")
|
|
|
|
self.realize()
|
|
|
|
case b'':
|
|
|
|
break
|
|
|
|
self.s.setblocking(False)
|
|
|
|
except BlockingIOError:
|
|
|
|
pass
|
|
|
|
except OSError:
|
|
|
|
pass
|
2025-01-13 18:09:02 +01:00
|
|
|
|
2025-01-14 23:34:07 +01:00
|
|
|
def simulate(self):
|
|
|
|
pass
|
|
|
|
|
2025-01-14 11:24:37 +01:00
|
|
|
def realize(self):
|
|
|
|
pass
|
|
|
|
|
|
|
|
def request(self):
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
|
|
class Douche:
|
|
|
|
colors = [(125, 29, 211), (255, 229, 0)]
|
|
|
|
cm = LinearSegmentedColormap.from_list(
|
|
|
|
"Custom", colors, N=42
|
|
|
|
)
|
|
|
|
last_x = None
|
|
|
|
last_y = None
|
|
|
|
|
|
|
|
def __init__(self):
|
|
|
|
# We will assume no points are `really` in 3D.
|
|
|
|
plt.ion()
|
|
|
|
fig = plt.figure()
|
|
|
|
self.ax = fig.gca()
|
|
|
|
self.xs = []
|
|
|
|
self.ys = []
|
2025-01-14 23:07:23 +01:00
|
|
|
self.line, = self.ax.plot(self.xs, self.ys)#, color=self.cm)
|
2025-01-14 11:24:37 +01:00
|
|
|
|
|
|
|
def add_point(self, x, y):
|
2025-01-14 23:34:07 +01:00
|
|
|
print(x, y)
|
2025-01-14 11:24:37 +01:00
|
|
|
self.xs.append(x)
|
|
|
|
self.ys.append(y)
|
|
|
|
self.line.set_data(self.xs, self.ys)
|
|
|
|
plt.draw()
|
2025-01-14 23:34:07 +01:00
|
|
|
plt.show()
|
2025-01-14 11:24:37 +01:00
|
|
|
|
|
|
|
|
|
|
|
class NaiveSimulator(Simulator, Douche):
|
|
|
|
steps = [0, 0, 0]
|
|
|
|
|
|
|
|
time_step = 0
|
|
|
|
last_update = 0.
|
|
|
|
|
2025-01-14 23:34:07 +01:00
|
|
|
J = np.array([1., 1., 1.]) # Moment of inertia vector times 1/R
|
|
|
|
f = np.array([0., 0., 0.]) # viscous friction coefficient
|
|
|
|
G = np.array([1., 1., 1.]) # Motor gain : Gamma = G * delta_w
|
2025-01-14 23:07:23 +01:00
|
|
|
# FIXME: this is not very realistic, this looks like a model of
|
|
|
|
# an asynchronous motor, not a stepper.
|
|
|
|
|
|
|
|
command_spd = [0., 0., 0.]
|
2025-01-14 11:24:37 +01:00
|
|
|
|
|
|
|
def __init__(self, bind):
|
|
|
|
self.last_update = time.time() # Custom Simulation
|
|
|
|
|
|
|
|
Douche.__init__(self) # Prepare to show the simulation
|
|
|
|
self.add_point(self.pos[0], self.pos[1])
|
|
|
|
Simulator.__init__(self, bind) # Simulator loop
|
|
|
|
|
2025-01-14 23:07:23 +01:00
|
|
|
def simulate(self):
|
|
|
|
lt = time.time()
|
|
|
|
self.time_step = lt - self.last_update
|
|
|
|
self.last_update = lt
|
2025-01-14 23:34:07 +01:00
|
|
|
gamma = self.G*(self.command_spd - self.speed)
|
2025-01-14 23:07:23 +01:00
|
|
|
|
2025-01-14 23:34:07 +01:00
|
|
|
self.speed += 1./self.J * (self.G - self.f*self.speed ) * self.time_step
|
2025-01-14 23:07:23 +01:00
|
|
|
self.pos += self.speed * self.time_step
|
|
|
|
|
|
|
|
self.steps = np.ceil(self.pos * np.array([self.X_STEPS_PER_MM,
|
|
|
|
self.Y_STEPS_PER_MM,
|
|
|
|
self.Z_STEPS_PER_MM]))
|
|
|
|
self.add_point(self.pos[0], self.pos[1])
|
|
|
|
|
2025-01-13 19:50:47 +01:00
|
|
|
def request(self):
|
|
|
|
self.s.send(f"{self.steps[0]}".encode())
|
|
|
|
self.s.send(f"{self.steps[1]}".encode())
|
|
|
|
self.s.send(f"{self.steps[2]}".encode())
|
2025-01-13 17:42:12 +01:00
|
|
|
|
2025-01-13 19:50:47 +01:00
|
|
|
def realize(self):
|
2025-01-14 11:24:37 +01:00
|
|
|
print("I JUST REALIZED: MA VIE C'EST DE LA MERDE")
|
2025-01-13 19:50:47 +01:00
|
|
|
x_v = parse_speed(socket.SocketIO(self.s, 'r').readline())
|
|
|
|
y_v = parse_speed(socket.SocketIO(self.s, 'r').readline())
|
|
|
|
z_v = parse_speed(socket.SocketIO(self.s, 'r').readline())
|
2025-01-14 23:07:23 +01:00
|
|
|
print("Everything parsed")
|
|
|
|
self.command_spd = np.array([x_v, y_v, z_v])
|
2025-01-14 18:29:43 +01:00
|
|
|
|
2025-01-13 19:50:47 +01:00
|
|
|
self.s.send("Realized".encode())
|
2025-01-14 23:07:23 +01:00
|
|
|
|
|
|
|
simu = NaiveSimulator("socket.sock")
|