bertos/drv/stepper.c

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