#!/usr/bin/env python
"""\
g_code_osc_fn.py : demonstration of creating an oscillatory motion represented as a G-code using a simple function

This is a fully self-contained script, i.e. it does not require any special libraries.

This emits a NC program with speeds faster than the grbl defaults.  Please issue
the following commands using a terminal emulator to set the non-volatile
registers:

  $110=2000
  $120=500

Afterward, the $$ command should show these entries:
  $110=2000.000 (x max rate, mm/min)
  $120=500.000 (x accel, mm/sec^2)

"""

# Redefine the print function to be compatible with Python 3.  This is a good
# practice even if we are using Python 2.
from __future__ import print_function

# Import the standard math functions into the current namespace.
import math

#================================================================
# Define a few useful functions to emit lines of G-code.

[docs]def emit_prelude(output):
    """Emit a header of G-code commands to reset the machine modes to a known
    standard state, including the following:

    G21   units are mm
    G90   absolute distance mode
    G17   select XY plane for arcs
    G94   set units/mm feed rate mode
    G54   use default work coordinates
    """
    
    output.write("G21 G90 G17 G94 G54\n")
    return

[docs]def emit_rapid_move(output, x_position):
    """Emit a single-axis rapid move to the given location at the maximum speed. The
    position precision is limited to three decimal places.

    :param x_position: target position in mm

    """
    output.write("G0 X%.3f\n" % x_position)
    return

[docs]def emit_move(output, x_position, speed):
    """Emit a single-axis move to the given location at the given speed.  The
    precision is limited to three decimal places.

    :param x_position: target position in mm
    :param speed:      motion rate in mm/min

    """
    output.write("G1 X%.3f F%0.3F\n" % (x_position,speed))
    return

#================================================================
[docs]def curve(time, frequency = 0.2, magnitude = 2.0):
    """Return a single-axis position value representing a position on a curved path at a given time.

    :param time: time in seconds
    :param frequency: rate in cycles/second (Hz)
    :return: position in mm
    """
    
    # Some elementary functions and calculus: the sin function accepts an argument in radians; 
    # one complete cycle takes 2pi radians.  
    return magnitude * math.sin(time * frequency * 2 * math.pi)
    
#================================================================
# The following section is run when this is loaded as a script.
if __name__ == "__main__":

    # Specify the name of the NC G-code file to generate.
    filename = "osc_fn.nc"
    print("Writing output to %s" % filename)

    # Open the file and begin writing out G-Code.
    with open(filename,"w") as output:
        emit_prelude(output)

        # specify the time step for each path segment in seconds
        dt = 0.2

        # specify the magnitude of the oscillation in millimeters
        mag = 2.0

        # specify the initial frequency in cycle/sec (Hz)
        f0 = 0.2 
        
        # compute the initial time and position
        t0 = 0.0
        x0 = curve(t0, frequency = f0, magnitude = mag)
        freq = f0

        # rapid to the start point
        emit_rapid_move(output, x0)
        
        # iteration to compute a fixed number of samples
        duration = 60
        
        for i in range(int(duration/dt)):

            # keep speeding up by adjusting the frequency
            freq += 0.005 * dt
            
            # compute the time and position of the next sample 
            t1 = t0 + dt
            x1 = curve(t1, frequency = freq, magnitude = mag)
            
            # Estimate the feed rate in mm/min. The move command expects speeds
            # in mm/min, so the mm/sec rate is scaled by 60 sec/min.
            feed_rate = 60 * abs(x1 - x0) / dt

            # Keep the feed rate from getting too low.
            if feed_rate < 20.0: feed_rate = 20.0

            # Generate a move command to the new position, using the feed rate
            # so that the move should take dt seconds.
            emit_move(output, x1, feed_rate)

            # update the time and position for next cycle
            t0 = t1
            x0 = x1