#derive show

- add class to allow for concurrent plotting of simulated position
- switch position for loop function since it will be the same for all
simulators
This commit is contained in:
mpboyer 2025-01-14 11:24:37 +01:00
parent bf5b728050
commit d02e08bf4b
2 changed files with 74 additions and 27 deletions

View file

@ -17,7 +17,7 @@ class Hardware:
class SimuHardware(Hardware): class SimuHardware(Hardware):
def __init__(self, bind): def __init__(self, bind):
super().__init__() Hardware.__init__(self)
self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
self.s.connect(bind) self.s.connect(bind)

View file

@ -3,8 +3,8 @@ import os
import typing import typing
import time import time
import numpy as np import numpy as np
import matplotlib.pyplot as plt
from sympy.abc import alpha from matplotlib.colors import LinearSegmentedColormap
def parse_speed(text): def parse_speed(text):
@ -13,24 +13,6 @@ def parse_speed(text):
class Simulator: class Simulator:
def __init__(self, bind):
self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
try:
os.remove(bind)
except FileNotFoundError:
pass
self.s.bind(bind)
self.s.listen()
class NaiveSimulator(Simulator):
steps = [0, 0, 0]
pos = np.array([0., 0., 0.])
speed = np.array([0., 0., 0.])
time_step = 0
last_update = 0.
# X_STEPS_PER_INCH = 4800 # X_STEPS_PER_INCH = 4800
X_STEPS_PER_MM: float = 188.97 X_STEPS_PER_MM: float = 188.97
X_MOTOR_STEPS: float = 200 X_MOTOR_STEPS: float = 200
@ -43,30 +25,95 @@ class NaiveSimulator(Simulator):
Z_STEPS_PER_MM: float = 188.97 Z_STEPS_PER_MM: float = 188.97
Z_MOTOR_STEPS: int = 200 Z_MOTOR_STEPS: int = 200
pos = np.array([0., 0., 0.])
speed = np.array([0., 0., 0.])
def __init__(self, bind): def __init__(self, bind):
super().__init__(bind) self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
self.last_update = time.time() try:
os.remove(bind)
except FileNotFoundError:
pass
self.s.bind(bind)
self.s.listen()
while True: while True:
self.loop() try:
self.loop()
except KeyboardInterrupt: # Just so everything is fine when quitting
self.s.close()
plt.ioff()
try:
os.remove(bind)
except FileNotFoundError:
pass
def loop(self): def loop(self):
req = socket.SocketIO(self.s, 'r').readline() req = socket.SocketIO(self.s, 'r').readline()
match req: match req:
case 'request': case 'request':
print("At your service") print("At your service")
self.request() self.request() # These are the functions that need to be implemented
case 'realize': case 'realize':
print("Contemplating Life") print("Contemplating Life")
self.realize() self.realize()
print("Job Done") print("Job Done")
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 = []
self.line, = self.ax.plot(self.xs, self.ys, cmap=self.cm)
def add_point(self, x, y):
self.xs.append(x)
self.ys.append(y)
self.line.set_data(self.xs, self.ys)
plt.draw()
class NaiveSimulator(Simulator, Douche):
steps = [0, 0, 0]
time_step = 0
last_update = 0.
alpha = 1.
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
def request(self): def request(self):
self.s.send(f"{self.steps[0]}".encode()) self.s.send(f"{self.steps[0]}".encode())
self.s.send(f"{self.steps[1]}".encode()) self.s.send(f"{self.steps[1]}".encode())
self.s.send(f"{self.steps[2]}".encode()) self.s.send(f"{self.steps[2]}".encode())
self.add_point(self.pos[0], self.pos[1])
def realize(self): def realize(self):
print("MA VIE C'EST DE LA MERDE") print("I JUST REALIZED: MA VIE C'EST DE LA MERDE")
lt = time.time() lt = time.time()
self.time_step = lt - self.last_update self.time_step = lt - self.last_update
self.last_update = lt self.last_update = lt
@ -74,7 +121,7 @@ class NaiveSimulator(Simulator):
y_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()) z_v = parse_speed(socket.SocketIO(self.s, 'r').readline())
inertia = - alpha * self.speed inertia = - self.alpha * self.speed
gamma_command = np.array([x_v, y_v, z_v]) - self.speed gamma_command = np.array([x_v, y_v, z_v]) - self.speed
self.speed = self.time_step * (inertia + gamma_command) self.speed = self.time_step * (inertia + gamma_command)
step_increment = np.ceil(self.speed * self.time_step * np.array([1 / self.X_MOTOR_STEPS, 1 / self.Y_MOTOR_STEPS, 1 / self.Z_MOTOR_STEPS])) step_increment = np.ceil(self.speed * self.time_step * np.array([1 / self.X_MOTOR_STEPS, 1 / self.Y_MOTOR_STEPS, 1 / self.Z_MOTOR_STEPS]))