path: root/src/hal/components/eoffset_per_angle.comp

component eoffset_per_angle "Compute External Offset Per Angle";
//" vim
description
"""

An offset is computed (from one of several functions) based on
an input angle in degrees.  The angle could be a rotary
coordinate value or a spindle angle.

The computed offset is represented as an s32 \\fBkcounts\\fR output
pin that is a compatible input to external offset pins like
\\fBaxis.L.eoffset-counts\\fR where \\fBL\\fR is the coordinate
letter.  Scaling of the s32 \\fBkcounts\\fR is controlled by the
input (\\fBk\\fR) -- its reciprocal value is presented on an output pin
(\\fBkreciprocal\\fR) for connection to \\fBaxis.L.eoffset-scale\\fR.
The default value for \\fBk\\fR should be suitable for most uses.

The built-in functions use pins \\fBfmult\\fR and \\fBrfraction\\fR
to control the output frequency (or number of polygon sides)
and amplitude respectively.  The  \\fBrfraction\\fR pin controls
the offset amplitude as a fraction of the \\fBradius-ref\\fR pin.

One of the four built-in functions is specified by the \\fBfnum\\fR pin:
  \\fB0\\fR: f0 inside  polygon  (requires \\fBfmult\\fR == nsides >= 3)
  \\fB1\\fR: f1 outside polygon  (requires \\fBfmult\\fR == nsides >= 3)
  \\fB2\\fR: f2 sinusoid
  \\fB3\\fR: f3 square wave

Unsupported \\fBfnum\\fR values default to use function f0.
""";

notes """
\\fBradius-ref\\fR:
The computed offsets are based on the \\fBradius-ref\\fR pin
value.  This pin may be set to a constant radius value or
controlled by a user interface or by g code program (using
\\fBM68\\fR and a \\fBmotion.analog-out-NN pin for instance).

\\fBStopping\\fR:
When the \\fBenable-in\\fR pin is deasserted, the offset is
returned to zero respecting the allocated acceleration
and velocity limits.  The allocations for coordinate \\fBL\\fR
are typically controlled by an ini file setting:
\\fB[AXIS_L]OFFSET_AV_RATIO\\fR.

If unsupported parameters are supplied to a function (for instance
a polygon with fewer than three sides), the current offset
will be returned to zero (respecting velocity and acceleration
constraints).  After correcting the offending parameter, the
\\fBenable-in\\fR pin must be toggled to resume offset
computations.
""";

examples """
An example simulation configuration is provided at:
\\fBconfigs/sim/axis/external_offsets/opa.ini\\fR.  A
simulated XZC machine uses the \\fBC\\fR coordinate angle to
offset the transverse \\fBX\\fR coordinate according to
the selected \\fBfnum\\fR function.
""";

//" quote char for vim highlighting

pin  in   bit active      = 0     "From: motion.eoffset-active";
pin  in   bit is_on       = 0     "From: halui.machine.is-on";

pin  in   bit enable_in   = 0     "Enable Input";
pin  in float radius_ref  = 1     "Radius reference (see notes)";
pin  in float angle       = 0     "Input angle (in degrees)";
pin  in float start_angle = 0     "Start angle (in degrees)";
pin  in   s32 fnum        = 0     "Function selector (default 0)";
pin  in float rfraction   = 0.1   "Offset amplitude (+/- fraction of radius_ref)";
pin  in float fmult       = 6     "Offset frequency multiplier";
pin  in   u32 k           = 10000 "Scaling Factor (if 0, use 10000)";

pin out   bit is_off      "invert is_on (for convenience)";

pin out   bit enable_out  "To: axis.L.eoffset-enable";
pin out   bit clear       "To: axis.L.eoffset-clear";
pin out   s32 kcounts     "To: axis.L.eoffset-counts";
pin out float kreciprocal "To: axis.L.eoffset-scale (1/k)";

pin out float eoffset_dbg "offset (debug pin--use kcounts & kreciprocal)";
pin out   u32 state_dbg   "state  (debug pin)";

//---------------------------------------------------------------------
// per-instance items:
variable int  run_ct   = 0;
variable int  delay_ct = 0;
variable int  messaged = 0;
variable int  err_stop = 0;

// Note: use of 'option data TYPE' with halcompile is deprecated
//       but the recommended alternative, 'variable CTYPE',
//       does not support typedefs for structs.
//       If support for 'option data TYPE' is removed, this file
//       should be converted from a .comp to a .c file with
//       additional Makefile support
option data the_data;
//---------------------------------------------------------------------

function _;
license "GPL";
author "Dewey Garrett";
;;

#include <rtapi_math.h>

#define FINISH_DELAY 0
#define TO_RAD       M_PI/180

typedef enum {
    OFF,
    READY,
    RUNNING,
    STOPPING,
    FINISH,
} state;

typedef struct ofunc_data {
  state  thestate;
  double adeg;
  double astart;
  double r_ref;
  double r_frac;
  double fmultiplier;
  double ovalue;
} the_data;

typedef int ofunc(struct ofunc_data*);
static      ofunc func0,func1,func2,func3;

#define OPA_DEBUG
#undef  OPA_DEBUG
#ifdef  OPA_DEBUG
  #define LVL RTAPI_MSG_INFO
  #define dprint(msg,n) do { \
    rtapi_set_msg_level(LVL); \
    rtapi_print_msg(LVL,"%20s %5d\n",msg,n); \
  } while (0)
#else
  #define dprint(msg,n)
#endif

FUNCTION(_) {
    struct ofunc_data* dptr = &data;
#define STATE dptr->thestate

    int    kfactor;
    ofunc* thefunc;

    run_ct++;
    state_dbg = STATE;
    kfactor = k;
    if (k == 0) {kfactor = 10000;}
    kreciprocal = 1/((float)kfactor);

    is_off = !is_on; // convenience pin

    if (is_off) {
        // note: the external_offsets implementation defines
        //       axis.L.eoffset as zero when machine is off
        err_stop   = 0;
        enable_out = 0;
        kcounts    = 0; eoffset_dbg = 0;
        messaged   = 0;
        STATE      = OFF;
        return;
    }

    switch (STATE) {
    case OFF:
        // require an enable_in 0-->1 transition to advance to READY
        if (enable_in) {
            if (!messaged) {
                rtapi_print_msg(RTAPI_MSG_ERR,
                "eoffset_per_angle: active enable-in not honored at start");
                messaged = 1;
            }
            return;
        }
        err_stop = 0;
        messaged = 1;
        kcounts  = 0;
        STATE    = READY;
        dprint("OFF->READY",kcounts);
        return;
        break;
    case READY:
        if (!enable_in) {return;}
        kcounts    = 0; eoffset_dbg = 0;
        enable_out = 0;
        delay_ct   = 0;
        STATE      = RUNNING;
        dprint("READY->RUNNING",kcounts);
        return;
        break;
    case RUNNING:
        if (enable_in) {
            enable_out = 1;
            STATE      = RUNNING;
        } else {
            /*
            ** When the enable_in pin is deasserted, kcounts are set to
            ** zero and the simple trajectory planner removes the offset to
            ** within its stopping criterion.  Under some conditions, a
            ** residual offset may remain.  Connecting the clear pin to
            ** axis.L.eoffset-clear forces the axis->ext_offset_tp.pos_cmd
            ** to zero to remove any residual with no modifications to
            ** simple_tp.c
            */
            clear    = 1;
            kcounts  = 0; eoffset_dbg = 0;
            STATE    = STOPPING;
            delay_ct = run_ct;
            dprint("RUNNING->STOPPING",kcounts);
            return;
        }
        break;
    case STOPPING:
        if (active) {
            STATE = STOPPING;
        } else {
            // !active ==> stopping criterion met
            delay_ct = run_ct;
            STATE    = FINISH;
            dprint("STOPPING->FINISH",kcounts);
        }
        return;
        break;
    case FINISH:
        // provision for delay if needed
        if (run_ct < (FINISH_DELAY + delay_ct) ) {
            STATE = FINISH;
        } else {
            enable_out = 0;
            if (err_stop) {
                STATE = OFF;
                dprint("FINISH->OFF",kcounts);
            } else {
                STATE = READY;
                dprint("FINISH->READY",kcounts);
            }
        }
        clear = 0;
        return;
        break;
    } //switch (STATE)

    switch (fnum) {
      case  0: thefunc = func0; break;
      case  1: thefunc = func1; break;
      case  2: thefunc = func2; break;
      case  3: thefunc = func3; break;
      default: thefunc = func0; break;
    }
    dptr->adeg        = angle;
    dptr->astart      = start_angle;
    dptr->r_ref       = radius_ref;
    dptr->r_frac      = rfraction;
    dptr->fmultiplier = fmult;

    if (thefunc(dptr) ) {
        // thefunc returned nonzero (problem)
        err_stop     = 1;
        dptr->ovalue = 0;
        kcounts      = 0; eoffset_dbg = 0;
        STATE        = STOPPING;
        rtapi_print_msg(RTAPI_MSG_ERR,
             "eoffset_per_angle stopping:func%d problem\n",fnum);
        return;
    }

    kcounts     = kfactor * dptr->ovalue;
    eoffset_dbg = kcounts * kreciprocal; // debug pin
    return;
}

static int func0 (struct ofunc_data* d)
{
    // for cutting an inside polygon
    // (start with negative rfraction, and gradually increase to zero)
    // polygon outscribed by circle of radius r_ref
    // (fmultiplier > 2)
    double uangle,divisor;

    if (d->fmultiplier <= 2) {
        rtapi_print_msg(RTAPI_MSG_ERR,
             "func0 bad fmultiplier: %g\n",d->fmultiplier);
        return -1;
    }

    divisor   = 360/d->fmultiplier;
    uangle    = d->adeg + divisor/2 - d->astart;
    d->ovalue = (1 + d->r_frac)
              * d->r_ref/cos((divisor/2 - fmod(uangle,divisor))*TO_RAD)
              - d->r_ref;
    return 0;
}

static int func1 (struct ofunc_data* d)
{
    // for cutting an outside polygon
    // (start with positive rfraction, and gradually reduce to zero)
    // polygon inscribed by circle of radius r_ref
    // (fmultiplier > 2)
    double uangle,divisor;

    if (d->fmultiplier <= 2) {
        rtapi_print_msg(RTAPI_MSG_ERR,
             "func1 bad fmultiplier: %g\n",d->fmultiplier);
        return -1;
    }

    divisor   = 360/d->fmultiplier;
    uangle    = d->adeg + divisor/2 - d->astart;
    d->ovalue = (1 + d->r_frac)
              * d->r_ref*cos(M_PI/d->fmultiplier)/cos((divisor/2 - fmod(uangle,divisor))*TO_RAD)
              - d->r_ref;
    return 0;
}

static int func2 (struct ofunc_data* d)
{
    // sin() -90 ==> start at zero amplitude
    double uangle;
    uangle    = d->fmultiplier * (d->adeg - d->astart) + -90;
    d->ovalue = (0.5 * d->r_frac* d->r_ref) * (1 + sin(uangle * TO_RAD));
    return 0;
}

static int func3 (struct ofunc_data* d)
{
    // square() -90 ==> start at zero amplitude
    // useful for looking at affects of ini settings
    // max vel & accel and offset_av_ratio
    double uangle;
    double value = -1;
    uangle = d->fmultiplier * (d->adeg - d->astart) + -90;
    if (sin(uangle * TO_RAD) >= 0) {value = 1;}
    d->ovalue = (0.5 * d->r_frac* d->r_ref) * (1 + value);
    return 0;
}