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