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 <cfg/debug.h>
22 #include <kern/proc.h>
23 #include <algo/ramp.h>
24 #include <drv/stepper_at91.h>
26 #include <hw/hw_stepper.h>
27 #include <hw/hw_sensor.h>
29 #include <string.h> // memset
31 #include "appconfig.h"
37 #define MOTOR_SWITCH_TICKS 60000 ///< Timer ticks to wait for 10ms
38 #define MOTOR_SWITCH_COUNT 5 ///< Number of intervals, long 10ms, to wait before/after switching current off/on
39 #define MOTOR_HOME_MAX_STEPS 30000 ///< Steps before giving up when trying to reach home
40 #define MOTOR_CURRENT_TICKS 6000 ///< Number of intervals, long 10ms, to mantain high current
44 static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS];
46 ///< General FSM states (or NULL if state is not handled)
47 static fsm_state general_states[STEPPER_MAX_STATES];
49 // IRQ functions for stepper motors
50 static void stepper_interrupt(struct Stepper *motor);
52 static void stepper_accel(struct Stepper *motor);
53 static void stepper_decel(struct Stepper *motor);
55 static bool stepper_isState(struct Stepper *motor, enum StepperState state);
56 INLINE void stepper_changeState(struct Stepper *motor, enum StepperState newState);
58 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive);
60 #define MOTOR_INDEX(motor) (motor->index)
62 //------------------------------------------------------------------------
64 INLINE bool setLowCurrent(struct Stepper* motor)
66 if (motor->power == motor->cfg->powerIdle)
69 motor->power = motor->cfg->powerIdle;
70 STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerIdle);
75 INLINE bool setHighCurrent(struct Stepper* motor)
77 if (motor->power == motor->cfg->powerRun)
80 motor->power = motor->cfg->powerRun;
81 STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerRun);
85 INLINE void setCheckSensor(struct Stepper* motor, enum MotorHomeSensorCheck value)
87 motor->enableCheckHome = value;
90 INLINE int8_t getCheckSensor(struct Stepper* motor)
92 return motor->enableCheckHome;
95 INLINE void setDirection(struct Stepper* motor, enum MotorDirection dir)
97 ASSERT(dir == DIR_POSITIVE || dir == DIR_NEGATIVE);
100 if (!motor->cfg->flags.axisInverted)
102 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir == DIR_POSITIVE));
106 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir != DIR_POSITIVE));
111 * Schedule a new stepper IRQ to happen after \a delay (number of clocks),
112 * and optionally doing a step at the same time (if \a do_step is true).
114 INLINE void FAST_FUNC stepper_schedule_irq(struct Stepper* motor, stepper_time_t delay, bool do_step)
119 // Record the step we just did
120 motor->step += motor->dir;
121 stepper_tc_doPulse(motor->timer);
124 stepper_tc_skipPulse(motor->timer);
126 stepper_tc_setDelay(motor->timer, delay);
130 static void stepper_accel(struct Stepper *motor)
133 uint16_t old_val = motor->rampValue;
134 uint32_t old_clock = motor->rampClock;
137 const struct Ramp *ramp = &motor->cfg->ramp;
139 ASSERT(motor->rampClock != 0);
141 motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
142 motor->rampClock += motor->rampValue;
146 if (old_val && motor->rampValue > old_val)
148 kprintf("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL);
149 kprintf(" %04x @ %lu --> %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock);
154 static void stepper_decel(struct Stepper *motor)
156 const struct Ramp *ramp = &motor->cfg->ramp;
157 DB(uint16_t old_val = motor->rampValue;)
159 motor->rampClock -= motor->rampValue;
160 ASSERT(motor->rampClock != 0);
161 motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
163 DB(ASSERT(!old_val || motor->rampValue >= old_val););
166 INLINE void stepper_enable_irq(struct Stepper* motor)
168 stepper_tc_irq_enable(motor->timer);
171 INLINE void stepper_disable_irq(struct Stepper* motor)
173 stepper_tc_irq_disable(motor->timer);
176 // the home sensor can be in the standard home list or in the digital
178 bool stepper_readHome(struct Stepper* motor)
180 return (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS) ?
181 hw_home_sensor_read(motor->cfg->homeSensorIndex) :
182 bld_hw_sensor_read(motor->cfg->homeSensorIndex - NUM_HOME_SENSORS);
185 bool stepper_readLevel(struct Stepper* motor)
187 return hw_level_sensor_read(motor->cfg->levelSensorIndex);
190 /************************************************************************/
191 /* Finite-state machine to drive stepper logic from IRQ */
192 /************************************************************************/
194 INLINE void stepper_changeState(struct Stepper* motor, enum StepperState newState)
196 ASSERT(newState < STEPPER_MAX_STATES);
198 motor->state = motor->cfg->states[newState];
200 motor->state = general_states[newState];
201 ASSERT(motor->state);
204 static bool stepper_isState(struct Stepper* motor, enum StepperState state)
206 return (motor->cfg->states[state]
207 ? motor->cfg->states[state] == motor->state
208 : general_states[state] == motor->state);
211 static bool stepper_checkHomeErrors(struct Stepper* motor)
215 home = stepper_readHome(motor);
217 if (motor->enableCheckHome == MOTOR_HOMESENSOR_INCHECK && home
218 && (!motor->stepCircular || motor->step < motor->stepCircular / 2))
220 * if home Sensor check enabled in movement to 0 position and
221 * the motor is in home increase the counter
222 * for rotating motor we include the check that the motor is
223 * inside the last "lap" (FIXME: check it better)
225 motor->stepsErrorHome++;
226 else if (motor->enableCheckHome == MOTOR_HOMESENSOR_OUTCHECK && !home)
228 * if home Sensor check enabled in movement from 0 position and
229 * the motor is not in home increase the counter
231 motor->stepsErrorHome++;
233 // clear error steps counter
234 motor->stepsErrorHome = 0;
236 // if this is the last consecutive position in which the motor is in/out home ...
237 ASSERT(motor->stepsErrorHome <= MOTOR_CONSECUTIVE_ERROR_STEPS);
238 if (motor->stepsErrorHome >= MOTOR_CONSECUTIVE_ERROR_STEPS)
240 // if the position at which the motor first saw/didn't see the home
241 // is out of tolerance -> breakmotor -> ERROR
242 if (motor->step > motor->stepsTollMax || motor->step < motor->stepsTollMin )
244 // break motor and error
245 motor->speed = SPEED_STOPPED;
246 motor->stepToReach = motor->step;
248 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
249 motor->skipIrqs = MOTOR_SWITCH_COUNT;
253 // the motor reached the home crossing -> disable error check
254 setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
260 static void stepper_checkLevelSensor(struct Stepper* motor)
262 // level sensor check
263 if (motor->step > motor->stepsDeaf)
265 if (stepper_readLevel(motor))
267 // record current position, disable check and stop motor
268 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
269 motor->stepsLevel = motor->step;
270 // motor->stepToReach = motor->step + motor->rampStep * motor->dir;
272 motor->stepToReach = motor->step;
273 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
274 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
279 static enum StepperState FAST_FUNC FSM_run(struct Stepper *motor)
283 if (!stepper_checkHomeErrors(motor))
286 stepper_checkLevelSensor(motor);
288 if ((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
289 (motor->stepToReach != STEPS_INFINITE_NEGATIVE ))
291 // Calculate (always positive) distance between current position and destination step
292 distance = (uint16_t)((motor->stepToReach - motor->step) * motor->dir);
296 // We're at a very long distance ;-)
298 // if the motor is rotating and it has just ran a complete round
299 // the position is set to 0
300 if(motor->step == motor->stepCircular)
305 // Position reached - stop motor
306 // motor->speed = SPEED_STOPPED;
307 motor->rampStep = -1;
308 // motor->rampClock = motor->ramp->clocksMaxWL;
309 // motor->rampValue = 0;
310 // motor->rampClock = motor->rampValue = motor->ramp->clocksMaxWL;
312 else if (distance <= motor->rampStep)
313 stepper_decel(motor);
315 // check whether the velocity must be changed
316 else if (motor->speed < (uint16_t)motor->rampValue)
318 stepper_accel(motor);
319 if (motor->speed > (uint16_t)motor->rampValue)
320 motor->speed = (uint16_t)motor->rampValue;
322 else if (motor->speed > (uint16_t)motor->rampValue)
323 stepper_decel(motor);
325 // If rampStep == -1, leave output pin high and wait for low current
326 if (motor->rampStep < 0)
328 // Wait before switching to low current
329 motor->speed = SPEED_STOPPED;
331 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
332 motor->skipIrqs = MOTOR_SWITCH_COUNT;
335 * If there was a home sensor check activated, and the check has not
336 * been done yet, it means that we reached the end position without
337 * finding the home (or exiting from it). This is bad!
339 if (motor->enableCheckHome != MOTOR_HOMESENSOR_NOCHECK)
342 // check if the motor has to stay in high current
343 if(motor->cfg->flags.highcurrentBit)
345 motor->changeCurrentIrqs = MOTOR_CURRENT_TICKS;
352 // Wait for high->low transition
353 ASSERT(motor->rampValue > motor->cfg->pulse);
354 stepper_schedule_irq(motor, motor->rampValue, true);
359 static enum StepperState FSM_idle(struct Stepper* motor)
361 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
363 if (motor->speed == SPEED_STOPPED)
365 // check if it's time to switch to low current
366 if(motor->changeCurrentIrqs > 0)
368 if(--motor->changeCurrentIrqs == 0)
369 setLowCurrent(motor);
374 // Switch to high current and wait for stabilization
375 // (if the motor is in low current)
376 if(motor->changeCurrentIrqs == 0)
378 setHighCurrent(motor);
379 motor->skipIrqs = MOTOR_SWITCH_COUNT;
385 static enum StepperState FSM_preidle(struct Stepper* motor)
387 // Normal operation mode
388 motor->changeCurrentIrqs = 0;
389 setLowCurrent(motor);
390 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
394 static enum StepperState FSM_error(struct Stepper* motor)
396 // Error condition mode
397 setLowCurrent(motor);
398 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
402 static enum StepperState FSM_prerun(struct Stepper* motor)
404 enum MotorDirection dir;
407 if ((motor->stepToReach != motor->step) ||
408 (motor->stepToReach == STEPS_INFINITE_POSITIVE) ||
409 (motor->stepToReach == STEPS_INFINITE_NEGATIVE) )
411 // Setup for first step
415 if(motor->stepToReach == STEPS_INFINITE_POSITIVE)
417 else if(motor->stepToReach == STEPS_INFINITE_NEGATIVE)
419 else if(motor->stepToReach > motor->step)
424 setDirection(motor, dir);
426 // Enable of the home sensor control, if necessary
427 // (before calling this function set the motor direction as above)
428 stepper_enableCheckHome(motor, (dir == DIR_POSITIVE));
430 // if the movement is infinite negative set the sw direction positive
431 // (not the hw: see below) to count the steps
432 if(motor->stepToReach == STEPS_INFINITE_NEGATIVE) motor->dir = DIR_POSITIVE;
434 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
440 * If we are here we should do at least one step.
443 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
444 motor->skipIrqs = MOTOR_SWITCH_COUNT;
449 static enum StepperState FSM_preinit(struct Stepper* motor)
451 // Set current high, and wait for stabilization
452 if (setHighCurrent(motor))
454 motor->skipIrqs = MOTOR_SWITCH_COUNT;
459 * This state is used when initializing the motor, to bring back
460 * to the home. The idea is that we do not know where the motor
461 * is at this point, so there can be two possibilities:
463 * - The motor is already in home. We do not know how much into the
464 * home we are. So we need to get out of the home (MSTS_LEAVING)
465 * and then get back into it of the desired number of steps.
467 * - The motor is not in home: we need to look for it (MSTS_INIT).
468 * We can safely assume that we will find the home in the negative
469 * direction. For circular motors, any direction would do. For
470 * other motors, the home is set at zero, so the current position
471 * has to be a positive value.
474 if (stepper_readHome(motor))
476 setDirection(motor, DIR_POSITIVE);
477 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
481 setDirection(motor, DIR_NEGATIVE);
482 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
487 static enum StepperState FSM_init(struct Stepper* motor)
489 // If we are not in home, keep looking
490 if (!stepper_readHome(motor))
492 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
497 * Home! We still need to enter the home of the specified number of steps.
498 * That will be our absolute zero.
501 motor->step = motor->cfg->stepsInHome - 1; // start counting down steps in home
502 motor->stepToReach = 0;
504 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
505 return MSTS_ENTERING;
508 static enum StepperState FSM_entering(struct Stepper* motor)
510 // We must be in home
511 // ASSERT(stepper_readHome(motor));
513 // if while entering the sensor we are no more in home we reset the steps
514 // counter (optical sensor)
515 if(!stepper_readHome(motor))
516 motor->step = motor->cfg->stepsInHome - 1;
518 // Current Position must be non-negative
519 ASSERT(motor->step >= 0);
523 // reach the final target inside home sensor
529 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
530 return MSTS_ENTERING;
533 static enum StepperState FSM_leaving(struct Stepper* motor)
535 ASSERT(motor->dir == DIR_POSITIVE);
538 if (!stepper_readHome(motor))
540 // we are out of home : change state and going far from sensor
541 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
546 // Still at home. Just wait here and keep doing steps
547 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
552 static enum StepperState FSM_outhome(struct Stepper* motor)
554 ASSERT(motor->dir == DIR_POSITIVE);
556 // We must be out of home: once we are no more in home
557 // we just need to move away, even if not very precide (optical sensor)
558 // ASSERT(!stepper_readHome(motor));
560 if(motor->step >= motor->cfg->stepsOutHome)
562 // reach the final target outside home sensor
565 // start home entering procedure (delay in executing step)
566 setDirection(motor, DIR_NEGATIVE);
567 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
568 motor->skipIrqs = MOTOR_SWITCH_COUNT;
573 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
577 static void FAST_FUNC stepper_interrupt(struct Stepper *motor)
579 enum StepperState newState;
581 // Check if we need to skip a certain number of IRQs
585 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
589 ASSERT(motor->state);
590 newState = motor->state(motor);
591 stepper_changeState(motor, newState);
597 /************************************************************************/
599 /************************************************************************/
602 * Initialize the stepper module
604 void stepper_init(void)
608 // before starting the power all the stepper enable must be surely low
611 // Bind functions to general states
612 memset(general_states, 0, sizeof(general_states));
613 general_states[MSTS_IDLE] = FSM_idle;
614 general_states[MSTS_PREIDLE] = FSM_preidle;
615 general_states[MSTS_PRERUN] = FSM_prerun;
616 general_states[MSTS_RUN] = FSM_run;
617 general_states[MSTS_PREINIT] = FSM_preinit;
618 general_states[MSTS_INIT] = FSM_init;
619 general_states[MSTS_ENTERING] = FSM_entering;
620 general_states[MSTS_LEAVING]= FSM_leaving;
621 general_states[MSTS_OUTHOME]= FSM_outhome;
622 general_states[MSTS_ERROR]= FSM_error;
625 void stepper_end(void)
627 // Disable all stepper timer interrupt to stop motors
628 for (int i = 0; i < CONFIG_NUM_STEPPER_MOTORS; i++)
629 stepper_disable_irq(&all_motors[i]);
633 * Apply a setup config to motor structure context
635 struct Stepper* stepper_setup(int index, struct StepperConfig *cfg)
637 struct Stepper* motor;
639 ASSERT(index < CONFIG_NUM_STEPPER_MOTORS);
641 motor = &all_motors[index];
642 motor->index = index;
645 //Register timer to stepper, and enable irq
646 stepper_tc_setup(motor->index, &stepper_interrupt, motor);
648 stepper_reset(motor);
650 stepper_enable_irq(motor);
656 * Set the enable for all the motors to 0 before switching on the power
658 void stepper_disable(void)
660 STEPPER_DISABLE_ALL();
666 void stepper_reset(struct Stepper *motor)
669 * To stop motor diable stepper irq.
671 stepper_disable_irq(motor);
673 //Disable a stepper motor
674 STEPPER_DISABLE(MOTOR_INDEX(motor));
676 // Setup context variables
679 motor->rampStep = -1;
680 // We cannot set the clock at zero at start because of a limit in the fixed point ramp
681 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
682 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
683 motor->speed = SPEED_STOPPED;
684 motor->stepToReach = 0;
686 motor->stepCircular = 0;
687 setDirection(motor, DIR_POSITIVE);
688 setLowCurrent(motor);
690 motor->changeCurrentIrqs = 0;
692 // default value (disable level sensor check)
693 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
695 STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
696 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
698 if (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS)
699 hw_home_sensor_set_inverted(motor->cfg->homeSensorIndex, motor->cfg->flags.homeInverted);
701 if (motor->cfg->levelSensorIndex != MOTOR_NO_LEVEL_SENSOR)
702 hw_level_sensor_set_inverted(motor->cfg->levelSensorIndex, motor->cfg->flags.levelInverted);
704 stepper_changeState(motor, MSTS_IDLE);
706 // Reset stepper timer counter
707 stepper_tc_resetTimer(motor->timer);
709 // reset hw to the stepper motor
710 STEPPER_RESET(MOTOR_INDEX(motor));
711 STEPPER_ENABLE(MOTOR_INDEX(motor));
715 void stepper_updateHalfStep(struct Stepper *motor)
717 STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
720 void stepper_updateControlBit(struct Stepper *motor)
722 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
725 void stepper_updateControlMoveBit(struct Stepper *motor)
727 STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlMoveBit);
731 * Find the home of a \a motor assuming no current knowledge about its position.
733 * This must be done when the motor is desynchronized with the firmware and
734 * we do not know anymore where it is.
736 * In normal operation mode, to go back to the home, it is sufficient to use
737 * move to step #0 with stepper_move, since the home is always at step #0.
739 void stepper_home(struct Stepper *motor)
742 // Begin home procedure
743 stepper_disable_irq(motor);
745 // disable home sensor check (default)
746 setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
747 // deafult value (disable level sensor check)
748 motor->stepsDeaf = DEAFSTEPS_DEFAULT;
750 setDirection(motor, DIR_POSITIVE);
751 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
752 stepper_changeState(motor, MSTS_PREINIT);
754 stepper_enable_irq(motor);
758 void stepper_setStep(struct Stepper *motor, int16_t step)
764 int16_t stepper_getStep(struct Stepper *motor)
769 int16_t stepper_getLevelStep(struct Stepper *motor)
771 return motor->stepsLevel;
774 void stepper_set_stepCircular(struct Stepper *motor, int16_t steps)
776 motor->stepCircular = steps;
779 int16_t stepper_get_stepCircular(struct Stepper *motor)
781 return motor->stepCircular;
784 int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir)
788 // scale the current position inside the motor lap
789 if(!motor->stepCircular) return 0;
792 while(motor->step > motor->stepCircular) motor->step -= motor->stepCircular;
794 if(dir == DIR_NEGATIVE)
796 steps = ((motor->stepCircular - motor->step) % motor->stepCircular);
801 steps = (motor->step % motor->stepCircular);
807 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive)
809 enum MotorHomeSensorCheck value = MOTOR_HOMESENSOR_NOCHECK; // default
811 motor->stepsTollMin = 0;
813 if( (motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
814 (motor->stepToReach != STEPS_INFINITE_NEGATIVE) )
816 if(bDirPositive) // else if(motor->dir == DIR_POSITIVE)
818 /* if the direction is positive (movement from 0 position),
819 * if the starting position is inside home and the target position
820 * is outside home -> the motor has to cross the home sensor -> enable the control
822 if (motor->step < motor->cfg->stepsInHome - motor->cfg->stepsTollOutHome &&
823 motor->stepToReach > motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome)
825 value = MOTOR_HOMESENSOR_OUTCHECK;
826 // home sensor out max position
827 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome + MOTOR_CONSECUTIVE_ERROR_STEPS;
828 // home sensor in max position
829 if(motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS > motor->cfg->stepsTollOutHome)
830 motor->stepsTollMin = motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS - motor->cfg->stepsTollOutHome;
833 else // if(motor->dir == DIR_NEGATIVE)
836 * if the direction is negative (movement to 0 position),
837 * if the starting position is far from home and the target position
838 * is inside home -> the motor has to cross the home sensor -> enable the control
840 if (motor->step > motor->cfg->stepsInHome + motor->cfg->stepsTollInHome &&
841 motor->stepToReach < motor->cfg->stepsInHome - motor->cfg->stepsTollInHome)
843 value = MOTOR_HOMESENSOR_INCHECK;
844 // home sensor out max position
845 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollInHome - MOTOR_CONSECUTIVE_ERROR_STEPS;
846 // home sensor in max position
847 if(motor->cfg->stepsInHome > motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS)
848 motor->stepsTollMin = motor->cfg->stepsInHome - (motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS);
852 setCheckSensor(motor, value);
856 * Move motor to absolute position at specified speed
858 * \arg steps position to reach in steps
859 * \arg speed speed in timer ticks (use TIME2CLOCKS() to convert)
861 int16_t stepper_move(struct Stepper *motor, int16_t steps, uint16_t speed, int16_t deafstep)
863 // if the stepper already is in the desired position -> nothing to do
864 if (motor->step == steps)
867 stepper_disable_irq(motor);
870 motor->stepToReach = steps;
873 motor->stepsErrorHome = 0;
875 // position to start level check
876 motor->stepsDeaf = deafstep;
878 // clear level position
879 motor->stepsLevel = 0;
881 if (speed < motor->cfg->ramp.clocksMinWL)
883 ASSERT2(0, "speed too fast (small number)");
884 speed = motor->cfg->ramp.clocksMinWL;
887 motor->rampClock = motor->cfg->ramp.clocksMaxWL;
888 motor->rampValue = motor->cfg->ramp.clocksMaxWL;
890 // TODO: find the exact value for motor->speed searching in the ramp array.
891 motor->speed = speed;
893 stepper_enable_irq(motor);
900 * Stop motor gracefully
902 void stepper_stop(struct Stepper *motor)
905 * The best way is to set the target of the movement to the minimum
906 * distance needed to decelerate. The logic in FSM_run will do the rest.
908 if(stepper_idle(motor))
911 stepper_disable_irq(motor);
912 motor->stepToReach = motor->step + motor->rampStep * motor->dir;
913 stepper_enable_irq(motor);
918 * Stop motor immediately, changing the status
920 void stepper_break(struct Stepper *motor, enum StepperState state)
922 // The best way to abort any operation is to go back to pre-idle mode
923 stepper_disable_irq(motor);
925 // Set of Speed disabled and Steps reached so that the function
926 // stepper_idle() succeeds
927 motor->speed = SPEED_STOPPED;
928 motor->stepToReach = motor->step;
929 stepper_changeState(motor, state);
930 stepper_enable_irq(motor);
933 ///< Returns true if the stepper is in idle at the final position or in error:
934 // this means anyway that the motor is not moving
935 bool stepper_idle(struct Stepper *motor)
937 return (stepper_isState(motor, MSTS_ERROR) ||
938 (stepper_isState(motor, MSTS_IDLE) && motor->step == motor->stepToReach) );
941 ///< Returns true if the stepper is in error mode
942 bool stepper_error(struct Stepper *motor)
944 return (stepper_isState(motor, MSTS_ERROR));
947 ///< check the home sensor in zero position
948 bool stepper_inhome(struct Stepper *motor)
950 return(stepper_getStep(motor) == 0 &&
951 !stepper_readHome(motor) );