diff --git a/GCode_Interpreterdc.py b/GCode_Interpreterdc.py index ccba089..dd71097 100644 --- a/GCode_Interpreterdc.py +++ b/GCode_Interpreterdc.py @@ -259,6 +259,92 @@ class GCodeToMotors: self.feedrate_micros = self.get_max_speed() self.move(self.feedrate_micros) + case 2, 3: + center = [0., 0., 0.] + center[0] = search_string('I', instruction) + self.current_units[0] + center[1] = search_string('J', instruction) + self.current_units[1] + + aX = self.current_units[0] - center[0] + aY = self.current_units[1] - center[1] + bX = fp[0] - center[0] + 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) + else: + angleA = np.atan2(aY, aX) + angleB = np.atan2(bY, bX) + + if angleB <= angleA: + angleB += 2 * np.pi + angle = angleB - angleA + + radius = np.linalg.norm([aX, aY]) + length = radius * angle + steps = np.ceil(length/self.curve_section) + + newPoint = [0., 0., 0.] + for step in range(1, steps + 1): + step = step if (code == 3) else steps - step + newPoint[0] = center[0] + radius * np.cos(angleA + angle * (step / steps)) + newPoint[1] = center[1] + radius * np.sin(angleA + angle * (step / steps)) + self.set_position(newPoint[0], newPoint[1], fp[2]) + + if self.feedrate > 0: + self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate) + else: + self.feedrate_micros = self.get_max_speed() + + self.move(self.feedrate_micros) + + case 4: + time.sleep(search_string('P', instruction) * 1e-3) + + case 20: + self.x_units = self.X_STEPS_PER_INCH + self.y_units = self.Y_STEPS_PER_INCH + self.z_units = self.Z_STEPS_PER_INCH + self.curve_section = self.CURVE_SECTION_INCHES + + self.calculate_deltas() + + case 21: + self.x_units = self.X_STEPS_PER_MM + self.y_units = self.Y_STEPS_PER_MM + self.z_units = self.Z_STEPS_PER_MM + self.curve_section = self.CURVE_SECTION_MM + + self.calculate_deltas() + + case 28: + self.set_target(0., 0., 0.) + self.move(self.get_max_speed()) + + case 30: + fp = [0., 0., 0.] + fp[0] = search_string('X', instruction) + fp[1] = search_string('Y', instruction) + fp[2] = search_string('Z', instruction) + + if self.abs_mode: + if not has_command('X', instruction): + fp[0] = self.current_units[0] + if not has_command('Y', instruction): + fp[1] = self.current_units[1] + if not has_command('Z', instruction): + fp[2] = self.current_units[2] + + self.set_target(fp[0], fp[1], fp[2]) + else: + self.set_target(self.current_units[0] + fp[0], self.current_units[1] + fp[1], self.current_units[2] + fp[2]) + + self.move(self.get_max_speed()) + self.set_target(0., 0., 0.) + self.move(self.get_max_speed()) + + + return def execute(self, gcode):