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 }