Deckel FP4 Gearbox Comp

/*
LinuxCNC component for controlling the Deckel FP4 gearbox.

Copymitte (C) 2018 Sergey 'Jin' Bostandzhyan <This email address is being protected from spambots. You need JavaScript enabled to view it.>

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 Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/

component FP4_gearbox "Component to control the Deckel FP4 gearbox to set the requested speed";
author "Sergey 'Jin' Bostandzhyan midified by Andreas Benz";
license "GPL";

/* to be connected with motion.spindle−speed−out−abs */
pin in float spindle_speed_in_abs = 0 "Desired spindle speed in rotations per minute, always positive regardless of spindle direction.";
/* to be conneced with motion.spindle−speed−in */
pin out float spindle_speed_out = 0 "Actual spindle speed feedback in revolutions per second";

/* gearbox status pins from the MESA 7i96S */
pin in bit vorgelege_vorne "MESA 7i96S INPUT 2: 28X2-11";
pin in bit vorgelege_mitte "MESA 7i96S INPUT 3: 28X2-11";
pin in bit vorgelege_hinten "MESA 7i96S INPUT 4: 28X2-13";

pin in bit block2_vorne "MESA 7i96S INPUT 5: 28X2-15";
pin in bit block2_mitte "MESA 7i96S INPUT 6: 28X2-16";
pin in bit block2_hinten "MESA 7i96S INPUT 7: 28X2-17";

pin in bit block1_vorne "MESA 7i96S INPUT 8: 28X2-19";
pin in bit block1_mitte "MESA 7i96S INPUT 9: 28X2-20";
pin in bit block1_hinten "MESA 7i96S INPUT 10: 28X2-21";


pin in bit spindle_stopped = 0 "MESA 7i84 INPUT 19: TB2-4";

pin out bit stop_spindle = 0 "Start or stop spindle";
pin out bit spindle_at_speed = 0;

/* control pins */
pin out bit motor_lowspeed = 0 "MESA 7i66 OUTPUT ";
pin out bit vorgelege_motor = 0 "MESA 7i66 OUTPUT C3 7i96S.0.7i66.0.0.output-03";
pin out bit block2_motor = 0 "MESA 7i66 OUTPUT C4 7i96S.0.7i66.0.0.output-04";
pin out bit block1_motor = 0 "MESA 7i66 OUTPUT C5 7i96S.0.7i66.0.0.output-05";
pin out bit rueckziehen = 0 "MESA 7i66 OUTPUT C6 7i96S.0.7i66.0.0.output-06";
pin out bit twitch_cw = 0 "MESA 7i66 OUTPUT 6: 28X1-14";
pin out bit twitch_ccw = 0 "MESA 7i66 OUTPUT 7: 28X1-15";

pin out bit estop_out = 0 "This pin will trigger emergency stop in case of an unrecoverably fatal error.";
pin in bit estop_in "This pin notifies us that an emergency stop was triggered outside the component.";

function _;

option singleton yes;

;;

#include <rtapi_math.h>

#include "FP4_common.h"
#include "FP4_util.h"
#include "FP4_gears.h"

static float g_last_spindle_speed = 0;

static tree_node_t *g_tree_rpm = NULL;
static tree_node_t *g_tree_mask = NULL;

static bool g_setup_done = false;

static bool g_last_estop = false;

/* one time setup, called from the main function to initialize whatever we
* need */
FUNCTION(setup)
{
int i;

/* Initialize state data structures */
gearbox_setup(__comp_inst, period);
twitch_setup(__comp_inst, period);

/* we want to have key:value pairs in the binary search tree, where
* the value represents the index of the key in our gears array. So
* we'll put things together the way we need them for the tree generation
*/
pair_t temp[FP4_NUM_GEARS];
for (i = 0; i < FP4_NUM_GEARS; i++)
{
temp.key = FP4_gears.key;
temp.value = i;
}

/* build up binary tree from the gears array to search by rpm, this
* array is already sorted */
g_tree_rpm = tree_from_sorted_array(temp, FP4_NUM_GEARS);

/* build up a key:value list where the bitmask from the m400e_gears
* array is the key and the index of the original position in the
* above array is the value */
for (i = 0; i < FP4_NUM_GEARS; i++)
{
temp.key = FP4_gears.value;
temp.value = i;
}

/* sort the newly created array by key (needed for tree build up) */
sort_array_by_key(temp, FP4_NUM_GEARS);

/* build up binary tree from the gears array to search for bitmask */
g_tree_mask = tree_from_sorted_array(temp, FP4_NUM_GEARS);

g_last_spindle_speed = spindle_speed_in_abs;

g_last_estop = estop_in;
}

/* When e-stop is triggered from the outside everything is already powered
* off, but we want to make sure that our pins are also inactive should
* the power come back. We don't have to care about timings here, because
* everything is already off at this point. */
FUNCTION(handle_external_e_stop)
{
rtapi_print_msg(RTAPI_MSG_ERR, "FP4_gearbox: EMERGENCY STOP condition "
"detected!\n");
/* reset state machine avoiding delays */
gearbox_handle_estop(); /* this function also stops/resets twitching */

spindle_at_speed = false;
stop_spindle = true;

/* reset estop_out pin since we could haave been the ones who triggered
* this e-stop */
estop_out = false;
}

/* main component function */
FUNCTION(_)
{
if (estop_in)
{
if (g_last_estop != estop_in)
{
handle_external_e_stop(__comp_inst, period);
g_last_estop = estop_in;
}
return;
}

g_last_estop = estop_in;

/* perform one time setup */
if (!g_setup_done)
{
setup(__comp_inst, period);
g_setup_done = true;
}

/* read and update global mask variables for each pin group */
update_current_pingroup_masks();

/* Gear shift is in progress */
if (!gearshift_in_progress())
{
if (stop_spindle && !spindle_stopped)
{
stop_spindle = false;
}

/* determine and update current spindle speed information */
pair_t *speed = get_current_gear(g_tree_mask);
if (speed != NULL)
{
spindle_speed_out = (float)speed->key;
}

if (g_last_spindle_speed == spindle_speed_in_abs)
{
/* Nothing to do */
spindle_at_speed = !spindle_stopped;
return;
}

/* We need to quantize the requested speed to see if our current
* gear already matches it */
pair_t *new_gear = select_gear_from_rpm(g_tree_rpm,
spindle_speed_in_abs);
/* Current speed already matches the requested speed, nothing to do */
if (new_gear->key == spindle_speed_out)
{
spindle_at_speed = !spindle_stopped;
return;
}

/* We don't attempt to do anything if the spindle is running. */
/* TODO: check that spindle_stopped triggers only when the spindle
* has come to a full stop and not just a that time it has been
* powered off (might still be moving due to inertia) */
if (!spindle_stopped)
{
gearshift_stop_spindle();
return;
}

/* We need to change to another gear */
g_last_spindle_speed = spindle_speed_in_abs;

spindle_at_speed = false;

/* This call will set the start_gear_shift pin! */
gearshift_start(new_gear, period);

/* Do the rest in the next cycle */
return;
}

/* Do the gear shifting */
gearshift_handle(period);
}

/*
LinuxCNC component for controlling the Deckel FP4 gearbox.

Copymitte (C) 2018 Sergey 'Jin' Bostandzhyan <This email address is being protected from spambots. You need JavaScript enabled to view it.>

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 Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/

/* Functions related to gear switching. */

#include "FP4_gears.h"
#include "FP4_twitch.h"

typedef enum
{
    SHAFT_STATE_OFF,    /* Initial shaft state */
    SHAFT_STATE_ON,     /* Shift in process (i.e. shaft motor running) */
    SHAFT_STATE_RESTART /* Error condition, we missed our target and reached
                           an end point, we need to go back */
} shaft_state_t;

/* Group all data that is required to operate on one shaft */
typedef struct
{
    shaft_state_t state;
    pin_group_t status_pins;
    hal_bit_t *motor_on;
    hal_bit_t *motor_reverse;
    hal_bit_t *motor_slow;
    unsigned char current_mask; /* auto updated via global variable */
    unsigned char target_mask;
} shaft_data_t;

/* Group all data required for gearshifting */
static struct
{
    hal_bit_t *start_shift;
    hal_bit_t *do_stop_spindle;
    hal_bit_t *is_spindle_stopped;
    hal_bit_t *trigger_estop;
    hal_bit_t *notify_spindle_at_speed;
    bool spindle_on_before_shift;
    shaft_data_t vorgelege;
    shaft_data_t block2;
    shaft_data_t block1;
    long delay;
    statefunc next;
} g_gearbox_data;

/* One time setup function to prepare data structures related to gearbox
 * switching*/
FUNCTION(gearbox_setup)
{
    /* Populate data structures that will be used be the state functions
     * when shifting gears */

    g_gearbox_data.vorgelege.state = SHAFT_STATE_OFF;
    /* Grabbing the pin pointers in EXTRA_SETUP did not work because the
     * component did not seem to be fully initializedt there.
     *
     * Another issue:
     * while output pins are defined as (*__comp_inst->pin_name) by
     * halcompile, input pins are turned into (0+*__comp_inst->pin_name)
     * which makes it impossible to get the pointers via the defines
     * created by halcompile. Accessing the pin variable directly did not
     * work due to macro expansion, only workaround I found was to temporarily
     * disable the macros.
     */
    #pragma push_macro("vorgelege_vorne")
    #pragma push_macro("vorgelege_mitte")
    #pragma push_macro("vorgelege_hinten")
    #pragma push_macro("vorgelege_vorne_hinten")
    #undef vorgelege_vorne
    #undef vorgelege_mitte
    #undef vorgelege_hinten
    #undef vorgelege_vorne_hinten
    g_gearbox_data.vorgelege.status_pins = (pin_group_t)
    {
        __comp_inst->vorgelege_vorne,
        __comp_inst->vorgelege_mitte,
        __comp_inst->vorgelege_hinten,
       
    };
    #pragma pop_macro("vorgelege_vorne")
    #pragma pop_macro("vorgelege_mitte")
    #pragma pop_macro("vorgelege_hinten")
    #pragma pop_macro("vorgelege_vorne_hinten")
    g_gearbox_data.vorgelege.motor_on = &vorgelege_motor;
    g_gearbox_data.vorgelege.motor_reverse = &rueckziehen;
    g_gearbox_data.vorgelege.motor_slow = &motor_lowspeed;
    g_gearbox_data.vorgelege.current_mask = 0;
    g_gearbox_data.vorgelege.target_mask =
        FP4_gears[FP4_NEUTRAL_GEAR_INDEX].value; /* neutral */

    g_gearbox_data.block2.state = SHAFT_STATE_OFF;
    #pragma push_macro("block2_vorne")
    #pragma push_macro("block2_mitte")
    #pragma push_macro("block2_hinten")
    #pragma push_macro("block2_vorne_hinten")
    #undef block2_vorne
    #undef block2_mitte
    #undef block2_hinten
    #undef block2_vorne_hinten
    g_gearbox_data.block2.status_pins = (pin_group_t)
    {
        __comp_inst->block2_vorne,
        __comp_inst->block2_mitte,
        __comp_inst->block2_hinten,
       
    };
    #pragma pop_macro("block2_vorne")
    #pragma pop_macro("block2_mitte")
    #pragma pop_macro("block2_hinten")
    #pragma pop_macro("block2_vorne_hinten")
    g_gearbox_data.block2.motor_on = &block2_motor;
    g_gearbox_data.block2.motor_reverse = &rueckziehen;
    g_gearbox_data.block2.motor_slow = &motor_lowspeed;
    g_gearbox_data.block2.current_mask = 0;
    g_gearbox_data.block2.target_mask = 0; /* don't care for neutral */

    g_gearbox_data.block1.state = SHAFT_STATE_OFF;
    #pragma push_macro("block1_vorne")
    #pragma push_macro("block1_mitte")
    #pragma push_macro("block1_hinten")
    #pragma push_macro("block1_vorne_hinten")
    #undef block1_vorne
    #undef block1_mitte
    #undef block1_hinten
    #undef block1_vorne_hinten
    g_gearbox_data.block1.status_pins = (pin_group_t)
    {
        __comp_inst->block1_vorne,
        __comp_inst->block1_mitte,
        __comp_inst->block1_hinten,
      
    };
    #pragma pop_macro("block1_vorne")
    #pragma pop_macro("block1_mitte")
    #pragma pop_macro("block1_hinten")
    #pragma pop_macro("block1_vorne_hinten")
    g_gearbox_data.block1.motor_on = &block1_motor;
    g_gearbox_data.block1.motor_reverse = &rueckziehen;
    g_gearbox_data.block1.motor_slow = &motor_lowspeed;
    g_gearbox_data.block1.current_mask = 0;
    g_gearbox_data.block1.target_mask = 0; /* don't care for neutral */

    #pragma push_macro("spindle_stopped")
    #undef spindle_stopped
    
    #pragma pop_macro("spindle_stopped")
    g_gearbox_data.do_stop_spindle = &stop_spindle;
    g_gearbox_data.spindle_on_before_shift = false;
    
    g_gearbox_data.trigger_estop = &estop_out;
    g_gearbox_data.notify_spindle_at_speed = &spindle_at_speed;
    g_gearbox_data.delay = 0;
    g_gearbox_data.next = NULL;
}

static void gearshift_stop_spindle(void)
{
    g_gearbox_data.spindle_on_before_shift =
        !(*g_gearbox_data.is_spindle_stopped);
    *g_gearbox_data.do_stop_spindle = true;
}

/* combine values of all pins in a group to a bitmask */
static unsigned char get_bitmask_from_pingroup(pin_group_t *group)
{
    unsigned char mask = 0;
    int i;
    for (i = 0; i < FP4_PINS_IN_GROUP; i++)
    {
        mask |= *(group->p) << i;
    }

    return mask;
}

/* Update current mask values for each shaft */
static void update_current_pingroup_masks(void)
{
    g_gearbox_data.vorgelege.current_mask =
        get_bitmask_from_pingroup(&g_gearbox_data.vorgelege.status_pins);
    g_gearbox_data.block2.current_mask =
        get_bitmask_from_pingroup(&g_gearbox_data.block2.status_pins);
    g_gearbox_data.block1.current_mask =
        get_bitmask_from_pingroup(&g_gearbox_data.block1.status_pins);
}

static bool estop_on_spindle_running(void)
{
    if (!*g_gearbox_data.is_spindle_stopped)
    {
        /* This is an invalid condition, spindle must be stopped if we are
         * shifting and we tested for it before we started.
         *
         * We expect that estop_out will be looped back to us so that
         * it will trigger our handler. */
        rtapi_print_msg(RTAPI_MSG_ERR, "FP4_gearbox FATAL ERROR: detected "
                "running spindle while shifting, triggering emergency stop!\n");
        *g_gearbox_data.trigger_estop = true;
        return true;
    }

    return false;
}

/* Combine masks from each pin group to a value representing the current
 * gear setting. A return of NULL means that a corresponding value could
 * not be found, which may indicate a gearshift being in progress- */
static pair_t* get_current_gear(tree_node_t *tree)
{
    tree_node_t *result;

    unsigned combined = (g_gearbox_data.block1.current_mask << 8) |
                        (g_gearbox_data.block2.current_mask << 4) |
                         g_gearbox_data.vorgelege.current_mask;

    /* special case: ignore all other bits for neutral */
    if (g_gearbox_data.vorgelege.current_mask ==
            FP4_gears[FP4_NEUTRAL_GEAR_INDEX].value)
    {
        return &(FP4_gears[FP4_NEUTRAL_GEAR_INDEX]);
    }

    result = tree_search(tree, combined);
    if (result != NULL)
    {
        return &(FP4_gears[result->value]);
    }
    return NULL;
}

/* Helper to update delays, returns true if time has not elapsed. */
static bool gearshift_wait_delay(long period)
{
    if ((period > 0) && (g_gearbox_data.delay > 0))
    {
        g_gearbox_data.delay = g_gearbox_data.delay - period;
        return true;
    }
    g_gearbox_data.delay = 0;
    return false;
}

/* From:
 * forum.linuxcnc.org/12-milling/33035-retr...FP4?start=460#117021
 *
 * 1. if u need to go to the vorne then turn cw
 * 2. if u need to go to the mitte than turn ccw
 * 3. if u need to go to the block2 and vorne-hinten is 1 then turn ccw else cw
 *
 * ┌───┐
 * ┘   └──────────────── vorne
 *         ┌───┐
 * ────────┘   └──────── hinten
 *                 ┌───┐
 * ────────────────┘   └ mitte
 *           ┌──────────
 * ──────────┘           vorne-hinten
 *
 */
static bool gearshift_need_reverse(unsigned char target_mask,
                                   unsigned char current_mask)
{
    if (FP4_STAGE_IS_mitte(target_mask))           /* CCW, reverse is on */
    {
        return true;
    }
    else if (FP4_STAGE_IS_vorne(target_mask))       /* CW, reverse is off */
    {
        return false;
    }
    else if (FP4_STAGE_IS_hinten(target_mask))
    {
            return false;
    }
            
    return true;
}


/* State functions */

/* This is more or less an "overshoot" protection check in case we missed the
 * target hinten pos and moved further. We know when we reach an end point
 * and we know we can't continue further in this direction, so stop trying and
 * go back. Returns true if action needs to be taken. */
static bool gearshift_protect(shaft_data_t *shaft)
{
    if (!*shaft->motor_on)
    {
        return false;
    }

    if (*shaft->motor_reverse)
    {
        /* If we move to the vorne/CW and we reached the furthest vorne position
         * which does not seem to be our desired target, then we should
         * disable the motor and trigger an E-STOP, we should never end up#
         * in this situation. */
        if ((shaft->current_mask == FP4_STAGE_POS_mitte) &&
            (shaft->current_mask != shaft->target_mask))
        {
            rtapi_print_msg(RTAPI_MSG_ERR, "FP4_gearbox: WARNING: "
                        "shaft motor at unexpected mitte position!\n");
        }
        else
        {
            return false;
        }
    }
    else
    {
        /* If we move to the vorne/CW and we reached the furthest vorne position
         * which does not seem to be our desired target, then we should
         * disable the motor and trigger an E-STOP, we should never end up#
         * in this situation. */
        if ((shaft->current_mask == FP4_STAGE_POS_vorne) &&
            (shaft->current_mask != shaft->target_mask))
        {
            rtapi_print_msg(RTAPI_MSG_ERR, "FP4_gearbox: WARNING: "
                        "shaft motor at unexpected vorne position!\n");
        }
        else
        {
            return false;
        }

    }

    return true;
}

/* Generic function that has the exact same logic, valid for all of the
 * three shafts. */
static void gearshift_stage(shaft_data_t *shaft, statefunc me, statefunc next,
                            long period)
{
    if (estop_on_spindle_running())
    {
        return;
    }

    if (gearshift_wait_delay(period))
    {
        g_gearbox_data.next = me;
        return;
    }

    if (shaft->state == SHAFT_STATE_OFF)
    {

        /* Are the pins already in the desired state? */
        if (shaft->current_mask == shaft->target_mask)
        {
            g_gearbox_data.next = next;
        }
        else
        {
            shaft->state = SHAFT_STATE_ON;

            if (gearshift_need_reverse(shaft->target_mask,
                                       shaft->current_mask))
            {
                *shaft->motor_reverse = true;
                g_gearbox_data.delay = FP4_REVERSE_MOTOR_INTERVAL;
            }
            g_gearbox_data.next = me;
        }
    }
    else if (shaft->state == SHAFT_STATE_ON)
    {
        /* Did we reach the desired position? */
        if (shaft->current_mask == shaft->target_mask)
        {
            if (*shaft->motor_on)
            {
                /* De-energize the shaft motor */
                *shaft->motor_on = false;
            }
            else
            {
                /* Second time we enter this state the motor will be off,
                 * that means that we already did the waiting that may have
                 * been set in the "if" below. If reverse direction was
                 * not active originally, then this does nothing */
                *shaft->motor_reverse = false;
            }

            /* If reverse direction has been set, disable it in 100ms */
            if (*shaft->motor_reverse)
            {
                g_gearbox_data.delay = FP4_GENERIC_PIN_INTERVAL;
                g_gearbox_data.next = me;
                return;
            }
            else
            {
                *shaft->motor_slow = false;
            }
          
            if (*shaft->motor_slow)
            {
                g_gearbox_data.delay = FP4_GENERIC_PIN_INTERVAL;
                g_gearbox_data.next = me;
                return;
            }

            /* We are done here, proceed to the next stage */
            shaft->state = SHAFT_STATE_OFF;
            g_gearbox_data.delay = FP4_GENERIC_PIN_INTERVAL;
            g_gearbox_data.next = next;
        }
        else
        {
            /* Protect furthest lect/CW and mitte/CCW end positions by not
             * allowing the motor to continue running if we reached them,
             * this should never happen, but it's better to have a safety
             * measure to prevent hardware damage. The function will
             * immediately stop the motor and trigger an emergency stop if this
             * error condition is detected. */
            if (gearshift_protect(shaft))
            {
                *shaft->motor_on = false;
                shaft->state = SHAFT_STATE_RESTART;
                g_gearbox_data.delay = FP4_REVERSE_MOTOR_INTERVAL;
                g_gearbox_data.next = me;
                return;
            }

            /* Going to the hinten requres lowering the motor speed */
            if (FP4_STAGE_IS_hinten(shaft->target_mask) &&
                !(*shaft->motor_slow))
            {
                *shaft->motor_slow = true;
            }
            else if (!(*shaft->motor_on))
            {
                /* Energize motor if it is not yet running */
                *shaft->motor_on = true;
            }

            g_gearbox_data.delay = FP4_GEAR_STAGE_POLL_INTERVAL;
            g_gearbox_data.next = me;
        }
    }
    else if (shaft->state == SHAFT_STATE_RESTART)
    {
        /* Protection function restarted us, motor is already off and
         * we came here after a certain delay. We now need to check what to do
         * and re-energize */
          if (*shaft->motor_reverse)
          {
              *shaft->motor_reverse = false;
              g_gearbox_data.delay = FP4_GENERIC_PIN_INTERVAL;
              g_gearbox_data.next = me;
              return;
          }

          if (*shaft->motor_slow)
          {
              *shaft->motor_slow = false;
              g_gearbox_data.delay = FP4_GENERIC_PIN_INTERVAL;
          }

          /* Going back to the OFF state will retrigger the shift logic for
           * this shaft */
          shaft->state = SHAFT_STATE_OFF;
          g_gearbox_data.next = me;
    }
}

static void gearshift_stop(long period)
{
    if (gearshift_wait_delay(period))
    {
        g_gearbox_data.next = gearshift_stop;
        return;
    }

    twitch_stop(period);

    if (!twitch_stop_completed())
    {
        g_gearbox_data.delay = FP4_TWITCH_KEEP_PIN_OFF;
        g_gearbox_data.next = gearshift_stop;
        return;
    }

    if (*g_gearbox_data.start_shift)
    {
       

        if (g_gearbox_data.spindle_on_before_shift)
        {
            *g_gearbox_data.do_stop_spindle = false;
            g_gearbox_data.delay = FP4_WAIT_SPINDLE_AT_SPEED;
            g_gearbox_data.next = gearshift_stop;
            return;
        }
    }

    if (g_gearbox_data.spindle_on_before_shift)
    {
        *g_gearbox_data.notify_spindle_at_speed = true;
    }

    /* We are done shifting, reset everything */
    g_gearbox_data.next = NULL;
    g_gearbox_data.spindle_on_before_shift = false;
}

static void gearshift_vorgelege(long period)
{
    gearshift_stage(&(g_gearbox_data.vorgelege), gearshift_vorgelege,
                    gearshift_stop, period);
}

static void gearshift_block2(long period)
{
    gearshift_stage(&(g_gearbox_data.block2), gearshift_block2,
                    gearshift_vorgelege, period);
}

static void gearshift_block1(long period)
{
    gearshift_stage(&(g_gearbox_data.block1), gearshift_block1,
                    gearshift_block2, period);
}

/* Call this function once per each thread cycle to handle gearshifting,
 * implies that gearshift_start() has been called in order to set the
 * target gear. */
static void gearshift_handle(long period)
{
    twitch_handle(period);

    if (g_gearbox_data.next == NULL)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "FP4_gearbox FATAL ERROR: "
                        "gearshift function not set up, triggering E-Stop!\n");
        *g_gearbox_data.trigger_estop = true;
        return;
    }

    g_gearbox_data.next(period);
}

/* Start shifting process */
static void gearshift_start(pair_t *target_gear, long period)
{
    if (estop_on_spindle_running())
    {
        return;
    }

    g_gearbox_data.vorgelege.target_mask = (target_gear->value) & 0x000f;
    g_gearbox_data.block2.target_mask = (target_gear->value & 0x00f0) >> 4;
    g_gearbox_data.block1.target_mask =
                                    (target_gear->value & 0x0f00) >> 8;

    /* Make sure to leave 100ms between setting start_gear_shift to "on"
     * and further operations */
    g_gearbox_data.delay = FP4_GENERIC_PIN_INTERVAL;

   

    twitch_start(period);

    /* Special case: if we want to go to the neutral position, we
     * only care about the vorgelege stage, so we can jump mitte to it */
    if (g_gearbox_data.vorgelege.target_mask ==
            FP4_gears[FP4_NEUTRAL_GEAR_INDEX].value) {
        g_gearbox_data.next = gearshift_vorgelege;
    }
    else
    {
        g_gearbox_data.next = gearshift_block1;
    }
}

/* Reset pins and state machine if an emergency stop was triggered. */
static void gearbox_handle_estop(void)
{
    *g_gearbox_data.block1.motor_on = false;
    *g_gearbox_data.block2.motor_on = false;
    *g_gearbox_data.vorgelege.motor_on = false;
    /* There are no separate pins for revers/slow for each shaft, each
     * shaft structure has pointers to the same pins, so its enough to
     * reset them only on one shaft. */
    *g_gearbox_data.vorgelege.motor_reverse = false;
    *g_gearbox_data.vorgelege.motor_slow = false;

    gearshift_stop(0); /* Will stop and reset twitching as well */
}

static bool gearshift_in_progress(void)
{
    return g_gearbox_data.next != NULL;
}