BeRTOS
stepper.c
Go to the documentation of this file.
00001 
00041 #include "stepper.h"
00042 
00043 #include "hw/hw_stepper.h"
00044 #include "hw/hw_sensor.h"
00045 
00046 #include "cfg/cfg_stepper.h"
00047 #include <cfg/debug.h>
00048 
00049 // Define logging setting (for cfg/log.h module).
00050 #define LOG_LEVEL   STEPPER_LOG_LEVEL
00051 #define LOG_FORMAT  STEPPER_LOG_FORMAT
00052 #include <cfg/log.h>
00053 
00054 #include <kern/proc.h>
00055 
00056 #include <algo/ramp.h>
00057 
00058 #include CPU_HEADER(stepper)
00059 
00060 #include <string.h>  // memset
00061 
00066 #define MOTOR_SWITCH_TICKS      60000   ///< Timer ticks to wait for 10ms
00067 #define MOTOR_SWITCH_COUNT          5   ///< Number of intervals, long 10ms, to wait before/after switching current off/on
00068 #define MOTOR_HOME_MAX_STEPS    30000   ///< Steps before giving up when trying to reach home
00069 #define MOTOR_CURRENT_TICKS      6000   ///< Number of intervals, long 10ms, to mantain high current
00070 // \}
00071 
00073 static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS];
00074 
00076 static fsm_state general_states[STEPPER_MAX_STATES];
00077 
00078 // IRQ functions for stepper motors
00079 static void stepper_interrupt(struct Stepper *motor);
00080 
00081 static void stepper_accel(struct Stepper *motor);
00082 static void stepper_decel(struct Stepper *motor);
00083 
00084 static bool stepper_isState(struct Stepper *motor, enum StepperState state);
00085 INLINE void stepper_changeState(struct Stepper *motor, enum StepperState newState);
00086 
00087 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive);
00088 
00089 #define MOTOR_INDEX(motor)     (motor->index)
00090 
00091 //------------------------------------------------------------------------
00092 
00093 INLINE bool setLowCurrent(struct Stepper* motor)
00094 {
00095     if (motor->power == motor->cfg->powerIdle)
00096         return false;
00097 
00098     motor->power = motor->cfg->powerIdle;
00099     STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerIdle);
00100 
00101     return true;
00102 }
00103 
00104 INLINE bool setHighCurrent(struct Stepper* motor)
00105 {
00106     if (motor->power == motor->cfg->powerRun)
00107         return false;
00108 
00109     motor->power = motor->cfg->powerRun;
00110     STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerRun);
00111     return true;
00112 }
00113 
00114 INLINE void setCheckSensor(struct Stepper* motor, enum MotorHomeSensorCheck value)
00115 {
00116     motor->enableCheckHome = value;
00117 }
00118 
00119 INLINE int8_t getCheckSensor(struct Stepper* motor)
00120 {
00121     return motor->enableCheckHome;
00122 }
00123 
00124 INLINE void setDirection(struct Stepper* motor, enum MotorDirection dir)
00125 {
00126     ASSERT(dir == DIR_POSITIVE || dir == DIR_NEGATIVE);
00127     motor->dir = dir;
00128 
00129     if (!motor->cfg->flags.axisInverted)
00130     {
00131         STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir == DIR_POSITIVE));
00132     }
00133     else
00134     {
00135         STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir != DIR_POSITIVE));
00136     }
00137 }
00138 
00143 INLINE void FAST_FUNC stepper_schedule_irq(struct Stepper* motor, stepper_time_t delay, bool do_step)
00144 {
00145 
00146     if (do_step)
00147     {
00148         // Record the step we just did
00149         motor->step += motor->dir;
00150         stepper_tc_doPulse(motor->timer);
00151     }
00152     else
00153         stepper_tc_skipPulse(motor->timer);
00154 
00155     stepper_tc_setDelay(motor->timer, delay);
00156 }
00157 
00158 
00159 static void stepper_accel(struct Stepper *motor)
00160 {
00161     DB(uint16_t old_val = motor->rampValue;)
00162     DB(uint32_t old_clock = motor->rampClock;)
00163 
00164     const struct Ramp *ramp = &motor->cfg->ramp;
00165 
00166     ASSERT(motor->rampClock != 0);
00167 
00168     motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
00169     motor->rampClock += motor->rampValue;
00170     motor->rampStep++;
00171 
00172     DB(if (old_val && motor->rampValue > old_val)
00173     {
00174         LOG_ERR("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL);
00175         LOG_ERR("    %04x @ %lu   -->   %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock);
00176     })
00177 
00178 }
00179 
00180 static void stepper_decel(struct Stepper *motor)
00181 {
00182     const struct Ramp *ramp = &motor->cfg->ramp;
00183     DB(uint16_t old_val = motor->rampValue;)
00184 
00185     motor->rampClock -= motor->rampValue;
00186     ASSERT(motor->rampClock != 0);
00187     motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
00188     motor->rampStep--;
00189     DB(ASSERT(!old_val || motor->rampValue >= old_val););
00190 }
00191 
00192 INLINE void stepper_enable_irq(struct Stepper* motor)
00193 {
00194     stepper_tc_irq_enable(motor->timer);
00195 }
00196 
00197 INLINE void stepper_disable_irq(struct Stepper* motor)
00198 {
00199     stepper_tc_irq_disable(motor->timer);
00200 }
00201 
00202 // the home sensor can be in the standard home list or in the digital
00203 // sensor list
00204 bool stepper_readHome(struct Stepper* motor)
00205 {
00206     return (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS) ?
00207         hw_home_sensor_read(motor->cfg->homeSensorIndex) :
00208         bld_hw_sensor_read(motor->cfg->homeSensorIndex - NUM_HOME_SENSORS);
00209 }
00210 
00211 bool stepper_readLevel(struct Stepper* motor)
00212 {
00213     return hw_level_sensor_read(motor->cfg->levelSensorIndex);
00214 }
00215 
00216 /************************************************************************/
00217 /* Finite-state machine to drive stepper logic from IRQ                 */
00218 /************************************************************************/
00219 
00220 INLINE void stepper_changeState(struct Stepper* motor, enum StepperState newState)
00221 {
00222     ASSERT(newState < STEPPER_MAX_STATES);
00223 
00224     motor->state = motor->cfg->states[newState];
00225     if (!motor->state)
00226         motor->state = general_states[newState];
00227     ASSERT(motor->state);
00228 }
00229 
00230 static bool stepper_isState(struct Stepper* motor, enum StepperState state)
00231 {
00232     return (motor->cfg->states[state]
00233             ? motor->cfg->states[state] == motor->state
00234             : general_states[state] == motor->state);
00235 }
00236 
00237 static bool stepper_checkHomeErrors(struct Stepper* motor)
00238 {
00239     bool home;
00240 
00241     home = stepper_readHome(motor);
00242 
00243     if (motor->enableCheckHome == MOTOR_HOMESENSOR_INCHECK && home
00244         && (!motor->stepCircular || motor->step < motor->stepCircular / 2))
00245         /*
00246         * if home Sensor check enabled in movement to 0 position and
00247         * the motor is in home increase the counter
00248         * for rotating motor we include the check that the motor is
00249         * inside the last "lap" (FIXME: check it better)
00250         */
00251         motor->stepsErrorHome++;
00252     else if (motor->enableCheckHome == MOTOR_HOMESENSOR_OUTCHECK && !home)
00253         /*
00254         * if home Sensor check enabled in movement from 0 position and
00255         * the motor is not in home increase the counter
00256         */
00257         motor->stepsErrorHome++;
00258     else
00259         // clear error steps counter
00260         motor->stepsErrorHome = 0;
00261 
00262     // if this is the last consecutive position in which the motor is in/out home ...
00263     ASSERT(motor->stepsErrorHome <= MOTOR_CONSECUTIVE_ERROR_STEPS);
00264     if (motor->stepsErrorHome >= MOTOR_CONSECUTIVE_ERROR_STEPS)
00265     {
00266         // if the position at which the motor first saw/didn't see the home
00267         // is out of tolerance -> breakmotor -> ERROR
00268         if (motor->step > motor->stepsTollMax || motor->step < motor->stepsTollMin )
00269         {
00270             // break motor and error
00271             motor->speed = SPEED_STOPPED;
00272             motor->stepToReach = motor->step;
00273 
00274             stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00275             motor->skipIrqs = MOTOR_SWITCH_COUNT;
00276             return false;
00277         }
00278 
00279         // the motor reached the home crossing -> disable error check
00280         setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
00281     }
00282 
00283     return true;
00284 }
00285 
00286 static void stepper_checkLevelSensor(struct Stepper* motor)
00287 {
00288     // level sensor check
00289     if (motor->step > motor->stepsDeaf)
00290     {
00291         if (stepper_readLevel(motor))
00292         {
00293             // record current position, disable check and stop motor
00294             motor->stepsDeaf = DEAFSTEPS_DEFAULT;
00295             motor->stepsLevel = motor->step;
00296                         //motor->stepToReach = motor->step + motor->rampStep * motor->dir;
00297 
00298             motor->stepToReach = motor->step;
00299             motor->rampClock = motor->cfg->ramp.clocksMaxWL;
00300             motor->rampValue = motor->cfg->ramp.clocksMaxWL;
00301         }
00302     }
00303 }
00304 
00305 static enum StepperState FAST_FUNC FSM_run(struct Stepper *motor)
00306 {
00307     uint16_t distance;
00308 
00309     if (!stepper_checkHomeErrors(motor))
00310         return MSTS_ERROR;
00311 
00312     stepper_checkLevelSensor(motor);
00313 
00314     if ((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
00315         (motor->stepToReach != STEPS_INFINITE_NEGATIVE ))
00316     {
00317         // Calculate (always positive) distance between current position and destination step
00318         distance = (uint16_t)((motor->stepToReach - motor->step) * motor->dir);
00319     }
00320     else
00321     {
00322         // We're at a very long distance ;-)
00323         distance = 0xFFFF;
00324         // if the motor is rotating and it has just ran a complete round
00325         // the position is set to 0
00326         if(motor->step == motor->stepCircular)
00327             motor->step = 0;
00328     }
00329 
00330     if (distance == 0)
00331         // Position reached - stop motor
00332         //motor->speed = SPEED_STOPPED;
00333         motor->rampStep = -1;
00334         //motor->rampClock = motor->ramp->clocksMaxWL;
00335         //motor->rampValue = 0;
00336         //motor->rampClock = motor->rampValue = motor->ramp->clocksMaxWL;
00337 
00338     else if (distance <= (uint16_t)motor->rampStep)
00339         stepper_decel(motor);
00340 
00341     // check whether the velocity must be changed
00342     else if (motor->speed < (uint16_t)motor->rampValue)
00343     {
00344         stepper_accel(motor);
00345         if (motor->speed > (uint16_t)motor->rampValue)
00346             motor->speed = (uint16_t)motor->rampValue;
00347     }
00348     else if (motor->speed > (uint16_t)motor->rampValue)
00349         stepper_decel(motor);
00350 
00351     // If rampStep == -1, leave output pin high and wait for low current
00352     if (motor->rampStep < 0)
00353     {
00354         // Wait before switching to low current
00355         motor->speed = SPEED_STOPPED;
00356 
00357         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00358         motor->skipIrqs = MOTOR_SWITCH_COUNT;
00359 
00360         /*
00361          * If there was a home sensor check activated, and the check has not
00362          * been done yet, it means that we reached the end position without
00363          * finding the home (or exiting from it). This is bad!
00364          */
00365         if (motor->enableCheckHome != MOTOR_HOMESENSOR_NOCHECK)
00366             return MSTS_ERROR;
00367 
00368         // check if the motor has to stay in high current
00369         if(motor->cfg->flags.highcurrentBit)
00370         {
00371             motor->changeCurrentIrqs = MOTOR_CURRENT_TICKS;
00372             return MSTS_IDLE;
00373         }
00374 
00375         return MSTS_PREIDLE;
00376     }
00377 
00378     // Wait for high->low transition
00379     ASSERT(motor->rampValue > motor->cfg->pulse);
00380     stepper_schedule_irq(motor, motor->rampValue, true);
00381 
00382     return MSTS_RUN;
00383 }
00384 
00385 static enum StepperState FSM_idle(struct Stepper* motor)
00386 {
00387     stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00388 
00389     if (motor->speed == SPEED_STOPPED)
00390     {
00391         // check if it's time to switch to low current
00392         if(motor->changeCurrentIrqs > 0)
00393         {
00394             if(--motor->changeCurrentIrqs == 0)
00395                 setLowCurrent(motor);
00396         }
00397         return MSTS_IDLE;
00398     }
00399 
00400     // Switch to high current and wait for stabilization
00401     // (if the motor is in low current)
00402     if(motor->changeCurrentIrqs == 0)
00403     {
00404         setHighCurrent(motor);
00405         motor->skipIrqs = MOTOR_SWITCH_COUNT;
00406     }
00407 
00408     return MSTS_PRERUN;
00409 }
00410 
00411 static enum StepperState FSM_preidle(struct Stepper* motor)
00412 {
00413     // Normal operation mode
00414     motor->changeCurrentIrqs = 0;
00415     setLowCurrent(motor);
00416     stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00417     return MSTS_IDLE;
00418 }
00419 
00420 static enum StepperState FSM_error(struct Stepper* motor)
00421 {
00422     // Error condition mode
00423     setLowCurrent(motor);
00424     stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00425     return MSTS_ERROR;
00426 }
00427 
00428 static enum StepperState FSM_prerun(struct Stepper* motor)
00429 {
00430     enum MotorDirection dir;
00431 
00432     // distance != 0?
00433     if ((motor->stepToReach != motor->step) ||
00434         (motor->stepToReach == STEPS_INFINITE_POSITIVE) ||
00435         (motor->stepToReach == STEPS_INFINITE_NEGATIVE)  )
00436     {
00437         // Setup for first step
00438         motor->rampStep = 0;
00439 
00440         // Setup Direction
00441         if(motor->stepToReach == STEPS_INFINITE_POSITIVE)
00442             dir = DIR_POSITIVE;
00443         else if(motor->stepToReach == STEPS_INFINITE_NEGATIVE)
00444              dir = DIR_NEGATIVE;
00445         else if(motor->stepToReach > motor->step)
00446             dir = DIR_POSITIVE;
00447         else
00448              dir = DIR_NEGATIVE;
00449 
00450         setDirection(motor, dir);
00451 
00452         // Enable of the home sensor control, if necessary
00453         // (before calling this function set the motor direction as above)
00454         stepper_enableCheckHome(motor, (dir == DIR_POSITIVE));
00455 
00456         // if the movement is infinite negative set the sw direction positive
00457         // (not the hw: see below) to count the steps
00458         if(motor->stepToReach == STEPS_INFINITE_NEGATIVE) motor->dir = DIR_POSITIVE;
00459 
00460         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00461         return MSTS_RUN;
00462     }
00463     else
00464     {
00465         /*
00466          * If we are here we should do at least one step.
00467          *  anyway ....
00468          */
00469         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00470         motor->skipIrqs = MOTOR_SWITCH_COUNT;
00471         return MSTS_PREIDLE;
00472     }
00473 }
00474 
00475 static enum StepperState FSM_preinit(struct Stepper* motor)
00476 {
00477     // Set current high, and wait for stabilization
00478     if (setHighCurrent(motor))
00479     {
00480         motor->skipIrqs = MOTOR_SWITCH_COUNT;
00481         return MSTS_PREINIT;
00482     }
00483 
00484     /*
00485      * This state is used when initializing the motor, to bring back
00486      * to the home. The idea is that we do not know where the motor
00487      * is at this point, so there can be two possibilities:
00488      *
00489      * - The motor is already in home. We do not know how much into the
00490      *   home we are. So we need to get out of the home (MSTS_LEAVING)
00491      *   and then get back into it of the desired number of steps.
00492      *
00493      * - The motor is not in home: we need to look for it (MSTS_INIT).
00494      *   We can safely assume that we will find the home in the negative
00495      *   direction. For circular motors, any direction would do. For
00496      *   other motors, the home is set at zero, so the current position
00497      *   has to be a positive value.
00498      *
00499      */
00500     if (stepper_readHome(motor))
00501     {
00502         setDirection(motor, DIR_POSITIVE);
00503         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00504         return MSTS_LEAVING;
00505     }
00506 
00507     setDirection(motor, DIR_NEGATIVE);
00508     stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00509     return MSTS_INIT;
00510 }
00511 
00512 
00513 static enum StepperState FSM_init(struct Stepper* motor)
00514 {
00515     // If we are not in home, keep looking
00516     if (!stepper_readHome(motor))
00517     {
00518         stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
00519         return MSTS_INIT;
00520     }
00521 
00522     /*
00523      * Home! We still need to enter the home of the specified number of steps.
00524      * That will be our absolute zero.
00525      */
00526 
00527     motor->step = motor->cfg->stepsInHome - 1; // start counting down steps in home
00528     motor->stepToReach = 0;
00529 
00530     stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
00531     return MSTS_ENTERING;
00532 }
00533 
00534 static enum StepperState FSM_entering(struct Stepper* motor)
00535 {
00536     // We must be in home
00537     //ASSERT(stepper_readHome(motor));
00538 
00539     // if while entering the sensor we are no more in home we reset the steps
00540     // counter (optical sensor)
00541     if(!stepper_readHome(motor))
00542         motor->step = motor->cfg->stepsInHome - 1;
00543 
00544     // Current Position must be non-negative
00545     ASSERT(motor->step >= 0);
00546 
00547     if(motor->step == 0)
00548     {
00549         // reach the final target inside home sensor
00550         motor->step = 0;
00551         return MSTS_PREIDLE;
00552     }
00553 
00554     // keep doing steps
00555     stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
00556     return MSTS_ENTERING;
00557 }
00558 
00559 static enum StepperState FSM_leaving(struct Stepper* motor)
00560 {
00561     ASSERT(motor->dir == DIR_POSITIVE);
00562 
00563     motor->step = 0;
00564     if (!stepper_readHome(motor))
00565     {
00566         // we are out of home : change state and going far from sensor
00567         stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
00568         return MSTS_OUTHOME;
00569     }
00570     else
00571     {
00572         // Still at home. Just wait here and keep doing steps
00573         stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
00574         return MSTS_LEAVING;
00575     }
00576 }
00577 
00578 static enum StepperState FSM_outhome(struct Stepper* motor)
00579 {
00580     ASSERT(motor->dir == DIR_POSITIVE);
00581 
00582     // We must be out of home: once we are no more in home
00583     // we just need to move away, even if not very precide (optical sensor)
00584     // ASSERT(!stepper_readHome(motor));
00585 
00586     if(motor->step >= motor->cfg->stepsOutHome)
00587     {
00588         // reach the final target outside home sensor
00589         motor->step = 0;
00590 
00591         // start home entering procedure (delay in executing step)
00592         setDirection(motor, DIR_NEGATIVE);
00593         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00594         motor->skipIrqs = MOTOR_SWITCH_COUNT;
00595         return MSTS_INIT;
00596     }
00597 
00598     // keep doing steps
00599     stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
00600     return MSTS_OUTHOME;
00601 }
00602 
00603 static void FAST_FUNC stepper_interrupt(struct Stepper *motor)
00604 {
00605     enum StepperState newState;
00606 
00607     // Check if we need to skip a certain number of IRQs
00608     if (motor->skipIrqs)
00609     {
00610         --motor->skipIrqs;
00611         stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00612         return;
00613     }
00614 
00615     ASSERT(motor->state);
00616     newState = motor->state(motor);
00617     stepper_changeState(motor, newState);
00618 }
00619 
00620 
00621 
00622 
00623 /************************************************************************/
00624 /* Public API                                                           */
00625 /************************************************************************/
00626 
00630 void stepper_init(void)
00631 {
00632     STEPPER_INIT();
00633 
00634     // before starting the power all the stepper enable must be surely low
00635     stepper_disable();
00636 
00637     // Bind functions to general states
00638     memset(general_states, 0, sizeof(general_states));
00639     general_states[MSTS_IDLE] = FSM_idle;
00640     general_states[MSTS_PREIDLE] = FSM_preidle;
00641     general_states[MSTS_PRERUN] = FSM_prerun;
00642     general_states[MSTS_RUN] = FSM_run;
00643     general_states[MSTS_PREINIT] = FSM_preinit;
00644     general_states[MSTS_INIT] = FSM_init;
00645     general_states[MSTS_ENTERING] = FSM_entering;
00646     general_states[MSTS_LEAVING]= FSM_leaving;
00647     general_states[MSTS_OUTHOME]= FSM_outhome;
00648     general_states[MSTS_ERROR]= FSM_error;
00649 }
00650 
00651 void stepper_end(void)
00652 {
00653     // Disable all stepper timer interrupt to stop motors
00654     for (int i = 0; i < CONFIG_NUM_STEPPER_MOTORS; i++)
00655         stepper_disable_irq(&all_motors[i]);
00656 }
00657 
00661 struct Stepper* stepper_setup(int index, struct StepperConfig *cfg)
00662 {
00663     struct Stepper* motor;
00664 
00665     ASSERT(index < CONFIG_NUM_STEPPER_MOTORS);
00666 
00667     motor = &all_motors[index];
00668     motor->index = index;
00669     motor->cfg = cfg;
00670 
00671     //Register timer to stepper, and enable irq
00672     stepper_tc_setup(motor->index, &stepper_interrupt, motor);
00673 
00674     stepper_reset(motor);
00675 
00676     stepper_enable_irq(motor);
00677 
00678     return motor;
00679 }
00680 
00684 void stepper_disable(void)
00685 {
00686     STEPPER_DISABLE_ALL();
00687 }
00688 
00692 void stepper_reset(struct Stepper *motor)
00693 {
00694     /*
00695      * To stop motor diable stepper irq.
00696      */
00697     stepper_disable_irq(motor);
00698 
00699     //Disable a stepper motor
00700     STEPPER_DISABLE(MOTOR_INDEX(motor));
00701 
00702     // Setup context variables
00703     motor->power = 0;
00704     motor->step = 0;
00705     motor->rampStep = -1;
00706     // We cannot set the clock at zero at start because of a limit in the fixed point ramp
00707     motor->rampClock = motor->cfg->ramp.clocksMaxWL;
00708     motor->rampValue = motor->cfg->ramp.clocksMaxWL;
00709     motor->speed = SPEED_STOPPED;
00710     motor->stepToReach = 0;
00711     motor->skipIrqs = 0;
00712     motor->stepCircular = 0;
00713     setDirection(motor, DIR_POSITIVE);
00714     setLowCurrent(motor);
00715 
00716     motor->changeCurrentIrqs = 0;
00717 
00718     // default value (disable level sensor check)
00719     motor->stepsDeaf = DEAFSTEPS_DEFAULT;
00720 
00721     STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
00722     STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
00723 
00724     if (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS)
00725         hw_home_sensor_set_inverted(motor->cfg->homeSensorIndex, motor->cfg->flags.homeInverted);
00726 
00727     if (motor->cfg->levelSensorIndex != MOTOR_NO_LEVEL_SENSOR)
00728         hw_level_sensor_set_inverted(motor->cfg->levelSensorIndex, motor->cfg->flags.levelInverted);
00729 
00730     stepper_changeState(motor, MSTS_IDLE);
00731 
00732     // Reset stepper timer counter
00733     stepper_tc_resetTimer(motor->timer);
00734 
00735     // reset hw to the stepper motor
00736     STEPPER_RESET(MOTOR_INDEX(motor));
00737     STEPPER_ENABLE(MOTOR_INDEX(motor));
00738 }
00739 
00740 
00741 void stepper_updateHalfStep(struct Stepper *motor)
00742 {
00743     STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
00744 }
00745 
00746 void stepper_updateControlBit(struct Stepper *motor)
00747 {
00748     STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
00749 }
00750 
00751 void stepper_updateControlMoveBit(struct Stepper *motor)
00752 {
00753     STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlMoveBit);
00754 }
00755 
00765 void stepper_home(struct Stepper *motor)
00766 {
00767 
00768     // Begin home procedure
00769     stepper_disable_irq(motor);
00770 
00771     // disable home sensor check (default)
00772     setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
00773     // deafult value (disable level sensor check)
00774     motor->stepsDeaf = DEAFSTEPS_DEFAULT;
00775 
00776     setDirection(motor, DIR_POSITIVE);
00777     stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
00778     stepper_changeState(motor, MSTS_PREINIT);
00779 
00780     stepper_enable_irq(motor);
00781 }
00782 
00783 
00784 void stepper_setStep(struct Stepper *motor, int16_t step)
00785 {
00786     motor->step = step;
00787 }
00788 
00789 
00790 int16_t stepper_getStep(struct Stepper *motor)
00791 {
00792     return motor->step;
00793 }
00794 
00795 int16_t stepper_getLevelStep(struct Stepper *motor)
00796 {
00797     return motor->stepsLevel;
00798 }
00799 
00800 void stepper_set_stepCircular(struct Stepper *motor, int16_t steps)
00801 {
00802     motor->stepCircular = steps;
00803 }
00804 
00805 int16_t stepper_get_stepCircular(struct Stepper *motor)
00806 {
00807     return motor->stepCircular;
00808 }
00809 
00810 int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir)
00811 {
00812     int16_t steps;
00813 
00814     // scale the current position inside the motor lap
00815     if(!motor->stepCircular) return 0;
00816 
00817     // to be sure ....
00818     while(motor->step > motor->stepCircular) motor->step -= motor->stepCircular;
00819 
00820     if(dir == DIR_NEGATIVE)
00821     {
00822         steps = ((motor->stepCircular - motor->step) % motor->stepCircular);
00823         motor->step = steps;
00824     }
00825     /*
00826     else
00827         steps = (motor->step % motor->stepCircular);
00828     motor->step = steps;
00829     */
00830     return motor->step;
00831 }
00832 
00833 static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive)
00834 {
00835     enum MotorHomeSensorCheck value = MOTOR_HOMESENSOR_NOCHECK; // default
00836 
00837     motor->stepsTollMin = 0;
00838 
00839     if((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
00840         (motor->stepToReach != STEPS_INFINITE_NEGATIVE)  )
00841     {
00842         if(bDirPositive) // else if(motor->dir == DIR_POSITIVE)
00843         {
00844             /* if the direction is positive (movement from 0 position),
00845              * if the starting position is inside home and the target position
00846              * is outside home -> the motor has to cross the home sensor -> enable the control
00847              */
00848             if (motor->step < motor->cfg->stepsInHome - motor->cfg->stepsTollOutHome &&
00849                 motor->stepToReach > motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome)
00850             {
00851                 value = MOTOR_HOMESENSOR_OUTCHECK;
00852                 // home sensor out max position
00853                 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome + MOTOR_CONSECUTIVE_ERROR_STEPS;
00854                 // home sensor in max position
00855                 if(motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS > motor->cfg->stepsTollOutHome)
00856                     motor->stepsTollMin = motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS - motor->cfg->stepsTollOutHome;
00857             }
00858         }
00859         else // if(motor->dir == DIR_NEGATIVE)
00860         {
00861             /*
00862              * if the direction is negative (movement to 0 position),
00863              * if the starting position is far from home and the target position
00864              * is inside home -> the motor has to cross the home sensor -> enable the control
00865              */
00866             if (motor->step > motor->cfg->stepsInHome + motor->cfg->stepsTollInHome &&
00867                 motor->stepToReach < motor->cfg->stepsInHome - motor->cfg->stepsTollInHome)
00868             {
00869                 value = MOTOR_HOMESENSOR_INCHECK;
00870                 // home sensor out max position
00871                 motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollInHome - MOTOR_CONSECUTIVE_ERROR_STEPS;
00872                 // home sensor in max position
00873                 if(motor->cfg->stepsInHome > motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS)
00874                     motor->stepsTollMin = motor->cfg->stepsInHome - (motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS);
00875             }
00876         }
00877     }
00878     setCheckSensor(motor, value);
00879 }
00880 
00887 int16_t stepper_move(struct Stepper *motor, int16_t steps, uint16_t speed, int16_t deafstep)
00888 {
00889     // if the stepper already is in the desired position -> nothing to do
00890     if (motor->step == steps)
00891         return 0;
00892 
00893     stepper_disable_irq(motor);
00894 
00895     // final position
00896     motor->stepToReach = steps;
00897 
00898     // clear error steps
00899     motor->stepsErrorHome = 0;
00900 
00901     // position to start level check
00902     motor->stepsDeaf = deafstep;
00903 
00904     // clear level position
00905     motor->stepsLevel = 0;
00906 
00907     if (speed < motor->cfg->ramp.clocksMinWL)
00908     {
00909         ASSERT2(0, "speed too fast (small number)");
00910         speed = motor->cfg->ramp.clocksMinWL;
00911     }
00912 
00913     motor->rampClock = motor->cfg->ramp.clocksMaxWL;
00914     motor->rampValue = motor->cfg->ramp.clocksMaxWL;
00915 
00916     // TODO: find the exact value for motor->speed searching  in the ramp array.
00917     motor->speed = speed;
00918 
00919     stepper_enable_irq(motor);
00920 
00921     return 0;
00922 }
00923 
00924 
00928 void stepper_stop(struct Stepper *motor)
00929 {
00930     /*
00931      * The best way is to set the target of the movement to the minimum
00932      * distance needed to decelerate. The logic in FSM_run will do the rest.
00933      */
00934     if(stepper_idle(motor))
00935         return;
00936 
00937     stepper_disable_irq(motor);
00938     motor->stepToReach = motor->step + motor->rampStep * motor->dir;
00939     stepper_enable_irq(motor);
00940 }
00941 
00942 
00946 void stepper_break(struct Stepper *motor, enum StepperState state)
00947 {
00948     // The best way to abort any operation is to go back to pre-idle mode
00949     stepper_disable_irq(motor);
00950 
00951     // Set of Speed disabled and Steps reached so that the function
00952     // stepper_idle() succeeds
00953     motor->speed = SPEED_STOPPED;
00954     motor->stepToReach = motor->step;
00955     stepper_changeState(motor, state);
00956     stepper_enable_irq(motor);
00957 }
00958 
00960 //  this means anyway that the motor is not moving
00961 bool stepper_idle(struct Stepper *motor)
00962 {
00963     return (stepper_isState(motor, MSTS_ERROR) ||
00964             (stepper_isState(motor, MSTS_IDLE) && motor->step == motor->stepToReach) );
00965 }
00966 
00968 bool stepper_error(struct Stepper *motor)
00969 {
00970     return (stepper_isState(motor, MSTS_ERROR));
00971 }
00972 
00974 bool stepper_inhome(struct Stepper *motor)
00975 {
00976     return(stepper_getStep(motor) == 0 &&
00977            !stepper_readHome(motor) );
00978 }