#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:
parent
bf5b728050
commit
d02e08bf4b
2 changed files with 74 additions and 27 deletions
|
@ -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)
|
||||||
|
|
||||||
|
|
97
simulator.py
97
simulator.py
|
@ -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:
|
||||||
|
try:
|
||||||
self.loop()
|
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]))
|
||||||
|
|
Loading…
Reference in a new issue