diff --git a/GCode_Interpreterdc.py b/GCode_Interpreterdc.py index 342acda..e49bd60 100644 --- a/GCode_Interpreterdc.py +++ b/GCode_Interpreterdc.py @@ -48,11 +48,11 @@ class GCodeToMotors: Z_STEPS_PER_MM = z_units = 188.97 Z_MOTOR_STEPS: int = 200 - FAST_XY_FEEDRATE: float = 1 # in m/mn + FAST_XY_FEEDRATE: float = 60 # in m/mn FAST_Z_FEEDRATE: float = 1 - CURVE_SECTION_INCHES = curve_section = .019685 - CURVE_SECTION_MM: float = .5 + CURVE_SECTION_INCHES = curve_section = 4*.019685 + CURVE_SECTION_MM: float = .5*4 SENSORS_INVERTING: bool = False @@ -181,8 +181,9 @@ class GCodeToMotors: match code: case 0 | 1: self.set_target(fp[0], fp[1], fp[2]) - if has_command('G', instruction) and code == 1: + if has_command('F', instruction) and code == 1: self.feedrate = search_string('F', instruction) + print("set feedrate to", self.feedrate) if self.feedrate == 0: self.feedrate = self.get_max_speed() self.move() @@ -198,16 +199,22 @@ class GCodeToMotors: bY = fp[1] - center[1] if code == 2: # If in fucked up anti-trigonometric direction - angleA = np.atan2(bY, bX) - angleB = np.atan2(aY, aX) + angleA = np.arctan2(bY, bX) + angleB = np.arctan2(aY, aX) else: - angleA = np.atan2(aY, aX) - angleB = np.atan2(bY, bX) + angleA = np.arctan2(aY, aX) + angleB = np.arctan2(bY, bX) if angleB <= angleA: angleB += 2 * np.pi angle = angleB - angleA + if has_command('F', instruction): + self.feedrate = search_string('F', instruction) + print("set feedrate to", self.feedrate) + if self.feedrate == 0: + self.feedrate = self.get_max_speed() + radius = np.linalg.norm([aX, aY]) length = radius * angle steps = int(length / self.curve_section) + 1 diff --git a/controller.py b/controller.py index f3c0b0b..c93098e 100644 --- a/controller.py +++ b/controller.py @@ -52,11 +52,12 @@ class PIDController(Controller): class DummyController(Controller): + last_delta = np.nan + def __init__(self): super().__init__() def __call__(self, gtm, *args, **kwargs): - print("Call Me") feed = gtm.feedrate dx, dy, dz = gtm.delta_units @@ -89,8 +90,16 @@ class DummyController(Controller): # If we are close enough, we're good if(gtm.x_throttle == 0 and gtm.y_throttle == 0 and gtm.z_throttle == 0): + last_delta = np.nan return True + delta = (dx**2 + dy**2 + dz**2)**.5 + if(delta > self.last_delta and self.last_delta < 2.0): + self.last_delta = np.nan + return True + + self.last_delta = delta + print("Expected move (mm/call)", gtm.ctrl_step*feed*1000./60) print("Delta (mm) ", (dx**2 + dy**2 + dz**2)**.5) - return (dx**2+dy**2+dz**2)**.5 <= 1. + return False diff --git a/main.py b/main.py index b3f3625..e0be193 100644 --- a/main.py +++ b/main.py @@ -6,14 +6,12 @@ b = "socket.sock" sh = hw.SimuHardware(b) ctrl = ctl.DummyController() gtm1 = gci.GCodeToMotors(ctrl, sh) -gtm1.instruction_converter("G0 X25.000 Y25.000 F0.5000") -gtm1.instruction_converter("G0 X30.000 Y25.000 F0.5000") -gtm1.instruction_converter("G2 X30.000 Y45.000 I0 J10 F0.5000") +gtm1.instruction_converter("G1 X25.000 Y25.000 F2.000") +gtm1.instruction_converter("G1 X30.000 Y25.000 F2.000") +gtm1.instruction_converter("G3 X30.000 Y45.000 I0 J10 F0.100") # gtm1.instruction_converter("G5 X50.000 Y45.000 I-5 J-5 P10 J5 F0.5000") -gtm1.instruction_converter("G0 X50.000 Y50.000 F0.5000") -gtm1.instruction_converter("G0 X50.000 Y200.000 F0.5000") -gtm1.instruction_converter("G0 X200.000 Y200.000 F0.5000") -gtm1.instruction_converter("G30 X200.000 Y50.000 F0.5000") - -while True: - pass +gtm1.instruction_converter("G1 X50.000 Y50.000 F2.000") +gtm1.instruction_converter("G1 X50.000 Y100.000 F2.000") +gtm1.instruction_converter("G1 X100.000 Y-50.000 F2.000") +gtm1.instruction_converter("G1 X-50.000 Y-100.000 F2.000") +gtm1.instruction_converter("G1 X-100.000 Y50.000 F2.000") diff --git a/simulator.py b/simulator.py index 3d2490e..c81126a 100755 --- a/simulator.py +++ b/simulator.py @@ -116,7 +116,7 @@ class NaiveSimulator(Simulator, Douche): # which can be expressed as # Jv' = (R*Gamma)(motor) - f0*v - J = np.array([1., 1., 1.]) # Moment of inertia vector + J = np.array([.1, .1, .1]) # Moment of inertia vector f = np.array([0., 0., 0.]) # viscous friction coefficient G = np.array([1., 1., 1.]) # Motor gain : Gamma*R = G * delta_v # FIXME: this is not very realistic, this looks like a model of