83 lines
2.4 KiB
Python
83 lines
2.4 KiB
Python
import socket
|
|
import os
|
|
import typing
|
|
import time
|
|
import numpy as np
|
|
|
|
from sympy.abc import alpha
|
|
|
|
|
|
def parse_speed(text):
|
|
tl = text.split(' ')
|
|
return int(tl[0]) * float(tl[1])
|
|
|
|
|
|
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_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
|
|
|
|
def __init__(self, bind):
|
|
super().__init__(bind)
|
|
self.last_update = time.time()
|
|
while True:
|
|
self.loop()
|
|
|
|
def loop(self):
|
|
req = socket.SocketIO(self.s, 'r').readline()
|
|
match req:
|
|
case 'request':
|
|
print("At your service")
|
|
self.request()
|
|
case 'realize':
|
|
print("Contemplating Life")
|
|
self.realize()
|
|
print("Job Done")
|
|
|
|
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())
|
|
|
|
def realize(self):
|
|
print("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())
|
|
|
|
inertia = - 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())
|