Homing signal processing

component toggle2index "Index-Enable signal processing component for Yaskawa_SGDXS-xxxxA0x";
//
//    Copyright (C) 2023 Masao Sakai <msakai@sakaigiken.co.jp>
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program; if not, write to the Free Software
//    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
//
author "Masao Sakai";


pin in bit tgl_in = false                        "Toggle bit for touch probe trigger signal";
pin io bit index_enable = false            "Connect the joint.N.index-enable signal";
pin in float tchprb_pos                            "Touch probe position value";
pin in float cmd_pos                                "Connect the joint.N.motor-pos-cmd signal";
pin in float sv_ac_pos                            "Position Actual Value";
pin out float tg_pos                                "Interpolation Data Record(60C1h)";
pin out float fb_pos                                "Connect joint.N.motor-pos-fb signal";
pin out float fb_vel                                " ";
pin out float fb_rps                                " ";

param rw float scale = 0.001                    "Encoder and joint unit conversion scale";
param rw float revcount = 10000                "Number of pulses per rotation of encoder";

variable bool old_tgl = false;
variable bool toggled = false;
variable float old_fbpos = 0;
variable float offset = 0;

function _;
license "GPL"; //indicates GPL v2 or later
;;

// Memorize that the toggle signal has switched
    if (old_tgl ^ tgl_in) {
        old_tgl = tgl_in;
        toggled = true;
    }

//Reset the encoder counter and memorize the reference position
// when toggled and index-enable is ON
    if index_enable {
        if (toggled && index_enable){
            offset = tchprb_pos;
            index_enable = false;
        }
    }
    toggled = false;
    fb_pos = (sv_ac_pos - offset) * scale;
    tg_pos = cmd_pos / scale + offset;
    fb_vel = (fb_pos - old_fbpos) / fperiod;
    fb_rps = fb_vel / (revcount * scale);
    old_fbpos = fb_pos;