component sim_axis_hardwarev2 "A component to simulate home and limit switches";

description
"""
This component creates simulated home and limit switches based on the current position.
.br
It currently can supply simulation for X,Y,Z and A axes.

""";
pin_ptr in float Xcurrent_pos "The current position on the axis - eg connect to axis.0.joint-pos-fb";
pin_ptr in float Ycurrent_pos;
pin_ptr in float Zcurrent_pos;
pin_ptr in float Acurrent_pos;

pin_ptr in float Xhomesw_pos =1 "The position of the home switch";
pin_ptr in float Yhomesw_pos =1;
pin_ptr in float Zhomesw_pos =1;
pin_ptr in float Ahomesw_pos =1;

pin_ptr in float Xmaxsw_upper "The upper range of the maximum limit switch, above this is false.";
pin_ptr in float Ymaxsw_upper;
pin_ptr in float Zmaxsw_upper;
pin_ptr in float Amaxsw_upper;

pin_ptr in float Xmaxsw_lower "The lower range of the maximum limit switch, below this is false.";
pin_ptr in float Ymaxsw_lower;
pin_ptr in float Zmaxsw_lower;
pin_ptr in float Amaxsw_lower;

pin_ptr in float Xminsw_upper "The upper range of the minimum limit switch, above this is false.";
pin_ptr in float Yminsw_upper;
pin_ptr in float Zminsw_upper;
pin_ptr in float Aminsw_upper;

pin_ptr in float Xminsw_lower "The lower range of the minimum limit switch, below this is false.";
pin_ptr in float Yminsw_lower;
pin_ptr in float Zminsw_lower;
pin_ptr in float Aminsw_lower;

pin_ptr in float Xhomesw_hyst = 0.02 "range that home switch will be true +- half this to the home position";
pin_ptr in float Yhomesw_hyst = 0.02;
pin_ptr in float Zhomesw_hyst = 0.02;
pin_ptr in float Ahomesw_hyst = 0.02;

pin_ptr out bit Xhomesw_out" Home switch for the X axis";
pin_ptr out bit Yhomesw_out;
pin_ptr out bit Zhomesw_out;
pin_ptr out bit Ahomesw_out;
pin_ptr out bit homesw_all;

pin_ptr out bit Xmaxsw_out "Max limit switch";
pin_ptr out bit Xminsw_out "min limit switch";
pin_ptr out bit Xbothsw_out "True for both max and min limit switch";

pin_ptr out bit Ymaxsw_out;
pin_ptr out bit Yminsw_out;
pin_ptr out bit Ybothsw_out;

pin_ptr out bit Zmaxsw_out;
pin_ptr out bit Zminsw_out;
pin_ptr out bit Zbothsw_out;

pin_ptr out bit Amaxsw_out;
pin_ptr out bit Aminsw_out;
pin_ptr out bit Abothsw_out;

pin_ptr out bit limitsw_all;
pin_ptr out bit limitsw_homesw_all "True for all limits and all home.";

pin_ptr out bit Xmaxsw_homesw_out;
pin_ptr out bit Xminsw_homesw_out;
pin_ptr out bit Xbothsw_homesw_out;

pin_ptr out bit Ymaxsw_homesw_out;
pin_ptr out bit Yminsw_homesw_out;
pin_ptr out bit Ybothsw_homesw_out;

pin_ptr out bit Zmaxsw_homesw_out;
pin_ptr out bit Zminsw_homesw_out;
pin_ptr out bit Zbothsw_homesw_out;

pin_ptr out bit Amaxsw_homesw_out;
pin_ptr out bit Aminsw_homesw_out;
pin_ptr out bit Abothsw_homesw_out;

pin_ptr in float limit_offset = 0.01 "how much the limit switches are offset from inputed position. added to max, subracted from min";

function update fp;

license "GPL";
author "Chris Morley";
;;

/* private comparator function */
static int comp(hal_float_t in0, hal_float_t in1, hal_float_t hyst)
{
hal_float_t tmp;
hal_float_t halfhyst;

    tmp = in1 - in0;
    halfhyst = 0.5 * hyst;

    if(tmp < -halfhyst)
        return false;
    else if(tmp > halfhyst)
        return false;
    else
        return true;
}

/* private window comparator function */
static int wcomp (hal_float_t in, hal_float_t max_, hal_float_t min_)
{
    return !((in >= max_)|| (in <= min_));
}


FUNCTION(update) {

    /*set home switches */
    sb(Xhomesw_out, comp(gf(Xhomesw_pos), gf(Xcurrent_pos), gf(Xhomesw_hyst)));
    sb(Yhomesw_out, comp(gf(Yhomesw_pos), gf(Ycurrent_pos), gf(Yhomesw_hyst)));
    sb(Zhomesw_out, comp(gf(Zhomesw_pos), gf(Zcurrent_pos), gf(Zhomesw_hyst)));
    sb(Ahomesw_out, comp(gf(Ahomesw_pos), gf(Acurrent_pos), gf(Ahomesw_hyst)));
    sb(homesw_all, (gb(Xhomesw_out) || gb(Yhomesw_out) || gb(Zhomesw_out) || gb(Ahomesw_out)));

    /* set limit switches */
    sb(Xmaxsw_out, wcomp(gf(Xcurrent_pos), gf(Xmaxsw_upper), gf(Xmaxsw_lower) + gf(limit_offset)));
    sb(Xminsw_out, wcomp(gf(Xcurrent_pos), gf(Xminsw_upper) - gf(limit_offset), gf(Xminsw_lower)));
    sb(Xbothsw_out, (gb(Xmaxsw_out) || gb(Xminsw_out)));

    sb(Ymaxsw_out, wcomp(gf(Ycurrent_pos), gf(Ymaxsw_upper), gf(Ymaxsw_lower) + gf(limit_offset)));
    sb(Yminsw_out, wcomp(gf(Ycurrent_pos), gf(Yminsw_upper) - gf(limit_offset), gf(Yminsw_lower)));
    sb(Ybothsw_out, (gb(Ymaxsw_out) || gb(Yminsw_out)));

    sb(Zmaxsw_out, wcomp(gf(Zcurrent_pos), gf(Zmaxsw_upper), gf(Zmaxsw_lower) + gf(limit_offset)));
    sb(Zminsw_out, wcomp(gf(Zcurrent_pos), gf(Zminsw_upper) - gf(limit_offset), gf(Zminsw_lower)));
    sb(Zbothsw_out, (gb(Zmaxsw_out) || gb(Zminsw_out)));

    sb(Amaxsw_out, wcomp(gf(Acurrent_pos), gf(Amaxsw_upper), gf(Amaxsw_lower) + gf(limit_offset)));
    sb(Aminsw_out, wcomp(gf(Acurrent_pos), gf(Aminsw_upper) - gf(limit_offset), gf(Aminsw_lower)));
    sb(Abothsw_out, (gb(Amaxsw_out) || gb(Aminsw_out)));

    sb(limitsw_all, (gb(Xbothsw_out) || gb(Ybothsw_out) || gb(Zbothsw_out) || gb(Abothsw_out)));

    /* set limit and home switches */
    sb(limitsw_homesw_all, (gb(homesw_all) || gb(limitsw_all)));
    sb(Xmaxsw_homesw_out, (gb(Xmaxsw_out) || gb(Xhomesw_out)));
    sb(Xminsw_homesw_out, (gb(Xminsw_out) || gb(Xhomesw_out)));
    sb(Xbothsw_homesw_out, (gb(Xbothsw_out) || gb(Xhomesw_out)));

    sb(Ymaxsw_homesw_out, (gb(Ymaxsw_out) || gb(Yhomesw_out)));
    sb(Yminsw_homesw_out, (gb(Yminsw_out) || gb(Yhomesw_out)));
    sb(Ybothsw_homesw_out, (gb(Ybothsw_out) || gb(Yhomesw_out)));

    sb(Zmaxsw_homesw_out, (gb(Zmaxsw_out) || gb(Zhomesw_out)));
    sb(Zminsw_homesw_out, (gb(Zminsw_out) || gb(Zhomesw_out)));
    sb(Zbothsw_homesw_out, (gb(Zbothsw_out) || gb(Zhomesw_out)));

    return 0;
}
