stepper.c

Go to the documentation of this file.

#include "stepper.h"

#include "hw/hw_stepper.h"
#include "hw/hw_sensor.h"

#include "cfg/cfg_stepper.h"
#include <cfg/debug.h>

// Define logging setting (for cfg/log.h module).
#define LOG_LEVEL   STEPPER_LOG_LEVEL
#define LOG_FORMAT  STEPPER_LOG_FORMAT
#include <cfg/log.h>

#include <kern/proc.h>

#include <algo/ramp.h>

#include CPU_HEADER(stepper)

#include <string.h>  // memset

#define MOTOR_SWITCH_TICKS      60000   ///< Timer ticks to wait for 10ms
#define MOTOR_SWITCH_COUNT          5   ///< Number of intervals, long 10ms, to wait before/after switching current off/on
#define MOTOR_HOME_MAX_STEPS    30000   ///< Steps before giving up when trying to reach home
#define MOTOR_CURRENT_TICKS      6000   ///< Number of intervals, long 10ms, to mantain high current
// \}

static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS];

static fsm_state general_states[STEPPER_MAX_STATES];

// IRQ functions for stepper motors
static void stepper_interrupt(struct Stepper *motor);

static void stepper_accel(struct Stepper *motor);
static void stepper_decel(struct Stepper *motor);

static bool stepper_isState(struct Stepper *motor, enum StepperState state);
INLINE void stepper_changeState(struct Stepper *motor, enum StepperState newState);

static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive);

#define MOTOR_INDEX(motor)     (motor->index)

//------------------------------------------------------------------------

INLINE bool setLowCurrent(struct Stepper* motor)
{
    if (motor->power == motor->cfg->powerIdle)
        return false;

    motor->power = motor->cfg->powerIdle;
    STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerIdle);

    return true;
}

INLINE bool setHighCurrent(struct Stepper* motor)
{
    if (motor->power == motor->cfg->powerRun)
        return false;

    motor->power = motor->cfg->powerRun;
    STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerRun);
    return true;
}

INLINE void setCheckSensor(struct Stepper* motor, enum MotorHomeSensorCheck value)
{
    motor->enableCheckHome = value;
}

INLINE int8_t getCheckSensor(struct Stepper* motor)
{
    return motor->enableCheckHome;
}

INLINE void setDirection(struct Stepper* motor, enum MotorDirection dir)
{
    ASSERT(dir == DIR_POSITIVE || dir == DIR_NEGATIVE);
    motor->dir = dir;

    if (!motor->cfg->flags.axisInverted)
    {
        STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir == DIR_POSITIVE));
    }
    else
    {
        STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir != DIR_POSITIVE));
    }
}

INLINE void FAST_FUNC stepper_schedule_irq(struct Stepper* motor, stepper_time_t delay, bool do_step)
{

    if (do_step)
    {
        // Record the step we just did
        motor->step += motor->dir;
        stepper_tc_doPulse(motor->timer);
    }
    else
        stepper_tc_skipPulse(motor->timer);

    stepper_tc_setDelay(motor->timer, delay);
}


static void stepper_accel(struct Stepper *motor)
{
    DB(uint16_t old_val = motor->rampValue;)
    DB(uint32_t old_clock = motor->rampClock;)

    const struct Ramp *ramp = &motor->cfg->ramp;

    ASSERT(motor->rampClock != 0);

    motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
    motor->rampClock += motor->rampValue;
    motor->rampStep++;

    DB(if (old_val && motor->rampValue > old_val)
    {
        LOG_ERR("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL);
        LOG_ERR("    %04x @ %lu   -->   %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock);
    })

}

static void stepper_decel(struct Stepper *motor)
{
    const struct Ramp *ramp = &motor->cfg->ramp;
    DB(uint16_t old_val = motor->rampValue;)

    motor->rampClock -= motor->rampValue;
    ASSERT(motor->rampClock != 0);
    motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
    motor->rampStep--;
    DB(ASSERT(!old_val || motor->rampValue >= old_val););
}

INLINE void stepper_enable_irq(struct Stepper* motor)
{
    stepper_tc_irq_enable(motor->timer);
}

INLINE void stepper_disable_irq(struct Stepper* motor)
{
    stepper_tc_irq_disable(motor->timer);
}

// the home sensor can be in the standard home list or in the digital
// sensor list
bool stepper_readHome(struct Stepper* motor)
{
    return (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS) ?
        hw_home_sensor_read(motor->cfg->homeSensorIndex) :
        bld_hw_sensor_read(motor->cfg->homeSensorIndex - NUM_HOME_SENSORS);
}

bool stepper_readLevel(struct Stepper* motor)
{
    return hw_level_sensor_read(motor->cfg->levelSensorIndex);
}

/************************************************************************/
/* Finite-state machine to drive stepper logic from IRQ                 */
/************************************************************************/

INLINE void stepper_changeState(struct Stepper* motor, enum StepperState newState)
{
    ASSERT(newState < STEPPER_MAX_STATES);

    motor->state = motor->cfg->states[newState];
    if (!motor->state)
        motor->state = general_states[newState];
    ASSERT(motor->state);
}

static bool stepper_isState(struct Stepper* motor, enum StepperState state)
{
    return (motor->cfg->states[state]
            ? motor->cfg->states[state] == motor->state
            : general_states[state] == motor->state);
}

static bool stepper_checkHomeErrors(struct Stepper* motor)
{
    bool home;

    home = stepper_readHome(motor);

    if (motor->enableCheckHome == MOTOR_HOMESENSOR_INCHECK && home
        && (!motor->stepCircular || motor->step < motor->stepCircular / 2))
        /*
        * if home Sensor check enabled in movement to 0 position and
        * the motor is in home increase the counter
        * for rotating motor we include the check that the motor is
        * inside the last "lap" (FIXME: check it better)
        */
        motor->stepsErrorHome++;
    else if (motor->enableCheckHome == MOTOR_HOMESENSOR_OUTCHECK && !home)
        /*
        * if home Sensor check enabled in movement from 0 position and
        * the motor is not in home increase the counter
        */
        motor->stepsErrorHome++;
    else
        // clear error steps counter
        motor->stepsErrorHome = 0;

    // if this is the last consecutive position in which the motor is in/out home ...
    ASSERT(motor->stepsErrorHome <= MOTOR_CONSECUTIVE_ERROR_STEPS);
    if (motor->stepsErrorHome >= MOTOR_CONSECUTIVE_ERROR_STEPS)
    {
        // if the position at which the motor first saw/didn't see the home
        // is out of tolerance -> breakmotor -> ERROR
        if (motor->step > motor->stepsTollMax || motor->step < motor->stepsTollMin )
        {
            // break motor and error
            motor->speed = SPEED_STOPPED;
            motor->stepToReach = motor->step;

            stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
            motor->skipIrqs = MOTOR_SWITCH_COUNT;
            return false;
        }

        // the motor reached the home crossing -> disable error check
        setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
    }

    return true;
}

static void stepper_checkLevelSensor(struct Stepper* motor)
{
    // level sensor check
    if (motor->step > motor->stepsDeaf)
    {
        if (stepper_readLevel(motor))
        {
            // record current position, disable check and stop motor
            motor->stepsDeaf = DEAFSTEPS_DEFAULT;
            motor->stepsLevel = motor->step;
                        //motor->stepToReach = motor->step + motor->rampStep * motor->dir;

            motor->stepToReach = motor->step;
            motor->rampClock = motor->cfg->ramp.clocksMaxWL;
            motor->rampValue = motor->cfg->ramp.clocksMaxWL;
        }
    }
}

static enum StepperState FAST_FUNC FSM_run(struct Stepper *motor)
{
    uint16_t distance;

    if (!stepper_checkHomeErrors(motor))
        return MSTS_ERROR;

    stepper_checkLevelSensor(motor);

    if ((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
        (motor->stepToReach != STEPS_INFINITE_NEGATIVE ))
    {
        // Calculate (always positive) distance between current position and destination step
        distance = (uint16_t)((motor->stepToReach - motor->step) * motor->dir);
    }
    else
    {
        // We're at a very long distance ;-)
        distance = 0xFFFF;
        // if the motor is rotating and it has just ran a complete round
        // the position is set to 0
        if(motor->step == motor->stepCircular)
            motor->step = 0;
    }

    if (distance == 0)
        // Position reached - stop motor
        //motor->speed = SPEED_STOPPED;
        motor->rampStep = -1;
        //motor->rampClock = motor->ramp->clocksMaxWL;
        //motor->rampValue = 0;
        //motor->rampClock = motor->rampValue = motor->ramp->clocksMaxWL;

    else if (distance <= (uint16_t)motor->rampStep)
        stepper_decel(motor);

    // check whether the velocity must be changed
    else if (motor->speed < (uint16_t)motor->rampValue)
    {
        stepper_accel(motor);
        if (motor->speed > (uint16_t)motor->rampValue)
            motor->speed = (uint16_t)motor->rampValue;
    }
    else if (motor->speed > (uint16_t)motor->rampValue)
        stepper_decel(motor);

    // If rampStep == -1, leave output pin high and wait for low current
    if (motor->rampStep < 0)
    {
        // Wait before switching to low current
        motor->speed = SPEED_STOPPED;

        stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
        motor->skipIrqs = MOTOR_SWITCH_COUNT;

        /*
         * If there was a home sensor check activated, and the check has not
         * been done yet, it means that we reached the end position without
         * finding the home (or exiting from it). This is bad!
         */
        if (motor->enableCheckHome != MOTOR_HOMESENSOR_NOCHECK)
            return MSTS_ERROR;

        // check if the motor has to stay in high current
        if(motor->cfg->flags.highcurrentBit)
        {
            motor->changeCurrentIrqs = MOTOR_CURRENT_TICKS;
            return MSTS_IDLE;
        }

        return MSTS_PREIDLE;
    }

    // Wait for high->low transition
    ASSERT(motor->rampValue > motor->cfg->pulse);
    stepper_schedule_irq(motor, motor->rampValue, true);

    return MSTS_RUN;
}

static enum StepperState FSM_idle(struct Stepper* motor)
{
    stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);

    if (motor->speed == SPEED_STOPPED)
    {
        // check if it's time to switch to low current
        if(motor->changeCurrentIrqs > 0)
        {
            if(--motor->changeCurrentIrqs == 0)
                setLowCurrent(motor);
        }
        return MSTS_IDLE;
    }

    // Switch to high current and wait for stabilization
    // (if the motor is in low current)
    if(motor->changeCurrentIrqs == 0)
    {
        setHighCurrent(motor);
        motor->skipIrqs = MOTOR_SWITCH_COUNT;
    }

    return MSTS_PRERUN;
}

static enum StepperState FSM_preidle(struct Stepper* motor)
{
    // Normal operation mode
    motor->changeCurrentIrqs = 0;
    setLowCurrent(motor);
    stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
    return MSTS_IDLE;
}

static enum StepperState FSM_error(struct Stepper* motor)
{
    // Error condition mode
    setLowCurrent(motor);
    stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
    return MSTS_ERROR;
}

static enum StepperState FSM_prerun(struct Stepper* motor)
{
    enum MotorDirection dir;

    // distance != 0?
    if ((motor->stepToReach != motor->step) ||
        (motor->stepToReach == STEPS_INFINITE_POSITIVE) ||
        (motor->stepToReach == STEPS_INFINITE_NEGATIVE)  )
    {
        // Setup for first step
        motor->rampStep = 0;

        // Setup Direction
        if(motor->stepToReach == STEPS_INFINITE_POSITIVE)
            dir = DIR_POSITIVE;
        else if(motor->stepToReach == STEPS_INFINITE_NEGATIVE)
             dir = DIR_NEGATIVE;
        else if(motor->stepToReach > motor->step)
            dir = DIR_POSITIVE;
        else
             dir = DIR_NEGATIVE;

        setDirection(motor, dir);

        // Enable of the home sensor control, if necessary
        // (before calling this function set the motor direction as above)
        stepper_enableCheckHome(motor, (dir == DIR_POSITIVE));

        // if the movement is infinite negative set the sw direction positive
        // (not the hw: see below) to count the steps
        if(motor->stepToReach == STEPS_INFINITE_NEGATIVE) motor->dir = DIR_POSITIVE;

        stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
        return MSTS_RUN;
    }
    else
    {
        /*
         * If we are here we should do at least one step.
         *  anyway ....
         */
        stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
        motor->skipIrqs = MOTOR_SWITCH_COUNT;
        return MSTS_PREIDLE;
    }
}

static enum StepperState FSM_preinit(struct Stepper* motor)
{
    // Set current high, and wait for stabilization
    if (setHighCurrent(motor))
    {
        motor->skipIrqs = MOTOR_SWITCH_COUNT;
        return MSTS_PREINIT;
    }

    /*
     * This state is used when initializing the motor, to bring back
     * to the home. The idea is that we do not know where the motor
     * is at this point, so there can be two possibilities:
     *
     * - The motor is already in home. We do not know how much into the
     *   home we are. So we need to get out of the home (MSTS_LEAVING)
     *   and then get back into it of the desired number of steps.
     *
     * - The motor is not in home: we need to look for it (MSTS_INIT).
     *   We can safely assume that we will find the home in the negative
     *   direction. For circular motors, any direction would do. For
     *   other motors, the home is set at zero, so the current position
     *   has to be a positive value.
     *
     */
    if (stepper_readHome(motor))
    {
        setDirection(motor, DIR_POSITIVE);
        stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
        return MSTS_LEAVING;
    }

    setDirection(motor, DIR_NEGATIVE);
    stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
    return MSTS_INIT;
}


static enum StepperState FSM_init(struct Stepper* motor)
{
    // If we are not in home, keep looking
    if (!stepper_readHome(motor))
    {
        stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
        return MSTS_INIT;
    }

    /*
     * Home! We still need to enter the home of the specified number of steps.
     * That will be our absolute zero.
     */

    motor->step = motor->cfg->stepsInHome - 1; // start counting down steps in home
    motor->stepToReach = 0;

    stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
    return MSTS_ENTERING;
}

static enum StepperState FSM_entering(struct Stepper* motor)
{
    // We must be in home
    //ASSERT(stepper_readHome(motor));

    // if while entering the sensor we are no more in home we reset the steps
    // counter (optical sensor)
    if(!stepper_readHome(motor))
        motor->step = motor->cfg->stepsInHome - 1;

    // Current Position must be non-negative
    ASSERT(motor->step >= 0);

    if(motor->step == 0)
    {
        // reach the final target inside home sensor
        motor->step = 0;
        return MSTS_PREIDLE;
    }

    // keep doing steps
    stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
    return MSTS_ENTERING;
}

static enum StepperState FSM_leaving(struct Stepper* motor)
{
    ASSERT(motor->dir == DIR_POSITIVE);

    motor->step = 0;
    if (!stepper_readHome(motor))
    {
        // we are out of home : change state and going far from sensor
        stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
        return MSTS_OUTHOME;
    }
    else
    {
        // Still at home. Just wait here and keep doing steps
        stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
        return MSTS_LEAVING;
    }
}

static enum StepperState FSM_outhome(struct Stepper* motor)
{
    ASSERT(motor->dir == DIR_POSITIVE);

    // We must be out of home: once we are no more in home
    // we just need to move away, even if not very precide (optical sensor)
    // ASSERT(!stepper_readHome(motor));

    if(motor->step >= motor->cfg->stepsOutHome)
    {
        // reach the final target outside home sensor
        motor->step = 0;

        // start home entering procedure (delay in executing step)
        setDirection(motor, DIR_NEGATIVE);
        stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
        motor->skipIrqs = MOTOR_SWITCH_COUNT;
        return MSTS_INIT;
    }

    // keep doing steps
    stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
    return MSTS_OUTHOME;
}

static void FAST_FUNC stepper_interrupt(struct Stepper *motor)
{
    enum StepperState newState;

    // Check if we need to skip a certain number of IRQs
    if (motor->skipIrqs)
    {
        --motor->skipIrqs;
        stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
        return;
    }

    ASSERT(motor->state);
    newState = motor->state(motor);
    stepper_changeState(motor, newState);
}




/************************************************************************/
/* Public API                                                           */
/************************************************************************/

void stepper_init(void)
{
    STEPPER_INIT();

    // before starting the power all the stepper enable must be surely low
    stepper_disable();

    // Bind functions to general states
    memset(general_states, 0, sizeof(general_states));
    general_states[MSTS_IDLE] = FSM_idle;
    general_states[MSTS_PREIDLE] = FSM_preidle;
    general_states[MSTS_PRERUN] = FSM_prerun;
    general_states[MSTS_RUN] = FSM_run;
    general_states[MSTS_PREINIT] = FSM_preinit;
    general_states[MSTS_INIT] = FSM_init;
    general_states[MSTS_ENTERING] = FSM_entering;
    general_states[MSTS_LEAVING]= FSM_leaving;
    general_states[MSTS_OUTHOME]= FSM_outhome;
    general_states[MSTS_ERROR]= FSM_error;
}

void stepper_end(void)
{
    // Disable all stepper timer interrupt to stop motors
    for (int i = 0; i < CONFIG_NUM_STEPPER_MOTORS; i++)
        stepper_disable_irq(&all_motors[i]);
}

struct Stepper* stepper_setup(int index, struct StepperConfig *cfg)
{
    struct Stepper* motor;

    ASSERT(index < CONFIG_NUM_STEPPER_MOTORS);

    motor = &all_motors[index];
    motor->index = index;
    motor->cfg = cfg;

    //Register timer to stepper, and enable irq
    stepper_tc_setup(motor->index, &stepper_interrupt, motor);

    stepper_reset(motor);

    stepper_enable_irq(motor);

    return motor;
}

void stepper_disable(void)
{
    STEPPER_DISABLE_ALL();
}

void stepper_reset(struct Stepper *motor)
{
    /*
     * To stop motor diable stepper irq.
     */
    stepper_disable_irq(motor);

    //Disable a stepper motor
    STEPPER_DISABLE(MOTOR_INDEX(motor));

    // Setup context variables
    motor->power = 0;
    motor->step = 0;
    motor->rampStep = -1;
    // We cannot set the clock at zero at start because of a limit in the fixed point ramp
    motor->rampClock = motor->cfg->ramp.clocksMaxWL;
    motor->rampValue = motor->cfg->ramp.clocksMaxWL;
    motor->speed = SPEED_STOPPED;
    motor->stepToReach = 0;
    motor->skipIrqs = 0;
    motor->stepCircular = 0;
    setDirection(motor, DIR_POSITIVE);
    setLowCurrent(motor);

    motor->changeCurrentIrqs = 0;

    // default value (disable level sensor check)
    motor->stepsDeaf = DEAFSTEPS_DEFAULT;

    STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
    STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);

    if (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS)
        hw_home_sensor_set_inverted(motor->cfg->homeSensorIndex, motor->cfg->flags.homeInverted);

    if (motor->cfg->levelSensorIndex != MOTOR_NO_LEVEL_SENSOR)
        hw_level_sensor_set_inverted(motor->cfg->levelSensorIndex, motor->cfg->flags.levelInverted);

    stepper_changeState(motor, MSTS_IDLE);

    // Reset stepper timer counter
    stepper_tc_resetTimer(motor->timer);

    // reset hw to the stepper motor
    STEPPER_RESET(MOTOR_INDEX(motor));
    STEPPER_ENABLE(MOTOR_INDEX(motor));
}


void stepper_updateHalfStep(struct Stepper *motor)
{
    STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
}

void stepper_updateControlBit(struct Stepper *motor)
{
    STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
}

void stepper_updateControlMoveBit(struct Stepper *motor)
{
    STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlMoveBit);
}

void stepper_home(struct Stepper *motor)
{

    // Begin home procedure
    stepper_disable_irq(motor);

    // disable home sensor check (default)
    setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
    // deafult value (disable level sensor check)
    motor->stepsDeaf = DEAFSTEPS_DEFAULT;

    setDirection(motor, DIR_POSITIVE);
    stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
    stepper_changeState(motor, MSTS_PREINIT);

    stepper_enable_irq(motor);
}


void stepper_setStep(struct Stepper *motor, int16_t step)
{
    motor->step = step;
}


int16_t stepper_getStep(struct Stepper *motor)
{
    return motor->step;
}

int16_t stepper_getLevelStep(struct Stepper *motor)
{
    return motor->stepsLevel;
}

void stepper_set_stepCircular(struct Stepper *motor, int16_t steps)
{
    motor->stepCircular = steps;
}

int16_t stepper_get_stepCircular(struct Stepper *motor)
{
    return motor->stepCircular;
}

int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir)
{
    int16_t steps;

    // scale the current position inside the motor lap
    if(!motor->stepCircular) return 0;

    // to be sure ....
    while(motor->step > motor->stepCircular) motor->step -= motor->stepCircular;

    if(dir == DIR_NEGATIVE)
    {
        steps = ((motor->stepCircular - motor->step) % motor->stepCircular);
        motor->step = steps;
    }
    /*
    else
        steps = (motor->step % motor->stepCircular);
    motor->step = steps;
    */
    return motor->step;
}

static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive)
{
    enum MotorHomeSensorCheck value = MOTOR_HOMESENSOR_NOCHECK; // default

    motor->stepsTollMin = 0;

    if((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
        (motor->stepToReach != STEPS_INFINITE_NEGATIVE)  )
    {
        if(bDirPositive) // else if(motor->dir == DIR_POSITIVE)
        {
            /* if the direction is positive (movement from 0 position),
             * if the starting position is inside home and the target position
             * is outside home -> the motor has to cross the home sensor -> enable the control
             */
            if (motor->step < motor->cfg->stepsInHome - motor->cfg->stepsTollOutHome &&
                motor->stepToReach > motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome)
            {
                value = MOTOR_HOMESENSOR_OUTCHECK;
                // home sensor out max position
                motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome + MOTOR_CONSECUTIVE_ERROR_STEPS;
                // home sensor in max position
                if(motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS > motor->cfg->stepsTollOutHome)
                    motor->stepsTollMin = motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS - motor->cfg->stepsTollOutHome;
            }
        }
        else // if(motor->dir == DIR_NEGATIVE)
        {
            /*
             * if the direction is negative (movement to 0 position),
             * if the starting position is far from home and the target position
             * is inside home -> the motor has to cross the home sensor -> enable the control
             */
            if (motor->step > motor->cfg->stepsInHome + motor->cfg->stepsTollInHome &&
                motor->stepToReach < motor->cfg->stepsInHome - motor->cfg->stepsTollInHome)
            {
                value = MOTOR_HOMESENSOR_INCHECK;
                // home sensor out max position
                motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollInHome - MOTOR_CONSECUTIVE_ERROR_STEPS;
                // home sensor in max position
                if(motor->cfg->stepsInHome > motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS)
                    motor->stepsTollMin = motor->cfg->stepsInHome - (motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS);
            }
        }
    }
    setCheckSensor(motor, value);
}

int16_t stepper_move(struct Stepper *motor, int16_t steps, uint16_t speed, int16_t deafstep)
{
    // if the stepper already is in the desired position -> nothing to do
    if (motor->step == steps)
        return 0;

    stepper_disable_irq(motor);

    // final position
    motor->stepToReach = steps;

    // clear error steps
    motor->stepsErrorHome = 0;

    // position to start level check
    motor->stepsDeaf = deafstep;

    // clear level position
    motor->stepsLevel = 0;

    if (speed < motor->cfg->ramp.clocksMinWL)
    {
        ASSERT2(0, "speed too fast (small number)");
        speed = motor->cfg->ramp.clocksMinWL;
    }

    motor->rampClock = motor->cfg->ramp.clocksMaxWL;
    motor->rampValue = motor->cfg->ramp.clocksMaxWL;

    // TODO: find the exact value for motor->speed searching  in the ramp array.
    motor->speed = speed;

    stepper_enable_irq(motor);

    return 0;
}


void stepper_stop(struct Stepper *motor)
{
    /*
     * The best way is to set the target of the movement to the minimum
     * distance needed to decelerate. The logic in FSM_run will do the rest.
     */
    if(stepper_idle(motor))
        return;

    stepper_disable_irq(motor);
    motor->stepToReach = motor->step + motor->rampStep * motor->dir;
    stepper_enable_irq(motor);
}


void stepper_break(struct Stepper *motor, enum StepperState state)
{
    // The best way to abort any operation is to go back to pre-idle mode
    stepper_disable_irq(motor);

    // Set of Speed disabled and Steps reached so that the function
    // stepper_idle() succeeds
    motor->speed = SPEED_STOPPED;
    motor->stepToReach = motor->step;
    stepper_changeState(motor, state);
    stepper_enable_irq(motor);
}

//  this means anyway that the motor is not moving
bool stepper_idle(struct Stepper *motor)
{
    return (stepper_isState(motor, MSTS_ERROR) ||
            (stepper_isState(motor, MSTS_IDLE) && motor->step == motor->stepToReach) );
}

bool stepper_error(struct Stepper *motor)
{
    return (stepper_isState(motor, MSTS_ERROR));
}

bool stepper_inhome(struct Stepper *motor)
{
    return(stepper_getStep(motor) == 0 &&
           !stepper_readHome(motor) );
}