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