78 lines
2.9 KiB
Python
78 lines
2.9 KiB
Python
import socket
|
|
|
|
import GCode_Interpreterdc
|
|
from controller import DummyController
|
|
|
|
|
|
class Hardware:
|
|
def __init__(self, bind):
|
|
self.s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
|
self.s.connect(bind)
|
|
|
|
def probe(self, gtm):
|
|
return
|
|
|
|
def realize(self, gtm):
|
|
return
|
|
|
|
|
|
class SimuHardware(Hardware):
|
|
def __init__(self, bind):
|
|
Hardware.__init__(self, bind)
|
|
|
|
def probe(self, gtm):
|
|
self.s.send("request\n".encode())
|
|
encoder_x = int(socket.SocketIO(self.s, "rw").readline())
|
|
encoder_y = int(socket.SocketIO(self.s, "rw").readline())
|
|
encoder_z = int(socket.SocketIO(self.s, "rw").readline())
|
|
|
|
last_x, last_y, last_z = gtm.current_steps
|
|
# We are now at some position which realizes the encoder positions
|
|
# curr_x = encoder_x + k*X_MOTOR_STEPS
|
|
# |curr_x - last_x| <= 1/2 * X_MOTOR_STEPS
|
|
curr_x = encoder_x + (last_x // gtm.X_MOTOR_STEPS) * gtm.X_MOTOR_STEPS
|
|
if curr_x - last_x > .5 * gtm.X_MOTOR_STEPS:
|
|
print("GOING BACKWARD, would have ", curr_x)
|
|
curr_x = curr_x - gtm.X_MOTOR_STEPS
|
|
elif curr_x - last_x < -.5*gtm.X_MOTOR_STEPS:
|
|
curr_x += gtm.X_MOTOR_STEPS
|
|
|
|
curr_y = encoder_y + (last_y // gtm.Y_MOTOR_STEPS) * gtm.Y_MOTOR_STEPS
|
|
if curr_y - last_y > .5 * gtm.Y_MOTOR_STEPS:
|
|
curr_y -= gtm.Y_MOTOR_STEPS
|
|
elif curr_y - last_y < -.5*gtm.Y_MOTOR_STEPS:
|
|
curr_y += gtm.Y_MOTOR_STEPS
|
|
|
|
curr_z = encoder_z + (last_z // gtm.Z_MOTOR_STEPS) * gtm.Z_MOTOR_STEPS
|
|
if curr_z - last_z > .5 * gtm.Z_MOTOR_STEPS:
|
|
curr_z -= gtm.Z_MOTOR_STEPS
|
|
elif curr_z - last_z < -.5*gtm.Z_MOTOR_STEPS:
|
|
curr_z += gtm.Z_MOTOR_STEPS
|
|
|
|
print("Last steps : ", gtm.current_steps)
|
|
print("Encoders : ", encoder_x, encoder_y, encoder_z)
|
|
print("Current steps : ", curr_x, curr_y, curr_z)
|
|
|
|
# Note that if we don't probe regularly enough, we lose track of the position
|
|
# Really, we should be able to set_steps
|
|
gtm.set_position(curr_x / gtm.X_STEPS_PER_MM,
|
|
curr_y / gtm.Y_STEPS_PER_MM,
|
|
curr_z / gtm.Z_STEPS_PER_MM)
|
|
|
|
def realize(self, gtm):
|
|
self.s.send("realize\n".encode())
|
|
|
|
if gtm.x_throttle == 0.:
|
|
self.s.send(str(gtm.x_direction).encode() + b" 0\n")
|
|
else:
|
|
self.s.send(str(gtm.x_direction).encode() + b" " + str(gtm.FAST_XY_FEEDRATE / gtm.x_throttle).encode() + b"\n")
|
|
|
|
if gtm.y_throttle == 0.:
|
|
self.s.send(str(gtm.y_direction).encode() + b" 0\n")
|
|
else:
|
|
self.s.send(str(gtm.y_direction).encode() + b" " + str(gtm.FAST_XY_FEEDRATE / gtm.y_throttle).encode() + b"\n")
|
|
|
|
if gtm.z_throttle == 0.:
|
|
self.s.send(str(gtm.z_direction).encode() + b" 0\n")
|
|
else:
|
|
self.s.send(str(gtm.z_direction).encode() + b" " + str(gtm.FAST_Z_FEEDRATE / gtm.z_throttle).encode() + b"\n")
|