From: asterix Date: Fri, 2 May 2008 11:51:11 +0000 (+0000) Subject: Add stepper driver module. X-Git-Tag: 1.0.0~9 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;h=fb7ec340d92c933b7c60e0c097992e733462b1d4;p=bertos.git Add stepper driver module. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@1243 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/bertos/drv/stepper.c b/bertos/drv/stepper.c new file mode 100644 index 00000000..716b094f --- /dev/null +++ b/bertos/drv/stepper.c @@ -0,0 +1,952 @@ +/** + * \file + * + * + * \brief Driver to control stepper motor + * + * \version $Id$ + * + * \author Francesco Michelini + * \author Giovanni Bajo + * \author Bernardo Innocenti + * \author Simone Zinanni + * \author Daniele Basile + */ + +#include "stepper.h" + +#include +#include +#include +#include + +#include +#include + +#include // memset + +#include "appconfig.h" + +/** + * \name Motor timings + * \{ + */ +#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 +// \} + +///< Stepper motors +static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS]; + +///< General FSM states (or NULL if state is not handled) +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)); + } +} + +/** + * Schedule a new stepper IRQ to happen after \a delay (number of clocks), + * and optionally doing a step at the same time (if \a do_step is true). + */ +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) +{ +#ifdef _DEBUG + uint16_t old_val = motor->rampValue; + uint32_t old_clock = motor->rampClock; +#endif + + const struct Ramp *ramp = &motor->cfg->ramp; + + ASSERT(motor->rampClock != 0); + + motor->rampValue = ramp_evaluate(ramp, motor->rampClock); + motor->rampClock += motor->rampValue; + motor->rampStep++; + +#ifdef _DEBUG + if (old_val && motor->rampValue > old_val) + { + kprintf("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL); + kprintf(" %04x @ %lu --> %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock); + } +#endif +} + +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 <= 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 */ +/************************************************************************/ + +/** + * Initialize the stepper module + */ +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]); +} + +/** + * Apply a setup config to motor structure context + */ +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; +} + +/** + * Set the enable for all the motors to 0 before switching on the power + */ +void stepper_disable(void) +{ + STEPPER_DISABLE_ALL(); +} + +/** + * Reset the motor + */ +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); +} + +/** + * Find the home of a \a motor assuming no current knowledge about its position. + * + * This must be done when the motor is desynchronized with the firmware and + * we do not know anymore where it is. + * + * In normal operation mode, to go back to the home, it is sufficient to use + * move to step #0 with stepper_move, since the home is always at step #0. + */ +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); +} + +/** + * Move motor to absolute position at specified speed + * + * \arg steps position to reach in steps + * \arg speed speed in timer ticks (use TIME2CLOCKS() to convert) + */ +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; +} + + +/** + * Stop motor gracefully + */ +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); +} + + +/** + * Stop motor immediately, changing the status + */ +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); +} + +///< Returns true if the stepper is in idle at the final position or in error: +// 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) ); +} + +///< Returns true if the stepper is in error mode +bool stepper_error(struct Stepper *motor) +{ + return (stepper_isState(motor, MSTS_ERROR)); +} + +///< check the home sensor in zero position +bool stepper_inhome(struct Stepper *motor) +{ + return(stepper_getStep(motor) == 0 && + !stepper_readHome(motor) ); +} diff --git a/bertos/drv/stepper.h b/bertos/drv/stepper.h new file mode 100644 index 00000000..5f74b9b1 --- /dev/null +++ b/bertos/drv/stepper.h @@ -0,0 +1,222 @@ +/** + * \file + * + * + * \brief Driver to control stepper motor + * + * \version $Id$ + * + * \author Francesco Michelini + * \author Giovanni Bajo + * \author Bernardo Innocenti + * \author Simone Zinanni + * \author Daniele Basile + * + */ + +#ifndef DRV_STEPPER_H +#define DRV_STEPPER_H + +#include +#include + +// Forward declaration +struct Stepper; + +///< Special value for steps to move the motor continuously +#define STEPS_INFINITE_POSITIVE ((int16_t)0xFFFF) +#define STEPS_INFINITE_NEGATIVE ((int16_t)0x8FFF) + +///< Maximum value for stepper steps +#define MAX_STEPS 0x7FFF + +///< Default value -> no level sensor associated to the motor +#define MOTOR_NO_LEVEL_SENSOR 0xFFFF + +///< Default value -> no home sensor associated to the motor +#define MOTOR_NO_HOME_SENSOR 0xFFFF + +///< Default value for deafsteps in normal movement (no level sensor) +#define DEAFSTEPS_DEFAULT MAX_STEPS + +///< Out-of-band values for speed +//\{ +#define SPEED_STOPPED 0xFFFF ///< motor is stopped +#define SPEED_HOMING 0xFFFE ///< motor is homing +//\} + +// default values for steps inside and outside home sensor +#define MOTOR_INSIDE_HOME_STEPS 10 +#define MOTOR_OUTSIDE_HOME_STEPS 40 + +// default value for home sensor tolerance +#define MOTOR_TOLERANCE_HOME_STEPS 2 + +// default value for consecutive error +#define MOTOR_CONSECUTIVE_ERROR_STEPS 3 + +// values for the home control enabling +enum MotorHomeSensorCheck +{ + MOTOR_HOMESENSOR_NOCHECK = 0, + MOTOR_HOMESENSOR_INCHECK, + MOTOR_HOMESENSOR_OUTCHECK +}; + +// default value in ms for home procedure timeout +#define MOTOR_TIMEOUT_HOME 20000 + +/** + * Motor direction + */ +enum MotorDirection +{ + DIR_POSITIVE = 1, ///< moving away from zero (which is the home) + DIR_NONE = 0, ///< no movement + DIR_NEGATIVE = -1 ///< moving towards towards zero (which is the home) +}; + +#define STEPPER_MAX_STATES 32 + + +/** + * Stepper state-machine conditions + */ +enum StepperState +{ + MSTS_UNINIT, ///< stepper_init() not yet called + MSTS_RUN, ///< running + MSTS_IDLE, ///< waiting for a command + MSTS_PREIDLE, ///< waiting before going low-current + MSTS_PRERUN, ///< waiting after high-current + +// Home procedure + MSTS_PREINIT, ///< preparing to initialize ;-) + MSTS_INIT, ///< initializing home procedure + MSTS_ENTERING, ///< entering home sensor + MSTS_LEAVING, ///< moving away from home (inside the sensor) + MSTS_OUTHOME, ///< moving away from home (outside the sensor) + + MSTS_ERROR, ///< error status + + ///< Dummy entry to guarantee the right underlying size for the enum + MSTS_DUMMY_ALIGN = STEPPER_MAX_STATES - 1 +}; + +///< Pointer to a function handling a state of the FSM driving the motor +typedef enum StepperState (*fsm_state)(struct Stepper* ); + +///< Pointer to a isr stepper function +typedef void (*stepper_isr_t)(struct Stepper* ); + +///< Time for steppers motor +typedef uint16_t stepper_time_t; + +/** + * Stepper configuration + */ +struct StepperConfig +{ + struct Ramp ramp; ///< Acceleration ramp + uint16_t pulse; ///< (clocks) Length of the clock pulse used to drive the motor + + fsm_state states[STEPPER_MAX_STATES]; ///< Custom FSM states (or NULL for default handling) + + int16_t stepsInHome; ///< Additional steps to do after home detection + int16_t stepsOutHome; ///< Additional steps to do leaving sensor in home procedure + uint16_t clocksHome; ///< Clock ticks for steps done when searching home + + int16_t stepsTollOutHome; ///< tolerance steps leaving home sensor control while moving + int16_t stepsTollInHome; ///< tolerance steps leaving home sensor control while moving + + int16_t timeoutHome; ///< timeout in ms in home procedure + + uint8_t powerRun; ///< Vref voltage when motor runs (0-255) + uint8_t powerIdle; ///< Vref voltage when motor is idle (0-255) + + uint16_t homeSensorIndex; ///< Home Sensor index in the sensor list + uint16_t levelSensorIndex; ///< Level Sensor index in the sensor list + + struct + { + bool homeInverted : 1; ///< True for inverted home sensor + bool halfStep : 1; ///< True for half-step mode + bool axisInverted : 1; ///< True if the CW/CCW are inverted from default + bool levelInverted : 1; ///< True for inverted level sensor + bool controlBit : 1; ///< Control bit status + bool controlMoveBit : 1; ///< Control bit status in movement + bool highcurrentBit : 1; ///< Mantain high current bit status + } flags; +}; + + +/** + * Motor context structure + */ +struct Stepper +{ + const struct StepperConfig *cfg; ///< Configuration of this stepper + fsm_state state; ///< Motor FSM state function + + struct TimerCounter *timer; ///< HW timer bound to this motor + uint16_t index; ///< Index of the motor + + volatile int16_t step; ///< Steps counter (used in interrupt) + volatile int16_t rampStep; ///< Current position in acceleration ramp (used in intrrupt) +#if RAMP_USE_FLOATING_POINT + float rampValue; ///< Nr of Ticks for current step in ramp + float rampClock; ///< Cumulative nr of ticks for current step in ramp +#else + uint16_t rampValue; + uint32_t rampClock; +#endif + + enum MotorDirection dir; ///< Current direction + uint8_t power; ///< Current power + + uint16_t speed; ///< Timer compare value to reach + int16_t stepToReach; ///< Final position to reach when running + + int16_t skipIrqs; ///< Counter used to skip IRQs (delay state changes) + int16_t changeCurrentIrqs; ///< Counter used to change current level (delay state changes) + + int8_t enableCheckHome; ///< enable the home sensor control during movement + int8_t stepsErrorHome; ///< number of consecutive steps in error + int16_t stepsTollMax; ///< home sensor out max position + int16_t stepsTollMin; ///< home sensor in max position + + int16_t stepsDeaf; ///< Position after which start the level check + int16_t stepsLevel; ///< Position of level contact + + int16_t stepCircular; ///< Steps corresponding to 360 degrees (rotating motor) +}; + + +void stepper_init(void); +void stepper_end(void); +struct Stepper *stepper_setup(int index, struct StepperConfig *cfg); +void stepper_disable(void); +void stepper_reset(struct Stepper *motor); +void stepper_home(struct Stepper *motor); +void stepper_setStep(struct Stepper *motor, int16_t step); +int16_t stepper_getStep(struct Stepper *motor); +int16_t stepper_move(struct Stepper *motor, int16_t step, uint16_t speed, int16_t deafstep); +void stepper_stop(struct Stepper *motor); +void stepper_break(struct Stepper *motor, enum StepperState state); +bool stepper_idle(struct Stepper *motor); +bool stepper_readHome(struct Stepper *motor); +bool stepper_readLevel(struct Stepper *motor); +void stepper_updateHalfStep(struct Stepper *motor); +void stepper_updateControlBit(struct Stepper *motor); +void stepper_updateControlMoveBit(struct Stepper *motor); +bool stepper_error(struct Stepper *motor); +bool stepper_inhome(struct Stepper *motor); +int16_t stepper_getLevelStep(struct Stepper *motor); +void stepper_set_stepCircular(struct Stepper *motor, int16_t steps); +int16_t stepper_get_stepCircular(struct Stepper *motor); +int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir); + +#endif /* DRV_STEPPER_H */