BeRTOS
|
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 }