--- /dev/null
+/**
+ * \file
+ * <!--
+ * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Driver to control stepper motor
+ *
+ * \version $Id$
+ *
+ * \author Francesco Michelini <francesco.michelini@seacfi.com>
+ * \author Giovanni Bajo <rasky@develer.com>
+ * \author Bernardo Innocenti <bernie@develer.com>
+ * \author Simone Zinanni <s.zinanni@develer.com>
+ * \author Daniele Basile <asterix@develer.com>
+ */
+
+#include "stepper.h"
+
+#include <cfg/debug.h>
+#include <kern/proc.h>
+#include <algo/ramp.h>
+#include <drv/stepper_at91.h>
+
+#include <hw/hw_stepper.h>
+#include <hw/hw_sensor.h>
+
+#include <string.h> // 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) );
+}
--- /dev/null
+/**
+ * \file
+ * <!--
+ * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Driver to control stepper motor
+ *
+ * \version $Id$
+ *
+ * \author Francesco Michelini <francesco.michelini@seacfi.com>
+ * \author Giovanni Bajo <rasky@develer.com>
+ * \author Bernardo Innocenti <bernie@develer.com>
+ * \author Simone Zinanni <s.zinanni@develer.com>
+ * \author Daniele Basile <asterix@develer.com>
+ *
+ */
+
+#ifndef DRV_STEPPER_H
+#define DRV_STEPPER_H
+
+#include <cfg/compiler.h>
+#include <algo/ramp.h>
+
+// 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 */
projects / bertos.git / commitdiff