4 * This file is part of BeRTOS.
6 * Bertos is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20 * As a special exception, you may use this file as part of a free software
21 * library without restriction. Specifically, if other files instantiate
22 * templates or use macros or inline functions from this file, or you compile
23 * this file and link it with other files to produce an executable, this
24 * file does not by itself cause the resulting executable to be covered by
25 * the GNU General Public License. This exception does not however
26 * invalidate any other reasons why the executable file might be covered by
27 * the GNU General Public License.
29 * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
32 * \brief Driver to control stepper motor
34 * \author Francesco Michelini <francesco.michelini@seacfi.com>
35 * \author Giovanni Bajo <rasky@develer.com>
36 * \author Bernie Innocenti <bernie@codewiz.org>
37 * \author Simone Zinanni <s.zinanni@develer.com>
38 * \author Daniele Basile <asterix@develer.com>
43 #include "hw/hw_stepper.h"
44 #include "hw/hw_sensor.h"
46 #include "cfg/cfg_stepper.h"
47 #include <cfg/debug.h>
49 // Define logging setting (for cfg/log.h module).
50 #define LOG_LEVEL STEPPER_LOG_LEVEL
51 #define LOG_FORMAT STEPPER_LOG_FORMAT
54 #include <kern/proc.h>
56 #include <algo/ramp.h>
58 #include CPU_HEADER(stepper)
60 #include <string.h> // memset
66 #define MOTOR_SWITCH_TICKS 60000 ///< Timer ticks to wait for 10ms
67 #define MOTOR_SWITCH_COUNT 5 ///< Number of intervals, long 10ms, to wait before/after switching current off/on
68 #define MOTOR_HOME_MAX_STEPS 30000 ///< Steps before giving up when trying to reach home
69 #define MOTOR_CURRENT_TICKS 6000 ///< Number of intervals, long 10ms, to mantain high current
73 static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS];
75 ///< General FSM states (or NULL if state is not handled)
76 static fsm_state general_states[STEPPER_MAX_STATES];
78 // IRQ functions for stepper motors
79 static void stepper_interrupt(struct Stepper *motor);
81 static void stepper_accel(struct Stepper *motor);
82 static void stepper_decel(struct Stepper *motor);
84 static bool stepper_isState(struct Stepper *motor, enum StepperState state);
85 INLINE void stepper_changeState(struct Stepper *motor, enum StepperState newState);
87 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive);
89 #define MOTOR_INDEX(motor) (motor->index)
91 //------------------------------------------------------------------------
93 INLINE bool setLowCurrent(struct Stepper* motor)
95 if (motor->power == motor->cfg->powerIdle)
98 motor->power = motor->cfg->powerIdle;
99 STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerIdle);
104 INLINE bool setHighCurrent(struct Stepper* motor)
106 if (motor->power == motor->cfg->powerRun)
109 motor->power = motor->cfg->powerRun;
110 STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerRun);
114 INLINE void setCheckSensor(struct Stepper* motor, enum MotorHomeSensorCheck value)
116 motor->enableCheckHome = value;
119 INLINE int8_t getCheckSensor(struct Stepper* motor)
121 return motor->enableCheckHome;
124 INLINE void setDirection(struct Stepper* motor, enum MotorDirection dir)
126 ASSERT(dir == DIR_POSITIVE || dir == DIR_NEGATIVE);
129 if (!motor->cfg->flags.axisInverted)
131 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir == DIR_POSITIVE));
135 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir != DIR_POSITIVE));
140 * Schedule a new stepper IRQ to happen after \a delay (number of clocks),
141 * and optionally doing a step at the same time (if \a do_step is true).
143 INLINE void FAST_FUNC stepper_schedule_irq(struct Stepper* motor, stepper_time_t delay, bool do_step)
148 // Record the step we just did
149 motor->step += motor->dir;
150 stepper_tc_doPulse(motor->timer);
153 stepper_tc_skipPulse(motor->timer);
155 stepper_tc_setDelay(motor->timer, delay);
159 static void stepper_accel(struct Stepper *motor)
161 DB(uint16_t old_val = motor->rampValue;)
162 DB(uint32_t old_clock = motor->rampClock;)
164 const struct Ramp *ramp = &motor->cfg->ramp;
166 ASSERT(motor->rampClock != 0);
168 motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
169 motor->rampClock += motor->rampValue;
172 DB(if (old_val && motor->rampValue > old_val)
174 LOG_ERR("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL);
175 LOG_ERR(" %04x @ %lu --> %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock);
180 static void stepper_decel(struct Stepper *motor)
182 const struct Ramp *ramp = &motor->cfg->ramp;
183 DB(uint16_t old_val = motor->rampValue;)
185 motor->rampClock -= motor->rampValue;
186 ASSERT(motor->rampClock != 0);
187 motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
189 DB(ASSERT(!old_val || motor->rampValue >= old_val););
192 INLINE void stepper_enable_irq(struct Stepper* motor)
194 stepper_tc_irq_enable(motor->timer);
197 INLINE void stepper_disable_irq(struct Stepper* motor)
199 stepper_tc_irq_disable(motor->timer);
202 // the home sensor can be in the standard home list or in the digital
204 bool stepper_readHome(struct Stepper* motor)
206 return (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS) ?
207 hw_home_sensor_read(motor->cfg->homeSensorIndex) :
208 bld_hw_sensor_read(motor->cfg->homeSensorIndex - NUM_HOME_SENSORS);
211 bool stepper_readLevel(struct Stepper* motor)
213 return hw_level_sensor_read(motor->cfg->levelSensorIndex);
216 /************************************************************************/
217 /* Finite-state machine to drive stepper logic from IRQ */
218 /************************************************************************/
220 INLINE void stepper_changeState(struct Stepper* motor, enum StepperState newState)
222 ASSERT(newState < STEPPER_MAX_STATES);
224 motor->state = motor->cfg->states[newState];
226 motor->state = general_states[newState];
227 ASSERT(motor->state);
230 static bool stepper_isState(struct Stepper* motor, enum StepperState state)
232 return (motor->cfg->states[state]
233 ? motor->cfg->states[state] == motor->state
234 : general_states[state] == motor->state);
237 static bool stepper_checkHomeErrors(struct Stepper* motor)
241 home = stepper_readHome(motor);
243 if (motor->enableCheckHome == MOTOR_HOMESENSOR_INCHECK && home
244 && (!motor->stepCircular || motor->step < motor->stepCircular / 2))
246 * if home Sensor check enabled in movement to 0 position and
247 * the motor is in home increase the counter
248 * for rotating motor we include the check that the motor is
249 * inside the last "lap" (FIXME: check it better)
251 motor->stepsErrorHome++;
252 else if (motor->enableCheckHome == MOTOR_HOMESENSOR_OUTCHECK && !home)
254 * if home Sensor check enabled in movement from 0 position and
255 * the motor is not in home increase the counter
257 motor->stepsErrorHome++;
259 // clear error steps counter
260 motor->stepsErrorHome = 0;
262 // if this is the last consecutive position in which the motor is in/out home ...
263 ASSERT(motor->stepsErrorHome <= MOTOR_CONSECUTIVE_ERROR_STEPS);
264 if (motor->stepsErrorHome >= MOTOR_CONSECUTIVE_ERROR_STEPS)
266 // if the position at which the motor first saw/didn't see the home
267 // is out of tolerance -> breakmotor -> ERROR
268 if (motor->step > motor->stepsTollMax || motor->step < motor->stepsTollMin )
270 // break motor and error
271 motor->speed = SPEED_STOPPED;
272 motor->stepToReach = motor->step;
274 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
275 motor->skipIrqs = MOTOR_SWITCH_COUNT;
279 // the motor reached the home crossing -> disable error check
280 setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
286 static void stepper_checkLevelSensor(struct Stepper* motor)
288 // level sensor check
289 if (motor->step > motor->stepsDeaf)
291 if (stepper_readLevel(motor))
293 // record current position, disable check and stop motor
294 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
295 motor->stepsLevel = motor->step;
296 //motor->stepToReach = motor->step + motor->rampStep * motor->dir;
298 motor->stepToReach = motor->step;
299 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
300 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
305 static enum StepperState FAST_FUNC FSM_run(struct Stepper *motor)
309 if (!stepper_checkHomeErrors(motor))
312 stepper_checkLevelSensor(motor);
314 if ((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
315 (motor->stepToReach != STEPS_INFINITE_NEGATIVE ))
317 // Calculate (always positive) distance between current position and destination step
318 distance = (uint16_t)((motor->stepToReach - motor->step) * motor->dir);
322 // We're at a very long distance ;-)
324 // if the motor is rotating and it has just ran a complete round
325 // the position is set to 0
326 if(motor->step == motor->stepCircular)
331 // Position reached - stop motor
332 //motor->speed = SPEED_STOPPED;
333 motor->rampStep = -1;
334 //motor->rampClock = motor->ramp->clocksMaxWL;
335 //motor->rampValue = 0;
336 //motor->rampClock = motor->rampValue = motor->ramp->clocksMaxWL;
338 else if (distance <= (uint16_t)motor->rampStep)
339 stepper_decel(motor);
341 // check whether the velocity must be changed
342 else if (motor->speed < (uint16_t)motor->rampValue)
344 stepper_accel(motor);
345 if (motor->speed > (uint16_t)motor->rampValue)
346 motor->speed = (uint16_t)motor->rampValue;
348 else if (motor->speed > (uint16_t)motor->rampValue)
349 stepper_decel(motor);
351 // If rampStep == -1, leave output pin high and wait for low current
352 if (motor->rampStep < 0)
354 // Wait before switching to low current
355 motor->speed = SPEED_STOPPED;
357 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
358 motor->skipIrqs = MOTOR_SWITCH_COUNT;
361 * If there was a home sensor check activated, and the check has not
362 * been done yet, it means that we reached the end position without
363 * finding the home (or exiting from it). This is bad!
365 if (motor->enableCheckHome != MOTOR_HOMESENSOR_NOCHECK)
368 // check if the motor has to stay in high current
369 if(motor->cfg->flags.highcurrentBit)
371 motor->changeCurrentIrqs = MOTOR_CURRENT_TICKS;
378 // Wait for high->low transition
379 ASSERT(motor->rampValue > motor->cfg->pulse);
380 stepper_schedule_irq(motor, motor->rampValue, true);
385 static enum StepperState FSM_idle(struct Stepper* motor)
387 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
389 if (motor->speed == SPEED_STOPPED)
391 // check if it's time to switch to low current
392 if(motor->changeCurrentIrqs > 0)
394 if(--motor->changeCurrentIrqs == 0)
395 setLowCurrent(motor);
400 // Switch to high current and wait for stabilization
401 // (if the motor is in low current)
402 if(motor->changeCurrentIrqs == 0)
404 setHighCurrent(motor);
405 motor->skipIrqs = MOTOR_SWITCH_COUNT;
411 static enum StepperState FSM_preidle(struct Stepper* motor)
413 // Normal operation mode
414 motor->changeCurrentIrqs = 0;
415 setLowCurrent(motor);
416 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
420 static enum StepperState FSM_error(struct Stepper* motor)
422 // Error condition mode
423 setLowCurrent(motor);
424 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
428 static enum StepperState FSM_prerun(struct Stepper* motor)
430 enum MotorDirection dir;
433 if ((motor->stepToReach != motor->step) ||
434 (motor->stepToReach == STEPS_INFINITE_POSITIVE) ||
435 (motor->stepToReach == STEPS_INFINITE_NEGATIVE) )
437 // Setup for first step
441 if(motor->stepToReach == STEPS_INFINITE_POSITIVE)
443 else if(motor->stepToReach == STEPS_INFINITE_NEGATIVE)
445 else if(motor->stepToReach > motor->step)
450 setDirection(motor, dir);
452 // Enable of the home sensor control, if necessary
453 // (before calling this function set the motor direction as above)
454 stepper_enableCheckHome(motor, (dir == DIR_POSITIVE));
456 // if the movement is infinite negative set the sw direction positive
457 // (not the hw: see below) to count the steps
458 if(motor->stepToReach == STEPS_INFINITE_NEGATIVE) motor->dir = DIR_POSITIVE;
460 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
466 * If we are here we should do at least one step.
469 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
470 motor->skipIrqs = MOTOR_SWITCH_COUNT;
475 static enum StepperState FSM_preinit(struct Stepper* motor)
477 // Set current high, and wait for stabilization
478 if (setHighCurrent(motor))
480 motor->skipIrqs = MOTOR_SWITCH_COUNT;
485 * This state is used when initializing the motor, to bring back
486 * to the home. The idea is that we do not know where the motor
487 * is at this point, so there can be two possibilities:
489 * - The motor is already in home. We do not know how much into the
490 * home we are. So we need to get out of the home (MSTS_LEAVING)
491 * and then get back into it of the desired number of steps.
493 * - The motor is not in home: we need to look for it (MSTS_INIT).
494 * We can safely assume that we will find the home in the negative
495 * direction. For circular motors, any direction would do. For
496 * other motors, the home is set at zero, so the current position
497 * has to be a positive value.
500 if (stepper_readHome(motor))
502 setDirection(motor, DIR_POSITIVE);
503 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
507 setDirection(motor, DIR_NEGATIVE);
508 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
513 static enum StepperState FSM_init(struct Stepper* motor)
515 // If we are not in home, keep looking
516 if (!stepper_readHome(motor))
518 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
523 * Home! We still need to enter the home of the specified number of steps.
524 * That will be our absolute zero.
527 motor->step = motor->cfg->stepsInHome - 1; // start counting down steps in home
528 motor->stepToReach = 0;
530 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
531 return MSTS_ENTERING;
534 static enum StepperState FSM_entering(struct Stepper* motor)
536 // We must be in home
537 //ASSERT(stepper_readHome(motor));
539 // if while entering the sensor we are no more in home we reset the steps
540 // counter (optical sensor)
541 if(!stepper_readHome(motor))
542 motor->step = motor->cfg->stepsInHome - 1;
544 // Current Position must be non-negative
545 ASSERT(motor->step >= 0);
549 // reach the final target inside home sensor
555 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
556 return MSTS_ENTERING;
559 static enum StepperState FSM_leaving(struct Stepper* motor)
561 ASSERT(motor->dir == DIR_POSITIVE);
564 if (!stepper_readHome(motor))
566 // we are out of home : change state and going far from sensor
567 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
572 // Still at home. Just wait here and keep doing steps
573 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
578 static enum StepperState FSM_outhome(struct Stepper* motor)
580 ASSERT(motor->dir == DIR_POSITIVE);
582 // We must be out of home: once we are no more in home
583 // we just need to move away, even if not very precide (optical sensor)
584 // ASSERT(!stepper_readHome(motor));
586 if(motor->step >= motor->cfg->stepsOutHome)
588 // reach the final target outside home sensor
591 // start home entering procedure (delay in executing step)
592 setDirection(motor, DIR_NEGATIVE);
593 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
594 motor->skipIrqs = MOTOR_SWITCH_COUNT;
599 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
603 static void FAST_FUNC stepper_interrupt(struct Stepper *motor)
605 enum StepperState newState;
607 // Check if we need to skip a certain number of IRQs
611 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
615 ASSERT(motor->state);
616 newState = motor->state(motor);
617 stepper_changeState(motor, newState);
623 /************************************************************************/
625 /************************************************************************/
628 * Initialize the stepper module
630 void stepper_init(void)
634 // before starting the power all the stepper enable must be surely low
637 // Bind functions to general states
638 memset(general_states, 0, sizeof(general_states));
639 general_states[MSTS_IDLE] = FSM_idle;
640 general_states[MSTS_PREIDLE] = FSM_preidle;
641 general_states[MSTS_PRERUN] = FSM_prerun;
642 general_states[MSTS_RUN] = FSM_run;
643 general_states[MSTS_PREINIT] = FSM_preinit;
644 general_states[MSTS_INIT] = FSM_init;
645 general_states[MSTS_ENTERING] = FSM_entering;
646 general_states[MSTS_LEAVING]= FSM_leaving;
647 general_states[MSTS_OUTHOME]= FSM_outhome;
648 general_states[MSTS_ERROR]= FSM_error;
651 void stepper_end(void)
653 // Disable all stepper timer interrupt to stop motors
654 for (int i = 0; i < CONFIG_NUM_STEPPER_MOTORS; i++)
655 stepper_disable_irq(&all_motors[i]);
659 * Apply a setup config to motor structure context
661 struct Stepper* stepper_setup(int index, struct StepperConfig *cfg)
663 struct Stepper* motor;
665 ASSERT(index < CONFIG_NUM_STEPPER_MOTORS);
667 motor = &all_motors[index];
668 motor->index = index;
671 //Register timer to stepper, and enable irq
672 stepper_tc_setup(motor->index, &stepper_interrupt, motor);
674 stepper_reset(motor);
676 stepper_enable_irq(motor);
682 * Set the enable for all the motors to 0 before switching on the power
684 void stepper_disable(void)
686 STEPPER_DISABLE_ALL();
692 void stepper_reset(struct Stepper *motor)
695 * To stop motor diable stepper irq.
697 stepper_disable_irq(motor);
699 //Disable a stepper motor
700 STEPPER_DISABLE(MOTOR_INDEX(motor));
702 // Setup context variables
705 motor->rampStep = -1;
706 // We cannot set the clock at zero at start because of a limit in the fixed point ramp
707 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
708 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
709 motor->speed = SPEED_STOPPED;
710 motor->stepToReach = 0;
712 motor->stepCircular = 0;
713 setDirection(motor, DIR_POSITIVE);
714 setLowCurrent(motor);
716 motor->changeCurrentIrqs = 0;
718 // default value (disable level sensor check)
719 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
721 STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
722 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
724 if (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS)
725 hw_home_sensor_set_inverted(motor->cfg->homeSensorIndex, motor->cfg->flags.homeInverted);
727 if (motor->cfg->levelSensorIndex != MOTOR_NO_LEVEL_SENSOR)
728 hw_level_sensor_set_inverted(motor->cfg->levelSensorIndex, motor->cfg->flags.levelInverted);
730 stepper_changeState(motor, MSTS_IDLE);
732 // Reset stepper timer counter
733 stepper_tc_resetTimer(motor->timer);
735 // reset hw to the stepper motor
736 STEPPER_RESET(MOTOR_INDEX(motor));
737 STEPPER_ENABLE(MOTOR_INDEX(motor));
741 void stepper_updateHalfStep(struct Stepper *motor)
743 STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
746 void stepper_updateControlBit(struct Stepper *motor)
748 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
751 void stepper_updateControlMoveBit(struct Stepper *motor)
753 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlMoveBit);
757 * Find the home of a \a motor assuming no current knowledge about its position.
759 * This must be done when the motor is desynchronized with the firmware and
760 * we do not know anymore where it is.
762 * In normal operation mode, to go back to the home, it is sufficient to use
763 * move to step #0 with stepper_move, since the home is always at step #0.
765 void stepper_home(struct Stepper *motor)
768 // Begin home procedure
769 stepper_disable_irq(motor);
771 // disable home sensor check (default)
772 setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
773 // deafult value (disable level sensor check)
774 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
776 setDirection(motor, DIR_POSITIVE);
777 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
778 stepper_changeState(motor, MSTS_PREINIT);
780 stepper_enable_irq(motor);
784 void stepper_setStep(struct Stepper *motor, int16_t step)
790 int16_t stepper_getStep(struct Stepper *motor)
795 int16_t stepper_getLevelStep(struct Stepper *motor)
797 return motor->stepsLevel;
800 void stepper_set_stepCircular(struct Stepper *motor, int16_t steps)
802 motor->stepCircular = steps;
805 int16_t stepper_get_stepCircular(struct Stepper *motor)
807 return motor->stepCircular;
810 int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir)
814 // scale the current position inside the motor lap
815 if(!motor->stepCircular) return 0;
818 while(motor->step > motor->stepCircular) motor->step -= motor->stepCircular;
820 if(dir == DIR_NEGATIVE)
822 steps = ((motor->stepCircular - motor->step) % motor->stepCircular);
827 steps = (motor->step % motor->stepCircular);
833 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive)
835 enum MotorHomeSensorCheck value = MOTOR_HOMESENSOR_NOCHECK; // default
837 motor->stepsTollMin = 0;
839 if((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
840 (motor->stepToReach != STEPS_INFINITE_NEGATIVE) )
842 if(bDirPositive) // else if(motor->dir == DIR_POSITIVE)
844 /* if the direction is positive (movement from 0 position),
845 * if the starting position is inside home and the target position
846 * is outside home -> the motor has to cross the home sensor -> enable the control
848 if (motor->step < motor->cfg->stepsInHome - motor->cfg->stepsTollOutHome &&
849 motor->stepToReach > motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome)
851 value = MOTOR_HOMESENSOR_OUTCHECK;
852 // home sensor out max position
853 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome + MOTOR_CONSECUTIVE_ERROR_STEPS;
854 // home sensor in max position
855 if(motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS > motor->cfg->stepsTollOutHome)
856 motor->stepsTollMin = motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS - motor->cfg->stepsTollOutHome;
859 else // if(motor->dir == DIR_NEGATIVE)
862 * if the direction is negative (movement to 0 position),
863 * if the starting position is far from home and the target position
864 * is inside home -> the motor has to cross the home sensor -> enable the control
866 if (motor->step > motor->cfg->stepsInHome + motor->cfg->stepsTollInHome &&
867 motor->stepToReach < motor->cfg->stepsInHome - motor->cfg->stepsTollInHome)
869 value = MOTOR_HOMESENSOR_INCHECK;
870 // home sensor out max position
871 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollInHome - MOTOR_CONSECUTIVE_ERROR_STEPS;
872 // home sensor in max position
873 if(motor->cfg->stepsInHome > motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS)
874 motor->stepsTollMin = motor->cfg->stepsInHome - (motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS);
878 setCheckSensor(motor, value);
882 * Move motor to absolute position at specified speed
884 * \arg steps position to reach in steps
885 * \arg speed speed in timer ticks (use TIME2CLOCKS() to convert)
887 int16_t stepper_move(struct Stepper *motor, int16_t steps, uint16_t speed, int16_t deafstep)
889 // if the stepper already is in the desired position -> nothing to do
890 if (motor->step == steps)
893 stepper_disable_irq(motor);
896 motor->stepToReach = steps;
899 motor->stepsErrorHome = 0;
901 // position to start level check
902 motor->stepsDeaf = deafstep;
904 // clear level position
905 motor->stepsLevel = 0;
907 if (speed < motor->cfg->ramp.clocksMinWL)
909 ASSERT2(0, "speed too fast (small number)");
910 speed = motor->cfg->ramp.clocksMinWL;
913 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
914 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
916 // TODO: find the exact value for motor->speed searching in the ramp array.
917 motor->speed = speed;
919 stepper_enable_irq(motor);
926 * Stop motor gracefully
928 void stepper_stop(struct Stepper *motor)
931 * The best way is to set the target of the movement to the minimum
932 * distance needed to decelerate. The logic in FSM_run will do the rest.
934 if(stepper_idle(motor))
937 stepper_disable_irq(motor);
938 motor->stepToReach = motor->step + motor->rampStep * motor->dir;
939 stepper_enable_irq(motor);
944 * Stop motor immediately, changing the status
946 void stepper_break(struct Stepper *motor, enum StepperState state)
948 // The best way to abort any operation is to go back to pre-idle mode
949 stepper_disable_irq(motor);
951 // Set of Speed disabled and Steps reached so that the function
952 // stepper_idle() succeeds
953 motor->speed = SPEED_STOPPED;
954 motor->stepToReach = motor->step;
955 stepper_changeState(motor, state);
956 stepper_enable_irq(motor);
959 ///< Returns true if the stepper is in idle at the final position or in error:
960 // this means anyway that the motor is not moving
961 bool stepper_idle(struct Stepper *motor)
963 return (stepper_isState(motor, MSTS_ERROR) ||
964 (stepper_isState(motor, MSTS_IDLE) && motor->step == motor->stepToReach) );
967 ///< Returns true if the stepper is in error mode
968 bool stepper_error(struct Stepper *motor)
970 return (stepper_isState(motor, MSTS_ERROR));
973 ///< check the home sensor in zero position
974 bool stepper_inhome(struct Stepper *motor)
976 return(stepper_getStep(motor) == 0 &&
977 !stepper_readHome(motor) );