4 * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
8 * \brief Driver to control stepper motor
12 * \author Francesco Michelini <francesco.michelini@seacfi.com>
13 * \author Giovanni Bajo <rasky@develer.com>
14 * \author Bernardo Innocenti <bernie@develer.com>
15 * \author Simone Zinanni <s.zinanni@develer.com>
16 * \author Daniele Basile <asterix@develer.com>
21 #include "hw/hw_stepper.h"
22 #include "hw/hw_sensor.h"
24 #include "cfg/cfg_stepper.h"
25 #include <cfg/debug.h>
27 // Define logging setting (for cfg/log.h module).
28 #define LOG_LEVEL STEPPER_LOG_LEVEL
29 #define LOG_VERBOSITY STEPPER_LOG_VERBOSITY
32 #include <kern/proc.h>
34 #include <algo/ramp.h>
36 #include CPU_HEADER(stepper)
38 #include <string.h> // memset
44 #define MOTOR_SWITCH_TICKS 60000 ///< Timer ticks to wait for 10ms
45 #define MOTOR_SWITCH_COUNT 5 ///< Number of intervals, long 10ms, to wait before/after switching current off/on
46 #define MOTOR_HOME_MAX_STEPS 30000 ///< Steps before giving up when trying to reach home
47 #define MOTOR_CURRENT_TICKS 6000 ///< Number of intervals, long 10ms, to mantain high current
51 static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS];
53 ///< General FSM states (or NULL if state is not handled)
54 static fsm_state general_states[STEPPER_MAX_STATES];
56 // IRQ functions for stepper motors
57 static void stepper_interrupt(struct Stepper *motor);
59 static void stepper_accel(struct Stepper *motor);
60 static void stepper_decel(struct Stepper *motor);
62 static bool stepper_isState(struct Stepper *motor, enum StepperState state);
63 INLINE void stepper_changeState(struct Stepper *motor, enum StepperState newState);
65 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive);
67 #define MOTOR_INDEX(motor) (motor->index)
69 //------------------------------------------------------------------------
71 INLINE bool setLowCurrent(struct Stepper* motor)
73 if (motor->power == motor->cfg->powerIdle)
76 motor->power = motor->cfg->powerIdle;
77 STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerIdle);
82 INLINE bool setHighCurrent(struct Stepper* motor)
84 if (motor->power == motor->cfg->powerRun)
87 motor->power = motor->cfg->powerRun;
88 STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerRun);
92 INLINE void setCheckSensor(struct Stepper* motor, enum MotorHomeSensorCheck value)
94 motor->enableCheckHome = value;
97 INLINE int8_t getCheckSensor(struct Stepper* motor)
99 return motor->enableCheckHome;
102 INLINE void setDirection(struct Stepper* motor, enum MotorDirection dir)
104 ASSERT(dir == DIR_POSITIVE || dir == DIR_NEGATIVE);
107 if (!motor->cfg->flags.axisInverted)
109 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir == DIR_POSITIVE));
113 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir != DIR_POSITIVE));
118 * Schedule a new stepper IRQ to happen after \a delay (number of clocks),
119 * and optionally doing a step at the same time (if \a do_step is true).
121 INLINE void FAST_FUNC stepper_schedule_irq(struct Stepper* motor, stepper_time_t delay, bool do_step)
126 // Record the step we just did
127 motor->step += motor->dir;
128 stepper_tc_doPulse(motor->timer);
131 stepper_tc_skipPulse(motor->timer);
133 stepper_tc_setDelay(motor->timer, delay);
137 static void stepper_accel(struct Stepper *motor)
139 DB(uint16_t old_val = motor->rampValue;)
140 DB(uint32_t old_clock = motor->rampClock;)
142 const struct Ramp *ramp = &motor->cfg->ramp;
144 ASSERT(motor->rampClock != 0);
146 motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
147 motor->rampClock += motor->rampValue;
150 DB(if (old_val && motor->rampValue > old_val)
152 LOG_ERR("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL);
153 LOG_ERR(" %04x @ %lu --> %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock);
158 static void stepper_decel(struct Stepper *motor)
160 const struct Ramp *ramp = &motor->cfg->ramp;
161 DB(uint16_t old_val = motor->rampValue;)
163 motor->rampClock -= motor->rampValue;
164 ASSERT(motor->rampClock != 0);
165 motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
167 DB(ASSERT(!old_val || motor->rampValue >= old_val););
170 INLINE void stepper_enable_irq(struct Stepper* motor)
172 stepper_tc_irq_enable(motor->timer);
175 INLINE void stepper_disable_irq(struct Stepper* motor)
177 stepper_tc_irq_disable(motor->timer);
180 // the home sensor can be in the standard home list or in the digital
182 bool stepper_readHome(struct Stepper* motor)
184 return (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS) ?
185 hw_home_sensor_read(motor->cfg->homeSensorIndex) :
186 bld_hw_sensor_read(motor->cfg->homeSensorIndex - NUM_HOME_SENSORS);
189 bool stepper_readLevel(struct Stepper* motor)
191 return hw_level_sensor_read(motor->cfg->levelSensorIndex);
194 /************************************************************************/
195 /* Finite-state machine to drive stepper logic from IRQ */
196 /************************************************************************/
198 INLINE void stepper_changeState(struct Stepper* motor, enum StepperState newState)
200 ASSERT(newState < STEPPER_MAX_STATES);
202 motor->state = motor->cfg->states[newState];
204 motor->state = general_states[newState];
205 ASSERT(motor->state);
208 static bool stepper_isState(struct Stepper* motor, enum StepperState state)
210 return (motor->cfg->states[state]
211 ? motor->cfg->states[state] == motor->state
212 : general_states[state] == motor->state);
215 static bool stepper_checkHomeErrors(struct Stepper* motor)
219 home = stepper_readHome(motor);
221 if (motor->enableCheckHome == MOTOR_HOMESENSOR_INCHECK && home
222 && (!motor->stepCircular || motor->step < motor->stepCircular / 2))
224 * if home Sensor check enabled in movement to 0 position and
225 * the motor is in home increase the counter
226 * for rotating motor we include the check that the motor is
227 * inside the last "lap" (FIXME: check it better)
229 motor->stepsErrorHome++;
230 else if (motor->enableCheckHome == MOTOR_HOMESENSOR_OUTCHECK && !home)
232 * if home Sensor check enabled in movement from 0 position and
233 * the motor is not in home increase the counter
235 motor->stepsErrorHome++;
237 // clear error steps counter
238 motor->stepsErrorHome = 0;
240 // if this is the last consecutive position in which the motor is in/out home ...
241 ASSERT(motor->stepsErrorHome <= MOTOR_CONSECUTIVE_ERROR_STEPS);
242 if (motor->stepsErrorHome >= MOTOR_CONSECUTIVE_ERROR_STEPS)
244 // if the position at which the motor first saw/didn't see the home
245 // is out of tolerance -> breakmotor -> ERROR
246 if (motor->step > motor->stepsTollMax || motor->step < motor->stepsTollMin )
248 // break motor and error
249 motor->speed = SPEED_STOPPED;
250 motor->stepToReach = motor->step;
252 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
253 motor->skipIrqs = MOTOR_SWITCH_COUNT;
257 // the motor reached the home crossing -> disable error check
258 setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
264 static void stepper_checkLevelSensor(struct Stepper* motor)
266 // level sensor check
267 if (motor->step > motor->stepsDeaf)
269 if (stepper_readLevel(motor))
271 // record current position, disable check and stop motor
272 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
273 motor->stepsLevel = motor->step;
274 //motor->stepToReach = motor->step + motor->rampStep * motor->dir;
276 motor->stepToReach = motor->step;
277 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
278 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
283 static enum StepperState FAST_FUNC FSM_run(struct Stepper *motor)
287 if (!stepper_checkHomeErrors(motor))
290 stepper_checkLevelSensor(motor);
292 if ((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
293 (motor->stepToReach != STEPS_INFINITE_NEGATIVE ))
295 // Calculate (always positive) distance between current position and destination step
296 distance = (uint16_t)((motor->stepToReach - motor->step) * motor->dir);
300 // We're at a very long distance ;-)
302 // if the motor is rotating and it has just ran a complete round
303 // the position is set to 0
304 if(motor->step == motor->stepCircular)
309 // Position reached - stop motor
310 //motor->speed = SPEED_STOPPED;
311 motor->rampStep = -1;
312 //motor->rampClock = motor->ramp->clocksMaxWL;
313 //motor->rampValue = 0;
314 //motor->rampClock = motor->rampValue = motor->ramp->clocksMaxWL;
316 else if (distance <= (uint16_t)motor->rampStep)
317 stepper_decel(motor);
319 // check whether the velocity must be changed
320 else if (motor->speed < (uint16_t)motor->rampValue)
322 stepper_accel(motor);
323 if (motor->speed > (uint16_t)motor->rampValue)
324 motor->speed = (uint16_t)motor->rampValue;
326 else if (motor->speed > (uint16_t)motor->rampValue)
327 stepper_decel(motor);
329 // If rampStep == -1, leave output pin high and wait for low current
330 if (motor->rampStep < 0)
332 // Wait before switching to low current
333 motor->speed = SPEED_STOPPED;
335 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
336 motor->skipIrqs = MOTOR_SWITCH_COUNT;
339 * If there was a home sensor check activated, and the check has not
340 * been done yet, it means that we reached the end position without
341 * finding the home (or exiting from it). This is bad!
343 if (motor->enableCheckHome != MOTOR_HOMESENSOR_NOCHECK)
346 // check if the motor has to stay in high current
347 if(motor->cfg->flags.highcurrentBit)
349 motor->changeCurrentIrqs = MOTOR_CURRENT_TICKS;
356 // Wait for high->low transition
357 ASSERT(motor->rampValue > motor->cfg->pulse);
358 stepper_schedule_irq(motor, motor->rampValue, true);
363 static enum StepperState FSM_idle(struct Stepper* motor)
365 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
367 if (motor->speed == SPEED_STOPPED)
369 // check if it's time to switch to low current
370 if(motor->changeCurrentIrqs > 0)
372 if(--motor->changeCurrentIrqs == 0)
373 setLowCurrent(motor);
378 // Switch to high current and wait for stabilization
379 // (if the motor is in low current)
380 if(motor->changeCurrentIrqs == 0)
382 setHighCurrent(motor);
383 motor->skipIrqs = MOTOR_SWITCH_COUNT;
389 static enum StepperState FSM_preidle(struct Stepper* motor)
391 // Normal operation mode
392 motor->changeCurrentIrqs = 0;
393 setLowCurrent(motor);
394 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
398 static enum StepperState FSM_error(struct Stepper* motor)
400 // Error condition mode
401 setLowCurrent(motor);
402 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
406 static enum StepperState FSM_prerun(struct Stepper* motor)
408 enum MotorDirection dir;
411 if ((motor->stepToReach != motor->step) ||
412 (motor->stepToReach == STEPS_INFINITE_POSITIVE) ||
413 (motor->stepToReach == STEPS_INFINITE_NEGATIVE) )
415 // Setup for first step
419 if(motor->stepToReach == STEPS_INFINITE_POSITIVE)
421 else if(motor->stepToReach == STEPS_INFINITE_NEGATIVE)
423 else if(motor->stepToReach > motor->step)
428 setDirection(motor, dir);
430 // Enable of the home sensor control, if necessary
431 // (before calling this function set the motor direction as above)
432 stepper_enableCheckHome(motor, (dir == DIR_POSITIVE));
434 // if the movement is infinite negative set the sw direction positive
435 // (not the hw: see below) to count the steps
436 if(motor->stepToReach == STEPS_INFINITE_NEGATIVE) motor->dir = DIR_POSITIVE;
438 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
444 * If we are here we should do at least one step.
447 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
448 motor->skipIrqs = MOTOR_SWITCH_COUNT;
453 static enum StepperState FSM_preinit(struct Stepper* motor)
455 // Set current high, and wait for stabilization
456 if (setHighCurrent(motor))
458 motor->skipIrqs = MOTOR_SWITCH_COUNT;
463 * This state is used when initializing the motor, to bring back
464 * to the home. The idea is that we do not know where the motor
465 * is at this point, so there can be two possibilities:
467 * - The motor is already in home. We do not know how much into the
468 * home we are. So we need to get out of the home (MSTS_LEAVING)
469 * and then get back into it of the desired number of steps.
471 * - The motor is not in home: we need to look for it (MSTS_INIT).
472 * We can safely assume that we will find the home in the negative
473 * direction. For circular motors, any direction would do. For
474 * other motors, the home is set at zero, so the current position
475 * has to be a positive value.
478 if (stepper_readHome(motor))
480 setDirection(motor, DIR_POSITIVE);
481 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
485 setDirection(motor, DIR_NEGATIVE);
486 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
491 static enum StepperState FSM_init(struct Stepper* motor)
493 // If we are not in home, keep looking
494 if (!stepper_readHome(motor))
496 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
501 * Home! We still need to enter the home of the specified number of steps.
502 * That will be our absolute zero.
505 motor->step = motor->cfg->stepsInHome - 1; // start counting down steps in home
506 motor->stepToReach = 0;
508 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
509 return MSTS_ENTERING;
512 static enum StepperState FSM_entering(struct Stepper* motor)
514 // We must be in home
515 //ASSERT(stepper_readHome(motor));
517 // if while entering the sensor we are no more in home we reset the steps
518 // counter (optical sensor)
519 if(!stepper_readHome(motor))
520 motor->step = motor->cfg->stepsInHome - 1;
522 // Current Position must be non-negative
523 ASSERT(motor->step >= 0);
527 // reach the final target inside home sensor
533 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
534 return MSTS_ENTERING;
537 static enum StepperState FSM_leaving(struct Stepper* motor)
539 ASSERT(motor->dir == DIR_POSITIVE);
542 if (!stepper_readHome(motor))
544 // we are out of home : change state and going far from sensor
545 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
550 // Still at home. Just wait here and keep doing steps
551 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
556 static enum StepperState FSM_outhome(struct Stepper* motor)
558 ASSERT(motor->dir == DIR_POSITIVE);
560 // We must be out of home: once we are no more in home
561 // we just need to move away, even if not very precide (optical sensor)
562 // ASSERT(!stepper_readHome(motor));
564 if(motor->step >= motor->cfg->stepsOutHome)
566 // reach the final target outside home sensor
569 // start home entering procedure (delay in executing step)
570 setDirection(motor, DIR_NEGATIVE);
571 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
572 motor->skipIrqs = MOTOR_SWITCH_COUNT;
577 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
581 static void FAST_FUNC stepper_interrupt(struct Stepper *motor)
583 enum StepperState newState;
585 // Check if we need to skip a certain number of IRQs
589 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
593 ASSERT(motor->state);
594 newState = motor->state(motor);
595 stepper_changeState(motor, newState);
601 /************************************************************************/
603 /************************************************************************/
606 * Initialize the stepper module
608 void stepper_init(void)
612 // before starting the power all the stepper enable must be surely low
615 // Bind functions to general states
616 memset(general_states, 0, sizeof(general_states));
617 general_states[MSTS_IDLE] = FSM_idle;
618 general_states[MSTS_PREIDLE] = FSM_preidle;
619 general_states[MSTS_PRERUN] = FSM_prerun;
620 general_states[MSTS_RUN] = FSM_run;
621 general_states[MSTS_PREINIT] = FSM_preinit;
622 general_states[MSTS_INIT] = FSM_init;
623 general_states[MSTS_ENTERING] = FSM_entering;
624 general_states[MSTS_LEAVING]= FSM_leaving;
625 general_states[MSTS_OUTHOME]= FSM_outhome;
626 general_states[MSTS_ERROR]= FSM_error;
629 void stepper_end(void)
631 // Disable all stepper timer interrupt to stop motors
632 for (int i = 0; i < CONFIG_NUM_STEPPER_MOTORS; i++)
633 stepper_disable_irq(&all_motors[i]);
637 * Apply a setup config to motor structure context
639 struct Stepper* stepper_setup(int index, struct StepperConfig *cfg)
641 struct Stepper* motor;
643 ASSERT(index < CONFIG_NUM_STEPPER_MOTORS);
645 motor = &all_motors[index];
646 motor->index = index;
649 //Register timer to stepper, and enable irq
650 stepper_tc_setup(motor->index, &stepper_interrupt, motor);
652 stepper_reset(motor);
654 stepper_enable_irq(motor);
660 * Set the enable for all the motors to 0 before switching on the power
662 void stepper_disable(void)
664 STEPPER_DISABLE_ALL();
670 void stepper_reset(struct Stepper *motor)
673 * To stop motor diable stepper irq.
675 stepper_disable_irq(motor);
677 //Disable a stepper motor
678 STEPPER_DISABLE(MOTOR_INDEX(motor));
680 // Setup context variables
683 motor->rampStep = -1;
684 // We cannot set the clock at zero at start because of a limit in the fixed point ramp
685 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
686 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
687 motor->speed = SPEED_STOPPED;
688 motor->stepToReach = 0;
690 motor->stepCircular = 0;
691 setDirection(motor, DIR_POSITIVE);
692 setLowCurrent(motor);
694 motor->changeCurrentIrqs = 0;
696 // default value (disable level sensor check)
697 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
699 STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
700 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
702 if (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS)
703 hw_home_sensor_set_inverted(motor->cfg->homeSensorIndex, motor->cfg->flags.homeInverted);
705 if (motor->cfg->levelSensorIndex != MOTOR_NO_LEVEL_SENSOR)
706 hw_level_sensor_set_inverted(motor->cfg->levelSensorIndex, motor->cfg->flags.levelInverted);
708 stepper_changeState(motor, MSTS_IDLE);
710 // Reset stepper timer counter
711 stepper_tc_resetTimer(motor->timer);
713 // reset hw to the stepper motor
714 STEPPER_RESET(MOTOR_INDEX(motor));
715 STEPPER_ENABLE(MOTOR_INDEX(motor));
719 void stepper_updateHalfStep(struct Stepper *motor)
721 STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
724 void stepper_updateControlBit(struct Stepper *motor)
726 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
729 void stepper_updateControlMoveBit(struct Stepper *motor)
731 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlMoveBit);
735 * Find the home of a \a motor assuming no current knowledge about its position.
737 * This must be done when the motor is desynchronized with the firmware and
738 * we do not know anymore where it is.
740 * In normal operation mode, to go back to the home, it is sufficient to use
741 * move to step #0 with stepper_move, since the home is always at step #0.
743 void stepper_home(struct Stepper *motor)
746 // Begin home procedure
747 stepper_disable_irq(motor);
749 // disable home sensor check (default)
750 setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
751 // deafult value (disable level sensor check)
752 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
754 setDirection(motor, DIR_POSITIVE);
755 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
756 stepper_changeState(motor, MSTS_PREINIT);
758 stepper_enable_irq(motor);
762 void stepper_setStep(struct Stepper *motor, int16_t step)
768 int16_t stepper_getStep(struct Stepper *motor)
773 int16_t stepper_getLevelStep(struct Stepper *motor)
775 return motor->stepsLevel;
778 void stepper_set_stepCircular(struct Stepper *motor, int16_t steps)
780 motor->stepCircular = steps;
783 int16_t stepper_get_stepCircular(struct Stepper *motor)
785 return motor->stepCircular;
788 int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir)
792 // scale the current position inside the motor lap
793 if(!motor->stepCircular) return 0;
796 while(motor->step > motor->stepCircular) motor->step -= motor->stepCircular;
798 if(dir == DIR_NEGATIVE)
800 steps = ((motor->stepCircular - motor->step) % motor->stepCircular);
805 steps = (motor->step % motor->stepCircular);
811 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive)
813 enum MotorHomeSensorCheck value = MOTOR_HOMESENSOR_NOCHECK; // default
815 motor->stepsTollMin = 0;
817 if((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
818 (motor->stepToReach != STEPS_INFINITE_NEGATIVE) )
820 if(bDirPositive) // else if(motor->dir == DIR_POSITIVE)
822 /* if the direction is positive (movement from 0 position),
823 * if the starting position is inside home and the target position
824 * is outside home -> the motor has to cross the home sensor -> enable the control
826 if (motor->step < motor->cfg->stepsInHome - motor->cfg->stepsTollOutHome &&
827 motor->stepToReach > motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome)
829 value = MOTOR_HOMESENSOR_OUTCHECK;
830 // home sensor out max position
831 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome + MOTOR_CONSECUTIVE_ERROR_STEPS;
832 // home sensor in max position
833 if(motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS > motor->cfg->stepsTollOutHome)
834 motor->stepsTollMin = motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS - motor->cfg->stepsTollOutHome;
837 else // if(motor->dir == DIR_NEGATIVE)
840 * if the direction is negative (movement to 0 position),
841 * if the starting position is far from home and the target position
842 * is inside home -> the motor has to cross the home sensor -> enable the control
844 if (motor->step > motor->cfg->stepsInHome + motor->cfg->stepsTollInHome &&
845 motor->stepToReach < motor->cfg->stepsInHome - motor->cfg->stepsTollInHome)
847 value = MOTOR_HOMESENSOR_INCHECK;
848 // home sensor out max position
849 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollInHome - MOTOR_CONSECUTIVE_ERROR_STEPS;
850 // home sensor in max position
851 if(motor->cfg->stepsInHome > motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS)
852 motor->stepsTollMin = motor->cfg->stepsInHome - (motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS);
856 setCheckSensor(motor, value);
860 * Move motor to absolute position at specified speed
862 * \arg steps position to reach in steps
863 * \arg speed speed in timer ticks (use TIME2CLOCKS() to convert)
865 int16_t stepper_move(struct Stepper *motor, int16_t steps, uint16_t speed, int16_t deafstep)
867 // if the stepper already is in the desired position -> nothing to do
868 if (motor->step == steps)
871 stepper_disable_irq(motor);
874 motor->stepToReach = steps;
877 motor->stepsErrorHome = 0;
879 // position to start level check
880 motor->stepsDeaf = deafstep;
882 // clear level position
883 motor->stepsLevel = 0;
885 if (speed < motor->cfg->ramp.clocksMinWL)
887 ASSERT2(0, "speed too fast (small number)");
888 speed = motor->cfg->ramp.clocksMinWL;
891 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
892 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
894 // TODO: find the exact value for motor->speed searching in the ramp array.
895 motor->speed = speed;
897 stepper_enable_irq(motor);
904 * Stop motor gracefully
906 void stepper_stop(struct Stepper *motor)
909 * The best way is to set the target of the movement to the minimum
910 * distance needed to decelerate. The logic in FSM_run will do the rest.
912 if(stepper_idle(motor))
915 stepper_disable_irq(motor);
916 motor->stepToReach = motor->step + motor->rampStep * motor->dir;
917 stepper_enable_irq(motor);
922 * Stop motor immediately, changing the status
924 void stepper_break(struct Stepper *motor, enum StepperState state)
926 // The best way to abort any operation is to go back to pre-idle mode
927 stepper_disable_irq(motor);
929 // Set of Speed disabled and Steps reached so that the function
930 // stepper_idle() succeeds
931 motor->speed = SPEED_STOPPED;
932 motor->stepToReach = motor->step;
933 stepper_changeState(motor, state);
934 stepper_enable_irq(motor);
937 ///< Returns true if the stepper is in idle at the final position or in error:
938 // this means anyway that the motor is not moving
939 bool stepper_idle(struct Stepper *motor)
941 return (stepper_isState(motor, MSTS_ERROR) ||
942 (stepper_isState(motor, MSTS_IDLE) && motor->step == motor->stepToReach) );
945 ///< Returns true if the stepper is in error mode
946 bool stepper_error(struct Stepper *motor)
948 return (stepper_isState(motor, MSTS_ERROR));
951 ///< check the home sensor in zero position
952 bool stepper_inhome(struct Stepper *motor)
954 return(stepper_getStep(motor) == 0 &&
955 !stepper_readHome(motor) );