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