Static freq optional graphs (#112)

shaketune/measurement/resonance_test.py

@@ -18,7 +18,7 @@ from ..helpers.console_output import ConsoleOutput
 # to test the resonance frequency of the printer and its components
 def vibrate_axis(toolhead, gcode, axis_direction, min_freq, max_freq, hz_per_sec, accel_per_hz):
     freq = min_freq
-    X, Y, Z, E = toolhead.get_position()  # Get current position
+    X, Y, Z, E = toolhead.get_position()
     sign = 1.0
 
     while freq <= max_freq + 0.000001:
@@ -48,3 +48,33 @@ def vibrate_axis(toolhead, gcode, axis_direction, min_freq, max_freq, hz_per_sec
             ConsoleOutput.print(f'Testing frequency: {freq:.0f} Hz')
 
     toolhead.wait_moves()
+
+
+# This function is used to vibrate the toolhead in a specific axis direction at a static frequency for a specific duration
+def vibrate_axis_at_static_freq(toolhead, gcode, axis_direction, freq, duration, accel_per_hz):
+    X, Y, Z, E = toolhead.get_position()
+    sign = 1.0
+
+    # Compute movements values
+    t_seg = 0.25 / freq
+    accel = accel_per_hz * freq
+    max_v = accel * t_seg
+    toolhead.cmd_M204(gcode.create_gcode_command('M204', 'M204', {'S': accel}))
+    L = 0.5 * accel * t_seg**2
+
+    # Calculate move points based on axis direction (X, Y and Z)
+    magnitude = math.sqrt(sum([component**2 for component in axis_direction]))
+    normalized_direction = tuple(component / magnitude for component in axis_direction)
+    dX, dY, dZ = normalized_direction[0] * L, normalized_direction[1] * L, normalized_direction[2] * L
+
+    # Start a timer to measure the duration of the test and execute the vibration within the specified time
+    start_time = toolhead.reactor.monotonic()
+    while toolhead.reactor.monotonic() - start_time < duration:
+        nX = X + sign * dX
+        nY = Y + sign * dY
+        nZ = Z + sign * dZ
+        toolhead.move([nX, nY, nZ, E], max_v)
+        toolhead.move([X, Y, Z, E], max_v)
+        sign *= -1
+
+    toolhead.wait_moves()