Fix the bug related to the empty version list
[bertos.git] / bertos / drv / stepper.c
1 /**
2  * \file
3  * <!--
4  * This file is part of BeRTOS.
5  *
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.
10  *
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.
15  *
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
19  *
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.
28  *
29  * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
30  * -->
31  *
32  * \brief Driver to control stepper motor
33  *
34  * \version $Id$
35  * \author Francesco Michelini <francesco.michelini@seacfi.com>
36  * \author Giovanni Bajo <rasky@develer.com>
37  * \author Bernie Innocenti <bernie@codewiz.org>
38  * \author Simone Zinanni <s.zinanni@develer.com>
39  * \author Daniele Basile <asterix@develer.com>
40  */
41
42 #include "stepper.h"
43
44 #include "hw/hw_stepper.h"
45 #include "hw/hw_sensor.h"
46
47 #include "cfg/cfg_stepper.h"
48 #include <cfg/debug.h>
49
50 // Define logging setting (for cfg/log.h module).
51 #define LOG_LEVEL   STEPPER_LOG_LEVEL
52 #define LOG_FORMAT  STEPPER_LOG_FORMAT
53 #include <cfg/log.h>
54
55 #include <kern/proc.h>
56
57 #include <algo/ramp.h>
58
59 #include CPU_HEADER(stepper)
60
61 #include <string.h>  // memset
62
63 /**
64  * \name Motor timings
65  * \{
66  */
67 #define MOTOR_SWITCH_TICKS      60000   ///< Timer ticks to wait for 10ms
68 #define MOTOR_SWITCH_COUNT          5   ///< Number of intervals, long 10ms, to wait before/after switching current off/on
69 #define MOTOR_HOME_MAX_STEPS    30000   ///< Steps before giving up when trying to reach home
70 #define MOTOR_CURRENT_TICKS      6000   ///< Number of intervals, long 10ms, to mantain high current
71 // \}
72
73 ///< Stepper motors
74 static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS];
75
76 ///< General FSM states (or NULL if state is not handled)
77 static fsm_state general_states[STEPPER_MAX_STATES];
78
79 // IRQ functions for stepper motors
80 static void stepper_interrupt(struct Stepper *motor);
81
82 static void stepper_accel(struct Stepper *motor);
83 static void stepper_decel(struct Stepper *motor);
84
85 static bool stepper_isState(struct Stepper *motor, enum StepperState state);
86 INLINE void stepper_changeState(struct Stepper *motor, enum StepperState newState);
87
88 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive);
89
90 #define MOTOR_INDEX(motor)     (motor->index)
91
92 //------------------------------------------------------------------------
93
94 INLINE bool setLowCurrent(struct Stepper* motor)
95 {
96         if (motor->power == motor->cfg->powerIdle)
97                 return false;
98
99         motor->power = motor->cfg->powerIdle;
100         STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerIdle);
101
102         return true;
103 }
104
105 INLINE bool setHighCurrent(struct Stepper* motor)
106 {
107         if (motor->power == motor->cfg->powerRun)
108                 return false;
109
110         motor->power = motor->cfg->powerRun;
111         STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerRun);
112         return true;
113 }
114
115 INLINE void setCheckSensor(struct Stepper* motor, enum MotorHomeSensorCheck value)
116 {
117         motor->enableCheckHome = value;
118 }
119
120 INLINE int8_t getCheckSensor(struct Stepper* motor)
121 {
122         return motor->enableCheckHome;
123 }
124
125 INLINE void setDirection(struct Stepper* motor, enum MotorDirection dir)
126 {
127         ASSERT(dir == DIR_POSITIVE || dir == DIR_NEGATIVE);
128         motor->dir = dir;
129
130         if (!motor->cfg->flags.axisInverted)
131         {
132                 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir == DIR_POSITIVE));
133         }
134         else
135         {
136                 STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir != DIR_POSITIVE));
137         }
138 }
139
140 /**
141  * Schedule a new stepper IRQ to happen after \a delay (number of clocks),
142  * and optionally doing a step at the same time (if \a do_step is true).
143  */
144 INLINE void FAST_FUNC stepper_schedule_irq(struct Stepper* motor, stepper_time_t delay, bool do_step)
145 {
146
147         if (do_step)
148         {
149                 // Record the step we just did
150                 motor->step += motor->dir;
151                 stepper_tc_doPulse(motor->timer);
152         }
153         else
154                 stepper_tc_skipPulse(motor->timer);
155
156         stepper_tc_setDelay(motor->timer, delay);
157 }
158
159
160 static void stepper_accel(struct Stepper *motor)
161 {
162         DB(uint16_t old_val = motor->rampValue;)
163         DB(uint32_t old_clock = motor->rampClock;)
164
165         const struct Ramp *ramp = &motor->cfg->ramp;
166
167         ASSERT(motor->rampClock != 0);
168
169         motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
170         motor->rampClock += motor->rampValue;
171         motor->rampStep++;
172
173         DB(if (old_val && motor->rampValue > old_val)
174         {
175                 LOG_ERR("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL);
176                 LOG_ERR("    %04x @ %lu   -->   %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock);
177         })
178
179 }
180
181 static void stepper_decel(struct Stepper *motor)
182 {
183         const struct Ramp *ramp = &motor->cfg->ramp;
184         DB(uint16_t old_val = motor->rampValue;)
185
186         motor->rampClock -= motor->rampValue;
187         ASSERT(motor->rampClock != 0);
188         motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
189         motor->rampStep--;
190         DB(ASSERT(!old_val || motor->rampValue >= old_val););
191 }
192
193 INLINE void stepper_enable_irq(struct Stepper* motor)
194 {
195         stepper_tc_irq_enable(motor->timer);
196 }
197
198 INLINE void stepper_disable_irq(struct Stepper* motor)
199 {
200         stepper_tc_irq_disable(motor->timer);
201 }
202
203 // the home sensor can be in the standard home list or in the digital
204 // sensor list
205 bool stepper_readHome(struct Stepper* motor)
206 {
207         return (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS) ?
208                 hw_home_sensor_read(motor->cfg->homeSensorIndex) :
209                 bld_hw_sensor_read(motor->cfg->homeSensorIndex - NUM_HOME_SENSORS);
210 }
211
212 bool stepper_readLevel(struct Stepper* motor)
213 {
214         return hw_level_sensor_read(motor->cfg->levelSensorIndex);
215 }
216
217 /************************************************************************/
218 /* Finite-state machine to drive stepper logic from IRQ                 */
219 /************************************************************************/
220
221 INLINE void stepper_changeState(struct Stepper* motor, enum StepperState newState)
222 {
223         ASSERT(newState < STEPPER_MAX_STATES);
224
225         motor->state = motor->cfg->states[newState];
226         if (!motor->state)
227                 motor->state = general_states[newState];
228         ASSERT(motor->state);
229 }
230
231 static bool stepper_isState(struct Stepper* motor, enum StepperState state)
232 {
233         return (motor->cfg->states[state]
234                 ? motor->cfg->states[state] == motor->state
235                 : general_states[state] == motor->state);
236 }
237
238 static bool stepper_checkHomeErrors(struct Stepper* motor)
239 {
240         bool home;
241
242         home = stepper_readHome(motor);
243
244         if (motor->enableCheckHome == MOTOR_HOMESENSOR_INCHECK && home
245                 && (!motor->stepCircular || motor->step < motor->stepCircular / 2))
246                 /*
247                 * if home Sensor check enabled in movement to 0 position and
248                 * the motor is in home increase the counter
249                 * for rotating motor we include the check that the motor is
250                 * inside the last "lap" (FIXME: check it better)
251                 */
252                 motor->stepsErrorHome++;
253         else if (motor->enableCheckHome == MOTOR_HOMESENSOR_OUTCHECK && !home)
254                 /*
255                 * if home Sensor check enabled in movement from 0 position and
256                 * the motor is not in home increase the counter
257                 */
258                 motor->stepsErrorHome++;
259         else
260                 // clear error steps counter
261                 motor->stepsErrorHome = 0;
262
263         // if this is the last consecutive position in which the motor is in/out home ...
264         ASSERT(motor->stepsErrorHome <= MOTOR_CONSECUTIVE_ERROR_STEPS);
265         if (motor->stepsErrorHome >= MOTOR_CONSECUTIVE_ERROR_STEPS)
266         {
267                 // if the position at which the motor first saw/didn't see the home
268                 // is out of tolerance -> breakmotor -> ERROR
269                 if (motor->step > motor->stepsTollMax || motor->step < motor->stepsTollMin )
270                 {
271                         // break motor and error
272                         motor->speed = SPEED_STOPPED;
273                         motor->stepToReach = motor->step;
274
275                         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
276                         motor->skipIrqs = MOTOR_SWITCH_COUNT;
277                         return false;
278                 }
279
280                 // the motor reached the home crossing -> disable error check
281                 setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
282         }
283
284         return true;
285 }
286
287 static void stepper_checkLevelSensor(struct Stepper* motor)
288 {
289         // level sensor check
290         if (motor->step > motor->stepsDeaf)
291         {
292                 if (stepper_readLevel(motor))
293                 {
294                         // record current position, disable check and stop motor
295                         motor->stepsDeaf = DEAFSTEPS_DEFAULT;
296                         motor->stepsLevel = motor->step;
297                         //motor->stepToReach = motor->step + motor->rampStep * motor->dir;
298
299                         motor->stepToReach = motor->step;
300                         motor->rampClock = motor->cfg->ramp.clocksMaxWL;
301                         motor->rampValue = motor->cfg->ramp.clocksMaxWL;
302                 }
303         }
304 }
305
306 static enum StepperState FAST_FUNC FSM_run(struct Stepper *motor)
307 {
308         uint16_t distance;
309
310         if (!stepper_checkHomeErrors(motor))
311                 return MSTS_ERROR;
312
313         stepper_checkLevelSensor(motor);
314
315         if ((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
316             (motor->stepToReach != STEPS_INFINITE_NEGATIVE ))
317         {
318                 // Calculate (always positive) distance between current position and destination step
319                 distance = (uint16_t)((motor->stepToReach - motor->step) * motor->dir);
320         }
321         else
322         {
323                 // We're at a very long distance ;-)
324                 distance = 0xFFFF;
325                 // if the motor is rotating and it has just ran a complete round
326                 // the position is set to 0
327                 if(motor->step == motor->stepCircular)
328                         motor->step = 0;
329         }
330
331         if (distance == 0)
332                 // Position reached - stop motor
333                 //motor->speed = SPEED_STOPPED;
334                 motor->rampStep = -1;
335                 //motor->rampClock = motor->ramp->clocksMaxWL;
336                 //motor->rampValue = 0;
337                 //motor->rampClock = motor->rampValue = motor->ramp->clocksMaxWL;
338
339         else if (distance <= (uint16_t)motor->rampStep)
340                 stepper_decel(motor);
341
342         // check whether the velocity must be changed
343         else if (motor->speed < (uint16_t)motor->rampValue)
344         {
345                 stepper_accel(motor);
346                 if (motor->speed > (uint16_t)motor->rampValue)
347                         motor->speed = (uint16_t)motor->rampValue;
348         }
349         else if (motor->speed > (uint16_t)motor->rampValue)
350                 stepper_decel(motor);
351
352         // If rampStep == -1, leave output pin high and wait for low current
353         if (motor->rampStep < 0)
354         {
355                 // Wait before switching to low current
356                 motor->speed = SPEED_STOPPED;
357
358                 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
359                 motor->skipIrqs = MOTOR_SWITCH_COUNT;
360
361                 /*
362                  * If there was a home sensor check activated, and the check has not
363                  * been done yet, it means that we reached the end position without
364                  * finding the home (or exiting from it). This is bad!
365                  */
366                 if (motor->enableCheckHome != MOTOR_HOMESENSOR_NOCHECK)
367                         return MSTS_ERROR;
368
369                 // check if the motor has to stay in high current
370                 if(motor->cfg->flags.highcurrentBit)
371                 {
372                         motor->changeCurrentIrqs = MOTOR_CURRENT_TICKS;
373                         return MSTS_IDLE;
374                 }
375
376                 return MSTS_PREIDLE;
377         }
378
379         // Wait for high->low transition
380         ASSERT(motor->rampValue > motor->cfg->pulse);
381         stepper_schedule_irq(motor, motor->rampValue, true);
382
383         return MSTS_RUN;
384 }
385
386 static enum StepperState FSM_idle(struct Stepper* motor)
387 {
388         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
389
390         if (motor->speed == SPEED_STOPPED)
391         {
392                 // check if it's time to switch to low current
393                 if(motor->changeCurrentIrqs > 0)
394                 {
395                         if(--motor->changeCurrentIrqs == 0)
396                                 setLowCurrent(motor);
397                 }
398                 return MSTS_IDLE;
399         }
400
401         // Switch to high current and wait for stabilization
402         // (if the motor is in low current)
403         if(motor->changeCurrentIrqs == 0)
404         {
405                 setHighCurrent(motor);
406                 motor->skipIrqs = MOTOR_SWITCH_COUNT;
407         }
408
409         return MSTS_PRERUN;
410 }
411
412 static enum StepperState FSM_preidle(struct Stepper* motor)
413 {
414         // Normal operation mode
415         motor->changeCurrentIrqs = 0;
416         setLowCurrent(motor);
417         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
418         return MSTS_IDLE;
419 }
420
421 static enum StepperState FSM_error(struct Stepper* motor)
422 {
423         // Error condition mode
424         setLowCurrent(motor);
425         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
426         return MSTS_ERROR;
427 }
428
429 static enum StepperState FSM_prerun(struct Stepper* motor)
430 {
431         enum MotorDirection dir;
432
433         // distance != 0?
434         if ((motor->stepToReach != motor->step) ||
435             (motor->stepToReach == STEPS_INFINITE_POSITIVE) ||
436             (motor->stepToReach == STEPS_INFINITE_NEGATIVE)  )
437         {
438                 // Setup for first step
439                 motor->rampStep = 0;
440
441                 // Setup Direction
442                 if(motor->stepToReach == STEPS_INFINITE_POSITIVE)
443                         dir = DIR_POSITIVE;
444                 else if(motor->stepToReach == STEPS_INFINITE_NEGATIVE)
445                          dir = DIR_NEGATIVE;
446                 else if(motor->stepToReach > motor->step)
447                         dir = DIR_POSITIVE;
448                 else
449                          dir = DIR_NEGATIVE;
450
451                 setDirection(motor, dir);
452
453                 // Enable of the home sensor control, if necessary
454                 // (before calling this function set the motor direction as above)
455                 stepper_enableCheckHome(motor, (dir == DIR_POSITIVE));
456
457                 // if the movement is infinite negative set the sw direction positive
458                 // (not the hw: see below) to count the steps
459                 if(motor->stepToReach == STEPS_INFINITE_NEGATIVE) motor->dir = DIR_POSITIVE;
460
461                 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
462                 return MSTS_RUN;
463         }
464         else
465         {
466                 /*
467                  * If we are here we should do at least one step.
468                  *  anyway ....
469                  */
470                 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
471                 motor->skipIrqs = MOTOR_SWITCH_COUNT;
472                 return MSTS_PREIDLE;
473         }
474 }
475
476 static enum StepperState FSM_preinit(struct Stepper* motor)
477 {
478         // Set current high, and wait for stabilization
479         if (setHighCurrent(motor))
480         {
481                 motor->skipIrqs = MOTOR_SWITCH_COUNT;
482                 return MSTS_PREINIT;
483         }
484
485         /*
486          * This state is used when initializing the motor, to bring back
487          * to the home. The idea is that we do not know where the motor
488          * is at this point, so there can be two possibilities:
489          *
490          * - The motor is already in home. We do not know how much into the
491          *   home we are. So we need to get out of the home (MSTS_LEAVING)
492          *   and then get back into it of the desired number of steps.
493          *
494          * - The motor is not in home: we need to look for it (MSTS_INIT).
495          *   We can safely assume that we will find the home in the negative
496          *   direction. For circular motors, any direction would do. For
497          *   other motors, the home is set at zero, so the current position
498          *   has to be a positive value.
499          *
500          */
501         if (stepper_readHome(motor))
502         {
503                 setDirection(motor, DIR_POSITIVE);
504                 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
505                 return MSTS_LEAVING;
506         }
507
508         setDirection(motor, DIR_NEGATIVE);
509         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
510         return MSTS_INIT;
511 }
512
513
514 static enum StepperState FSM_init(struct Stepper* motor)
515 {
516         // If we are not in home, keep looking
517         if (!stepper_readHome(motor))
518         {
519                 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
520                 return MSTS_INIT;
521         }
522
523         /*
524          * Home! We still need to enter the home of the specified number of steps.
525          * That will be our absolute zero.
526          */
527
528         motor->step = motor->cfg->stepsInHome - 1; // start counting down steps in home
529         motor->stepToReach = 0;
530
531         stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
532         return MSTS_ENTERING;
533 }
534
535 static enum StepperState FSM_entering(struct Stepper* motor)
536 {
537         // We must be in home
538         //ASSERT(stepper_readHome(motor));
539
540         // if while entering the sensor we are no more in home we reset the steps
541         // counter (optical sensor)
542         if(!stepper_readHome(motor))
543                 motor->step = motor->cfg->stepsInHome - 1;
544
545         // Current Position must be non-negative
546         ASSERT(motor->step >= 0);
547
548         if(motor->step == 0)
549         {
550                 // reach the final target inside home sensor
551                 motor->step = 0;
552                 return MSTS_PREIDLE;
553         }
554
555         // keep doing steps
556         stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
557         return MSTS_ENTERING;
558 }
559
560 static enum StepperState FSM_leaving(struct Stepper* motor)
561 {
562         ASSERT(motor->dir == DIR_POSITIVE);
563
564         motor->step = 0;
565         if (!stepper_readHome(motor))
566         {
567                 // we are out of home : change state and going far from sensor
568                 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
569                 return MSTS_OUTHOME;
570         }
571         else
572         {
573                 // Still at home. Just wait here and keep doing steps
574                 stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
575                 return MSTS_LEAVING;
576         }
577 }
578
579 static enum StepperState FSM_outhome(struct Stepper* motor)
580 {
581     ASSERT(motor->dir == DIR_POSITIVE);
582
583     // We must be out of home: once we are no more in home
584     // we just need to move away, even if not very precide (optical sensor)
585     // ASSERT(!stepper_readHome(motor));
586
587     if(motor->step >= motor->cfg->stepsOutHome)
588     {
589         // reach the final target outside home sensor
590         motor->step = 0;
591
592         // start home entering procedure (delay in executing step)
593         setDirection(motor, DIR_NEGATIVE);
594         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
595                 motor->skipIrqs = MOTOR_SWITCH_COUNT;
596         return MSTS_INIT;
597     }
598
599     // keep doing steps
600     stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
601     return MSTS_OUTHOME;
602 }
603
604 static void FAST_FUNC stepper_interrupt(struct Stepper *motor)
605 {
606         enum StepperState newState;
607
608         // Check if we need to skip a certain number of IRQs
609         if (motor->skipIrqs)
610         {
611                 --motor->skipIrqs;
612                 stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
613                 return;
614         }
615
616         ASSERT(motor->state);
617         newState = motor->state(motor);
618         stepper_changeState(motor, newState);
619 }
620
621
622
623
624 /************************************************************************/
625 /* Public API                                                           */
626 /************************************************************************/
627
628 /**
629  * Initialize the stepper module
630  */
631 void stepper_init(void)
632 {
633         STEPPER_INIT();
634
635         // before starting the power all the stepper enable must be surely low
636         stepper_disable();
637
638         // Bind functions to general states
639         memset(general_states, 0, sizeof(general_states));
640         general_states[MSTS_IDLE] = FSM_idle;
641         general_states[MSTS_PREIDLE] = FSM_preidle;
642         general_states[MSTS_PRERUN] = FSM_prerun;
643         general_states[MSTS_RUN] = FSM_run;
644         general_states[MSTS_PREINIT] = FSM_preinit;
645         general_states[MSTS_INIT] = FSM_init;
646         general_states[MSTS_ENTERING] = FSM_entering;
647         general_states[MSTS_LEAVING]= FSM_leaving;
648         general_states[MSTS_OUTHOME]= FSM_outhome;
649         general_states[MSTS_ERROR]= FSM_error;
650 }
651
652 void stepper_end(void)
653 {
654         // Disable all stepper timer interrupt to stop motors
655         for (int i = 0; i < CONFIG_NUM_STEPPER_MOTORS; i++)
656                 stepper_disable_irq(&all_motors[i]);
657 }
658
659 /**
660  * Apply a setup config to motor structure context
661  */
662 struct Stepper* stepper_setup(int index, struct StepperConfig *cfg)
663 {
664         struct Stepper* motor;
665
666         ASSERT(index < CONFIG_NUM_STEPPER_MOTORS);
667
668         motor = &all_motors[index];
669         motor->index = index;
670         motor->cfg = cfg;
671
672         //Register timer to stepper, and enable irq
673         stepper_tc_setup(motor->index, &stepper_interrupt, motor);
674
675         stepper_reset(motor);
676
677         stepper_enable_irq(motor);
678
679         return motor;
680 }
681
682 /**
683  * Set the enable for all the motors to 0 before switching on the power
684  */
685 void stepper_disable(void)
686 {
687         STEPPER_DISABLE_ALL();
688 }
689
690 /**
691  * Reset the motor
692  */
693 void stepper_reset(struct Stepper *motor)
694 {
695         /*
696          * To stop motor diable stepper irq.
697          */
698         stepper_disable_irq(motor);
699
700         //Disable a stepper motor
701         STEPPER_DISABLE(MOTOR_INDEX(motor));
702
703         // Setup context variables
704         motor->power = 0;
705         motor->step = 0;
706         motor->rampStep = -1;
707         // We cannot set the clock at zero at start because of a limit in the fixed point ramp
708         motor->rampClock = motor->cfg->ramp.clocksMaxWL;
709         motor->rampValue = motor->cfg->ramp.clocksMaxWL;
710         motor->speed = SPEED_STOPPED;
711         motor->stepToReach = 0;
712         motor->skipIrqs = 0;
713         motor->stepCircular = 0;
714         setDirection(motor, DIR_POSITIVE);
715         setLowCurrent(motor);
716
717         motor->changeCurrentIrqs = 0;
718
719         // default value (disable level sensor check)
720         motor->stepsDeaf = DEAFSTEPS_DEFAULT;
721
722         STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
723         STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
724
725         if (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS)
726                 hw_home_sensor_set_inverted(motor->cfg->homeSensorIndex, motor->cfg->flags.homeInverted);
727
728         if (motor->cfg->levelSensorIndex != MOTOR_NO_LEVEL_SENSOR)
729                 hw_level_sensor_set_inverted(motor->cfg->levelSensorIndex, motor->cfg->flags.levelInverted);
730
731         stepper_changeState(motor, MSTS_IDLE);
732
733         // Reset stepper timer counter
734         stepper_tc_resetTimer(motor->timer);
735
736         // reset hw to the stepper motor
737         STEPPER_RESET(MOTOR_INDEX(motor));
738         STEPPER_ENABLE(MOTOR_INDEX(motor));
739 }
740
741
742 void stepper_updateHalfStep(struct Stepper *motor)
743 {
744         STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
745 }
746
747 void stepper_updateControlBit(struct Stepper *motor)
748 {
749         STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
750 }
751
752 void stepper_updateControlMoveBit(struct Stepper *motor)
753 {
754         STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlMoveBit);
755 }
756
757 /**
758  * Find the home of a \a motor assuming no current knowledge about its position.
759  *
760  * This must be done when the motor is desynchronized with the firmware and
761  * we do not know anymore where it is.
762  *
763  * In normal operation mode, to go back to the home, it is sufficient to use
764  * move to step #0 with stepper_move, since the home is always at step #0.
765  */
766 void stepper_home(struct Stepper *motor)
767 {
768
769         // Begin home procedure
770         stepper_disable_irq(motor);
771
772         // disable home sensor check (default)
773         setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
774         // deafult value (disable level sensor check)
775         motor->stepsDeaf = DEAFSTEPS_DEFAULT;
776
777         setDirection(motor, DIR_POSITIVE);
778         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
779         stepper_changeState(motor, MSTS_PREINIT);
780
781         stepper_enable_irq(motor);
782 }
783
784
785 void stepper_setStep(struct Stepper *motor, int16_t step)
786 {
787         motor->step = step;
788 }
789
790
791 int16_t stepper_getStep(struct Stepper *motor)
792 {
793         return motor->step;
794 }
795
796 int16_t stepper_getLevelStep(struct Stepper *motor)
797 {
798         return motor->stepsLevel;
799 }
800
801 void stepper_set_stepCircular(struct Stepper *motor, int16_t steps)
802 {
803         motor->stepCircular = steps;
804 }
805
806 int16_t stepper_get_stepCircular(struct Stepper *motor)
807 {
808         return motor->stepCircular;
809 }
810
811 int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir)
812 {
813         int16_t steps;
814
815         // scale the current position inside the motor lap
816         if(!motor->stepCircular) return 0;
817
818         // to be sure ....
819         while(motor->step > motor->stepCircular) motor->step -= motor->stepCircular;
820
821         if(dir == DIR_NEGATIVE)
822         {
823                 steps = ((motor->stepCircular - motor->step) % motor->stepCircular);
824                 motor->step = steps;
825         }
826         /*
827         else
828                 steps = (motor->step % motor->stepCircular);
829         motor->step = steps;
830         */
831         return motor->step;
832 }
833
834 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive)
835 {
836         enum MotorHomeSensorCheck value = MOTOR_HOMESENSOR_NOCHECK; // default
837
838         motor->stepsTollMin = 0;
839
840     if((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
841                 (motor->stepToReach != STEPS_INFINITE_NEGATIVE)  )
842         {
843                 if(bDirPositive) // else if(motor->dir == DIR_POSITIVE)
844                 {
845                         /* if the direction is positive (movement from 0 position),
846                          * if the starting position is inside home and the target position
847                          * is outside home -> the motor has to cross the home sensor -> enable the control
848                          */
849                         if (motor->step < motor->cfg->stepsInHome - motor->cfg->stepsTollOutHome &&
850                                 motor->stepToReach > motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome)
851                         {
852                                 value = MOTOR_HOMESENSOR_OUTCHECK;
853                                 // home sensor out max position
854                                 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome + MOTOR_CONSECUTIVE_ERROR_STEPS;
855                                 // home sensor in max position
856                                 if(motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS > motor->cfg->stepsTollOutHome)
857                                         motor->stepsTollMin = motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS - motor->cfg->stepsTollOutHome;
858                         }
859                 }
860                 else // if(motor->dir == DIR_NEGATIVE)
861                 {
862                         /*
863                          * if the direction is negative (movement to 0 position),
864                          * if the starting position is far from home and the target position
865                          * is inside home -> the motor has to cross the home sensor -> enable the control
866                          */
867                         if (motor->step > motor->cfg->stepsInHome + motor->cfg->stepsTollInHome &&
868                             motor->stepToReach < motor->cfg->stepsInHome - motor->cfg->stepsTollInHome)
869                         {
870                                 value = MOTOR_HOMESENSOR_INCHECK;
871                                 // home sensor out max position
872                                 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollInHome - MOTOR_CONSECUTIVE_ERROR_STEPS;
873                                 // home sensor in max position
874                                 if(motor->cfg->stepsInHome > motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS)
875                                         motor->stepsTollMin = motor->cfg->stepsInHome - (motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS);
876                         }
877                 }
878         }
879         setCheckSensor(motor, value);
880 }
881
882 /**
883  * Move motor to absolute position at specified speed
884  *
885  * \arg steps position to reach in steps
886  * \arg speed speed in timer ticks (use TIME2CLOCKS() to convert)
887  */
888 int16_t stepper_move(struct Stepper *motor, int16_t steps, uint16_t speed, int16_t deafstep)
889 {
890         // if the stepper already is in the desired position -> nothing to do
891         if (motor->step == steps)
892                 return 0;
893
894         stepper_disable_irq(motor);
895
896         // final position
897         motor->stepToReach = steps;
898
899         // clear error steps
900         motor->stepsErrorHome = 0;
901
902         // position to start level check
903         motor->stepsDeaf = deafstep;
904
905         // clear level position
906         motor->stepsLevel = 0;
907
908         if (speed < motor->cfg->ramp.clocksMinWL)
909         {
910                 ASSERT2(0, "speed too fast (small number)");
911                 speed = motor->cfg->ramp.clocksMinWL;
912         }
913
914         motor->rampClock = motor->cfg->ramp.clocksMaxWL;
915         motor->rampValue = motor->cfg->ramp.clocksMaxWL;
916
917         // TODO: find the exact value for motor->speed searching  in the ramp array.
918         motor->speed = speed;
919
920         stepper_enable_irq(motor);
921
922         return 0;
923 }
924
925
926 /**
927  * Stop motor gracefully
928  */
929 void stepper_stop(struct Stepper *motor)
930 {
931         /*
932          * The best way is to set the target of the movement to the minimum
933          * distance needed to decelerate. The logic in FSM_run will do the rest.
934          */
935         if(stepper_idle(motor))
936                 return;
937
938         stepper_disable_irq(motor);
939         motor->stepToReach = motor->step + motor->rampStep * motor->dir;
940         stepper_enable_irq(motor);
941 }
942
943
944 /**
945  * Stop motor immediately, changing the status
946  */
947 void stepper_break(struct Stepper *motor, enum StepperState state)
948 {
949         // The best way to abort any operation is to go back to pre-idle mode
950         stepper_disable_irq(motor);
951
952         // Set of Speed disabled and Steps reached so that the function
953         // stepper_idle() succeeds
954         motor->speed = SPEED_STOPPED;
955         motor->stepToReach = motor->step;
956         stepper_changeState(motor, state);
957         stepper_enable_irq(motor);
958 }
959
960 ///< Returns true if the stepper is in idle at the final position or in error:
961 //  this means anyway that the motor is not moving
962 bool stepper_idle(struct Stepper *motor)
963 {
964         return (stepper_isState(motor, MSTS_ERROR) ||
965                 (stepper_isState(motor, MSTS_IDLE) && motor->step == motor->stepToReach) );
966 }
967
968 ///< Returns true if the stepper is in error mode
969 bool stepper_error(struct Stepper *motor)
970 {
971         return (stepper_isState(motor, MSTS_ERROR));
972 }
973
974 ///< check the home sensor in zero position
975 bool stepper_inhome(struct Stepper *motor)
976 {
977         return(stepper_getStep(motor) == 0 &&
978                    !stepper_readHome(motor) );
979 }