Make simulator event loop run!
However, no pyplot rn :'(
This commit is contained in:
parent
5edd9d665e
commit
500e281f2f
3 changed files with 68 additions and 36 deletions
|
|
@ -66,7 +66,7 @@ class GCodeToMotors:
|
|||
z_throttle : int = 0
|
||||
|
||||
feedrate: float = 0. # In m/mn
|
||||
ctrl_step: float = 1e-5 # in s
|
||||
ctrl_step: float = 1e-5 # in s
|
||||
|
||||
abs_mode: bool = False
|
||||
|
||||
|
|
@ -133,7 +133,8 @@ class GCodeToMotors:
|
|||
def move(self):
|
||||
self.HARDWARE.probe(self)
|
||||
while not self.CONTROLLER(self): # Allow controller to alter self
|
||||
self.HARDWARE.realize(self)
|
||||
self.HARDWARE.realize(self)
|
||||
time.sleep(self.ctrl_step)
|
||||
|
||||
self.calculate_deltas()
|
||||
|
||||
|
|
|
|||
|
|
@ -77,4 +77,4 @@ class DummyController(Controller):
|
|||
gtm.throttle_z = 0 if gtm.throttle_z >= 65536 else gtm.throttle_z
|
||||
|
||||
# If we are close enough, we're good
|
||||
return (gtm.ctrl_step*(feed*1000./60) >= 2.*(dx**2+dy**2+dz**2)**.5):
|
||||
return (gtm.ctrl_step*(feed*1000./60) >= 3.*(dx**2+dy**2+dz**2)**.5):
|
||||
|
|
|
|||
97
simulator.py
Normal file → Executable file
97
simulator.py
Normal file → Executable file
|
|
@ -8,7 +8,7 @@ from matplotlib.colors import LinearSegmentedColormap
|
|||
|
||||
|
||||
def parse_speed(text):
|
||||
tl = text.split(' ')
|
||||
tl = text.split(b' ')
|
||||
return (2 * int(tl[0]) - 1) * float(tl[1])
|
||||
|
||||
|
||||
|
|
@ -35,28 +35,47 @@ class Simulator:
|
|||
except FileNotFoundError:
|
||||
pass
|
||||
self.s.bind(bind)
|
||||
self.s.setblocking(False)
|
||||
self.s.listen()
|
||||
while True:
|
||||
try:
|
||||
self.loop()
|
||||
except KeyboardInterrupt: # Just so everything is fine when quitting
|
||||
self.s.close()
|
||||
plt.ioff()
|
||||
try:
|
||||
os.remove(bind)
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
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
|
||||
|
||||
def loop(self):
|
||||
req = socket.SocketIO(self.s, 'r').readline()
|
||||
match req:
|
||||
case 'request':
|
||||
print("At your service")
|
||||
self.request() # These are the functions that need to be implemented
|
||||
case 'realize':
|
||||
print("Contemplating Life")
|
||||
self.realize()
|
||||
print("Job Done")
|
||||
while(True):
|
||||
with socket.SocketIO(self.s, 'r') as buffer:
|
||||
try:
|
||||
time.sleep(1e-5)
|
||||
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
|
||||
|
||||
def realize(self):
|
||||
pass
|
||||
|
|
@ -80,7 +99,8 @@ class Douche:
|
|||
self.ax = fig.gca()
|
||||
self.xs = []
|
||||
self.ys = []
|
||||
self.line, = self.ax.plot(self.xs, self.ys, cmap=self.cm)
|
||||
self.line, = self.ax.plot(self.xs, self.ys)#, color=self.cm)
|
||||
plt.show()
|
||||
|
||||
def add_point(self, x, y):
|
||||
self.xs.append(x)
|
||||
|
|
@ -95,7 +115,13 @@ class NaiveSimulator(Simulator, Douche):
|
|||
time_step = 0
|
||||
last_update = 0.
|
||||
|
||||
alpha = 1.
|
||||
J = [1., 1., 1.] # Moment of inertia vector times 1/R
|
||||
f = [0., 0., 0.] # viscous friction coefficient
|
||||
G = [1., 1., 1.] # Motor gain : Gamma = G * delta_w
|
||||
# FIXME: this is not very realistic, this looks like a model of
|
||||
# an asynchronous motor, not a stepper.
|
||||
|
||||
command_spd = [0., 0., 0.]
|
||||
|
||||
def __init__(self, bind):
|
||||
self.last_update = time.time() # Custom Simulation
|
||||
|
|
@ -104,28 +130,33 @@ class NaiveSimulator(Simulator, Douche):
|
|||
self.add_point(self.pos[0], self.pos[1])
|
||||
Simulator.__init__(self, bind) # Simulator loop
|
||||
|
||||
def simulate(self):
|
||||
lt = time.time()
|
||||
self.time_step = lt - self.last_update
|
||||
self.last_update = lt
|
||||
gamma = G*(command_spd - spd)
|
||||
|
||||
self.speed += 1./J * (G - f*self.speed ) * self.time_step
|
||||
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])
|
||||
|
||||
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())
|
||||
|
||||
self.add_point(self.pos[0], self.pos[1])
|
||||
|
||||
|
||||
def realize(self):
|
||||
print("I JUST REALIZED: MA VIE C'EST DE LA MERDE")
|
||||
lt = time.time()
|
||||
self.time_step = lt - self.last_update
|
||||
self.last_update = lt
|
||||
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())
|
||||
self.speed = np.array([x_v, y_v, z_v])
|
||||
print("Everything parsed")
|
||||
self.command_spd = np.array([x_v, y_v, z_v])
|
||||
|
||||
inertia = - self.alpha * self.speed
|
||||
gamma_command = np.array([x_v, y_v, z_v]) - self.speed
|
||||
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]))
|
||||
self.steps = self.steps + step_increment
|
||||
self.pos = self.pos + np.array([self.X_STEPS_PER_MM * step_increment[0], self.Y_STEPS_PER_MM * step_increment[1], self.Z_STEPS_PER_MM * step_increment[2]])
|
||||
self.s.send("Realized".encode())
|
||||
|
||||
simu = NaiveSimulator("socket.sock")
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue