Mirrors
/
OpenModem
mirror of https://github.com/markqvist/OpenModem.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
OpenModem/bertos/drv/stepper.c

979 lines
27 KiB
C
Raw Blame History

/**
* \file
* <!--
* This file is part of BeRTOS.
*
* Bertos is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* As a special exception, you may use this file as part of a free software
* library without restriction. Specifically, if other files instantiate
* templates or use macros or inline functions from this file, or you compile
* this file and link it with other files to produce an executable, this
* file does not by itself cause the resulting executable to be covered by
* the GNU General Public License. This exception does not however
* invalidate any other reasons why the executable file might be covered by
* the GNU General Public License.
*
* Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
* -->
*
* \brief Driver to control stepper motor
*
* \author Francesco Michelini <francesco.michelini@seacfi.com>
* \author Giovanni Bajo <rasky@develer.com>
* \author Bernie Innocenti <bernie@codewiz.org>
* \author Simone Zinanni <s.zinanni@develer.com>
* \author Daniele Basile <asterix@develer.com>
*/
#include "stepper.h"
#include "hw/hw_stepper.h"
#include "hw/hw_sensor.h"
#include "cfg/cfg_stepper.h"
#include <cfg/debug.h>
// Define logging setting (for cfg/log.h module).
#define LOG_LEVEL STEPPER_LOG_LEVEL
#define LOG_FORMAT STEPPER_LOG_FORMAT
#include <cfg/log.h>
#include <kern/proc.h>
#include <algo/ramp.h>
#include CPU_HEADER(stepper)
#include <string.h> // memset
/**
* \name Motor timings
* \{
*/
#define MOTOR_SWITCH_TICKS 60000 ///< Timer ticks to wait for 10ms
#define MOTOR_SWITCH_COUNT 5 ///< Number of intervals, long 10ms, to wait before/after switching current off/on
#define MOTOR_HOME_MAX_STEPS 30000 ///< Steps before giving up when trying to reach home
#define MOTOR_CURRENT_TICKS 6000 ///< Number of intervals, long 10ms, to mantain high current
// \}
///< Stepper motors
static struct Stepper all_motors[CONFIG_NUM_STEPPER_MOTORS];
///< General FSM states (or NULL if state is not handled)
static fsm_state general_states[STEPPER_MAX_STATES];
// IRQ functions for stepper motors
static void stepper_interrupt(struct Stepper *motor);
static void stepper_accel(struct Stepper *motor);
static void stepper_decel(struct Stepper *motor);
static bool stepper_isState(struct Stepper *motor, enum StepperState state);
INLINE void stepper_changeState(struct Stepper *motor, enum StepperState newState);
static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive);
#define MOTOR_INDEX(motor) (motor->index)
//------------------------------------------------------------------------
INLINE bool setLowCurrent(struct Stepper* motor)
{
if (motor->power == motor->cfg->powerIdle)
return false;
motor->power = motor->cfg->powerIdle;
STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerIdle);
return true;
}
INLINE bool setHighCurrent(struct Stepper* motor)
{
if (motor->power == motor->cfg->powerRun)
return false;
motor->power = motor->cfg->powerRun;
STEPPER_SET_POWER_CURRENT(MOTOR_INDEX(motor), motor->cfg->powerRun);
return true;
}
INLINE void setCheckSensor(struct Stepper* motor, enum MotorHomeSensorCheck value)
{
motor->enableCheckHome = value;
}
INLINE int8_t getCheckSensor(struct Stepper* motor)
{
return motor->enableCheckHome;
}
INLINE void setDirection(struct Stepper* motor, enum MotorDirection dir)
{
ASSERT(dir == DIR_POSITIVE || dir == DIR_NEGATIVE);
motor->dir = dir;
if (!motor->cfg->flags.axisInverted)
{
STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir == DIR_POSITIVE));
}
else
{
STEPPER_SET_DIRECTION(MOTOR_INDEX(motor), (dir != DIR_POSITIVE));
}
}
/**
* Schedule a new stepper IRQ to happen after \a delay (number of clocks),
* and optionally doing a step at the same time (if \a do_step is true).
*/
INLINE void FAST_FUNC stepper_schedule_irq(struct Stepper* motor, stepper_time_t delay, bool do_step)
{
if (do_step)
{
// Record the step we just did
motor->step += motor->dir;
stepper_tc_doPulse(motor->timer);
}
else
stepper_tc_skipPulse(motor->timer);
stepper_tc_setDelay(motor->timer, delay);
}
static void stepper_accel(struct Stepper *motor)
{
DB(uint16_t old_val = motor->rampValue;)
DB(uint32_t old_clock = motor->rampClock;)
const struct Ramp *ramp = &motor->cfg->ramp;
ASSERT(motor->rampClock != 0);
motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
motor->rampClock += motor->rampValue;
motor->rampStep++;
DB(if (old_val && motor->rampValue > old_val)
{
LOG_ERR("Runtime ramp error: (max=%x, min=%x)\n", ramp->clocksMaxWL, ramp->clocksMinWL);
LOG_ERR(" %04x @ %lu --> %04x @ %lu\n", old_val, old_clock, motor->rampValue, motor->rampClock);
})
}
static void stepper_decel(struct Stepper *motor)
{
const struct Ramp *ramp = &motor->cfg->ramp;
DB(uint16_t old_val = motor->rampValue;)
motor->rampClock -= motor->rampValue;
ASSERT(motor->rampClock != 0);
motor->rampValue = ramp_evaluate(ramp, motor->rampClock);
motor->rampStep--;
DB(ASSERT(!old_val || motor->rampValue >= old_val););
}
INLINE void stepper_enable_irq(struct Stepper* motor)
{
stepper_tc_irq_enable(motor->timer);
}
INLINE void stepper_disable_irq(struct Stepper* motor)
{
stepper_tc_irq_disable(motor->timer);
}
// the home sensor can be in the standard home list or in the digital
// sensor list
bool stepper_readHome(struct Stepper* motor)
{
return (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS) ?
hw_home_sensor_read(motor->cfg->homeSensorIndex) :
bld_hw_sensor_read(motor->cfg->homeSensorIndex - NUM_HOME_SENSORS);
}
bool stepper_readLevel(struct Stepper* motor)
{
return hw_level_sensor_read(motor->cfg->levelSensorIndex);
}
/************************************************************************/
/* Finite-state machine to drive stepper logic from IRQ */
/************************************************************************/
INLINE void stepper_changeState(struct Stepper* motor, enum StepperState newState)
{
ASSERT(newState < STEPPER_MAX_STATES);
motor->state = motor->cfg->states[newState];
if (!motor->state)
motor->state = general_states[newState];
ASSERT(motor->state);
}
static bool stepper_isState(struct Stepper* motor, enum StepperState state)
{
return (motor->cfg->states[state]
? motor->cfg->states[state] == motor->state
: general_states[state] == motor->state);
}
static bool stepper_checkHomeErrors(struct Stepper* motor)
{
bool home;
home = stepper_readHome(motor);
if (motor->enableCheckHome == MOTOR_HOMESENSOR_INCHECK && home
&& (!motor->stepCircular || motor->step < motor->stepCircular / 2))
/*
* if home Sensor check enabled in movement to 0 position and
* the motor is in home increase the counter
* for rotating motor we include the check that the motor is
* inside the last "lap" (FIXME: check it better)
*/
motor->stepsErrorHome++;
else if (motor->enableCheckHome == MOTOR_HOMESENSOR_OUTCHECK && !home)
/*
* if home Sensor check enabled in movement from 0 position and
* the motor is not in home increase the counter
*/
motor->stepsErrorHome++;
else
// clear error steps counter
motor->stepsErrorHome = 0;
// if this is the last consecutive position in which the motor is in/out home ...
ASSERT(motor->stepsErrorHome <= MOTOR_CONSECUTIVE_ERROR_STEPS);
if (motor->stepsErrorHome >= MOTOR_CONSECUTIVE_ERROR_STEPS)
{
// if the position at which the motor first saw/didn't see the home
// is out of tolerance -> breakmotor -> ERROR
if (motor->step > motor->stepsTollMax || motor->step < motor->stepsTollMin )
{
// break motor and error
motor->speed = SPEED_STOPPED;
motor->stepToReach = motor->step;
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
motor->skipIrqs = MOTOR_SWITCH_COUNT;
return false;
}
// the motor reached the home crossing -> disable error check
setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
}
return true;
}
static void stepper_checkLevelSensor(struct Stepper* motor)
{
// level sensor check
if (motor->step > motor->stepsDeaf)
{
if (stepper_readLevel(motor))
{
// record current position, disable check and stop motor
motor->stepsDeaf = DEAFSTEPS_DEFAULT;
motor->stepsLevel = motor->step;
//motor->stepToReach = motor->step + motor->rampStep * motor->dir;
motor->stepToReach = motor->step;
motor->rampClock = motor->cfg->ramp.clocksMaxWL;
motor->rampValue = motor->cfg->ramp.clocksMaxWL;
}
}
}
static enum StepperState FAST_FUNC FSM_run(struct Stepper *motor)
{
uint16_t distance;
if (!stepper_checkHomeErrors(motor))
return MSTS_ERROR;
stepper_checkLevelSensor(motor);
if ((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
(motor->stepToReach != STEPS_INFINITE_NEGATIVE ))
{
// Calculate (always positive) distance between current position and destination step
distance = (uint16_t)((motor->stepToReach - motor->step) * motor->dir);
}
else
{
// We're at a very long distance ;-)
distance = 0xFFFF;
// if the motor is rotating and it has just ran a complete round
// the position is set to 0
if(motor->step == motor->stepCircular)
motor->step = 0;
}
if (distance == 0)
// Position reached - stop motor
//motor->speed = SPEED_STOPPED;
motor->rampStep = -1;
//motor->rampClock = motor->ramp->clocksMaxWL;
//motor->rampValue = 0;
//motor->rampClock = motor->rampValue = motor->ramp->clocksMaxWL;
else if (distance <= (uint16_t)motor->rampStep)
stepper_decel(motor);
// check whether the velocity must be changed
else if (motor->speed < (uint16_t)motor->rampValue)
{
stepper_accel(motor);
if (motor->speed > (uint16_t)motor->rampValue)
motor->speed = (uint16_t)motor->rampValue;
}
else if (motor->speed > (uint16_t)motor->rampValue)
stepper_decel(motor);
// If rampStep == -1, leave output pin high and wait for low current
if (motor->rampStep < 0)
{
// Wait before switching to low current
motor->speed = SPEED_STOPPED;
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
motor->skipIrqs = MOTOR_SWITCH_COUNT;
/*
* If there was a home sensor check activated, and the check has not
* been done yet, it means that we reached the end position without
* finding the home (or exiting from it). This is bad!
*/
if (motor->enableCheckHome != MOTOR_HOMESENSOR_NOCHECK)
return MSTS_ERROR;
// check if the motor has to stay in high current
if(motor->cfg->flags.highcurrentBit)
{
motor->changeCurrentIrqs = MOTOR_CURRENT_TICKS;
return MSTS_IDLE;
}
return MSTS_PREIDLE;
}
// Wait for high->low transition
ASSERT(motor->rampValue > motor->cfg->pulse);
stepper_schedule_irq(motor, motor->rampValue, true);
return MSTS_RUN;
}
static enum StepperState FSM_idle(struct Stepper* motor)
{
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
if (motor->speed == SPEED_STOPPED)
{
// check if it's time to switch to low current
if(motor->changeCurrentIrqs > 0)
{
if(--motor->changeCurrentIrqs == 0)
setLowCurrent(motor);
}
return MSTS_IDLE;
}
// Switch to high current and wait for stabilization
// (if the motor is in low current)
if(motor->changeCurrentIrqs == 0)
{
setHighCurrent(motor);
motor->skipIrqs = MOTOR_SWITCH_COUNT;
}
return MSTS_PRERUN;
}
static enum StepperState FSM_preidle(struct Stepper* motor)
{
// Normal operation mode
motor->changeCurrentIrqs = 0;
setLowCurrent(motor);
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
return MSTS_IDLE;
}
static enum StepperState FSM_error(struct Stepper* motor)
{
// Error condition mode
setLowCurrent(motor);
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
return MSTS_ERROR;
}
static enum StepperState FSM_prerun(struct Stepper* motor)
{
enum MotorDirection dir;
// distance != 0?
if ((motor->stepToReach != motor->step) ||
(motor->stepToReach == STEPS_INFINITE_POSITIVE) ||
(motor->stepToReach == STEPS_INFINITE_NEGATIVE) )
{
// Setup for first step
motor->rampStep = 0;
// Setup Direction
if(motor->stepToReach == STEPS_INFINITE_POSITIVE)
dir = DIR_POSITIVE;
else if(motor->stepToReach == STEPS_INFINITE_NEGATIVE)
dir = DIR_NEGATIVE;
else if(motor->stepToReach > motor->step)
dir = DIR_POSITIVE;
else
dir = DIR_NEGATIVE;
setDirection(motor, dir);
// Enable of the home sensor control, if necessary
// (before calling this function set the motor direction as above)
stepper_enableCheckHome(motor, (dir == DIR_POSITIVE));
// if the movement is infinite negative set the sw direction positive
// (not the hw: see below) to count the steps
if(motor->stepToReach == STEPS_INFINITE_NEGATIVE) motor->dir = DIR_POSITIVE;
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
return MSTS_RUN;
}
else
{
/*
* If we are here we should do at least one step.
* anyway ....
*/
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
motor->skipIrqs = MOTOR_SWITCH_COUNT;
return MSTS_PREIDLE;
}
}
static enum StepperState FSM_preinit(struct Stepper* motor)
{
// Set current high, and wait for stabilization
if (setHighCurrent(motor))
{
motor->skipIrqs = MOTOR_SWITCH_COUNT;
return MSTS_PREINIT;
}
/*
* This state is used when initializing the motor, to bring back
* to the home. The idea is that we do not know where the motor
* is at this point, so there can be two possibilities:
*
* - The motor is already in home. We do not know how much into the
* home we are. So we need to get out of the home (MSTS_LEAVING)
* and then get back into it of the desired number of steps.
*
* - The motor is not in home: we need to look for it (MSTS_INIT).
* We can safely assume that we will find the home in the negative
* direction. For circular motors, any direction would do. For
* other motors, the home is set at zero, so the current position
* has to be a positive value.
*
*/
if (stepper_readHome(motor))
{
setDirection(motor, DIR_POSITIVE);
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
return MSTS_LEAVING;
}
setDirection(motor, DIR_NEGATIVE);
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
return MSTS_INIT;
}
static enum StepperState FSM_init(struct Stepper* motor)
{
// If we are not in home, keep looking
if (!stepper_readHome(motor))
{
stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
return MSTS_INIT;
}
/*
* Home! We still need to enter the home of the specified number of steps.
* That will be our absolute zero.
*/
motor->step = motor->cfg->stepsInHome - 1; // start counting down steps in home
motor->stepToReach = 0;
stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
return MSTS_ENTERING;
}
static enum StepperState FSM_entering(struct Stepper* motor)
{
// We must be in home
//ASSERT(stepper_readHome(motor));
// if while entering the sensor we are no more in home we reset the steps
// counter (optical sensor)
if(!stepper_readHome(motor))
motor->step = motor->cfg->stepsInHome - 1;
// Current Position must be non-negative
ASSERT(motor->step >= 0);
if(motor->step == 0)
{
// reach the final target inside home sensor
motor->step = 0;
return MSTS_PREIDLE;
}
// keep doing steps
stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
return MSTS_ENTERING;
}
static enum StepperState FSM_leaving(struct Stepper* motor)
{
ASSERT(motor->dir == DIR_POSITIVE);
motor->step = 0;
if (!stepper_readHome(motor))
{
// we are out of home : change state and going far from sensor
stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
return MSTS_OUTHOME;
}
else
{
// Still at home. Just wait here and keep doing steps
stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
return MSTS_LEAVING;
}
}
static enum StepperState FSM_outhome(struct Stepper* motor)
{
ASSERT(motor->dir == DIR_POSITIVE);
// We must be out of home: once we are no more in home
// we just need to move away, even if not very precide (optical sensor)
// ASSERT(!stepper_readHome(motor));
if(motor->step >= motor->cfg->stepsOutHome)
{
// reach the final target outside home sensor
motor->step = 0;
// start home entering procedure (delay in executing step)
setDirection(motor, DIR_NEGATIVE);
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
motor->skipIrqs = MOTOR_SWITCH_COUNT;
return MSTS_INIT;
}
// keep doing steps
stepper_schedule_irq(motor, motor->cfg->clocksHome, true);
return MSTS_OUTHOME;
}
static void FAST_FUNC stepper_interrupt(struct Stepper *motor)
{
enum StepperState newState;
// Check if we need to skip a certain number of IRQs
if (motor->skipIrqs)
{
--motor->skipIrqs;
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
return;
}
ASSERT(motor->state);
newState = motor->state(motor);
stepper_changeState(motor, newState);
}
/************************************************************************/
/* Public API */
/************************************************************************/
/**
* Initialize the stepper module
*/
void stepper_init(void)
{
STEPPER_INIT();
// before starting the power all the stepper enable must be surely low
stepper_disable();
// Bind functions to general states
memset(general_states, 0, sizeof(general_states));
general_states[MSTS_IDLE] = FSM_idle;
general_states[MSTS_PREIDLE] = FSM_preidle;
general_states[MSTS_PRERUN] = FSM_prerun;
general_states[MSTS_RUN] = FSM_run;
general_states[MSTS_PREINIT] = FSM_preinit;
general_states[MSTS_INIT] = FSM_init;
general_states[MSTS_ENTERING] = FSM_entering;
general_states[MSTS_LEAVING]= FSM_leaving;
general_states[MSTS_OUTHOME]= FSM_outhome;
general_states[MSTS_ERROR]= FSM_error;
}
void stepper_end(void)
{
// Disable all stepper timer interrupt to stop motors
for (int i = 0; i < CONFIG_NUM_STEPPER_MOTORS; i++)
stepper_disable_irq(&all_motors[i]);
}
/**
* Apply a setup config to motor structure context
*/
struct Stepper* stepper_setup(int index, struct StepperConfig *cfg)
{
struct Stepper* motor;
ASSERT(index < CONFIG_NUM_STEPPER_MOTORS);
motor = &all_motors[index];
motor->index = index;
motor->cfg = cfg;
//Register timer to stepper, and enable irq
stepper_tc_setup(motor->index, &stepper_interrupt, motor);
stepper_reset(motor);
stepper_enable_irq(motor);
return motor;
}
/**
* Set the enable for all the motors to 0 before switching on the power
*/
void stepper_disable(void)
{
STEPPER_DISABLE_ALL();
}
/**
* Reset the motor
*/
void stepper_reset(struct Stepper *motor)
{
/*
* To stop motor diable stepper irq.
*/
stepper_disable_irq(motor);
//Disable a stepper motor
STEPPER_DISABLE(MOTOR_INDEX(motor));
// Setup context variables
motor->power = 0;
motor->step = 0;
motor->rampStep = -1;
// We cannot set the clock at zero at start because of a limit in the fixed point ramp
motor->rampClock = motor->cfg->ramp.clocksMaxWL;
motor->rampValue = motor->cfg->ramp.clocksMaxWL;
motor->speed = SPEED_STOPPED;
motor->stepToReach = 0;
motor->skipIrqs = 0;
motor->stepCircular = 0;
setDirection(motor, DIR_POSITIVE);
setLowCurrent(motor);
motor->changeCurrentIrqs = 0;
// default value (disable level sensor check)
motor->stepsDeaf = DEAFSTEPS_DEFAULT;
STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
if (motor->cfg->homeSensorIndex < NUM_HOME_SENSORS)
hw_home_sensor_set_inverted(motor->cfg->homeSensorIndex, motor->cfg->flags.homeInverted);
if (motor->cfg->levelSensorIndex != MOTOR_NO_LEVEL_SENSOR)
hw_level_sensor_set_inverted(motor->cfg->levelSensorIndex, motor->cfg->flags.levelInverted);
stepper_changeState(motor, MSTS_IDLE);
// Reset stepper timer counter
stepper_tc_resetTimer(motor->timer);
// reset hw to the stepper motor
STEPPER_RESET(MOTOR_INDEX(motor));
STEPPER_ENABLE(MOTOR_INDEX(motor));
}
void stepper_updateHalfStep(struct Stepper *motor)
{
STEPPER_SET_HALF_STEP(MOTOR_INDEX(motor), motor->cfg->flags.halfStep);
}
void stepper_updateControlBit(struct Stepper *motor)
{
STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlBit);
}
void stepper_updateControlMoveBit(struct Stepper *motor)
{
STEPPER_SET_CONTROL_BIT(MOTOR_INDEX(motor), motor->cfg->flags.controlMoveBit);
}
/**
* Find the home of a \a motor assuming no current knowledge about its position.
*
* This must be done when the motor is desynchronized with the firmware and
* we do not know anymore where it is.
*
* In normal operation mode, to go back to the home, it is sufficient to use
* move to step #0 with stepper_move, since the home is always at step #0.
*/
void stepper_home(struct Stepper *motor)
{
// Begin home procedure
stepper_disable_irq(motor);
// disable home sensor check (default)
setCheckSensor(motor, MOTOR_HOMESENSOR_NOCHECK);
// deafult value (disable level sensor check)
motor->stepsDeaf = DEAFSTEPS_DEFAULT;
setDirection(motor, DIR_POSITIVE);
stepper_schedule_irq(motor, MOTOR_SWITCH_TICKS, false);
stepper_changeState(motor, MSTS_PREINIT);
stepper_enable_irq(motor);
}
void stepper_setStep(struct Stepper *motor, int16_t step)
{
motor->step = step;
}
int16_t stepper_getStep(struct Stepper *motor)
{
return motor->step;
}
int16_t stepper_getLevelStep(struct Stepper *motor)
{
return motor->stepsLevel;
}
void stepper_set_stepCircular(struct Stepper *motor, int16_t steps)
{
motor->stepCircular = steps;
}
int16_t stepper_get_stepCircular(struct Stepper *motor)
{
return motor->stepCircular;
}
int16_t stepper_scaleSteps(struct Stepper *motor, int16_t dir)
{
int16_t steps;
// scale the current position inside the motor lap
if(!motor->stepCircular) return 0;
// to be sure ....
while(motor->step > motor->stepCircular) motor->step -= motor->stepCircular;
if(dir == DIR_NEGATIVE)
{
steps = ((motor->stepCircular - motor->step) % motor->stepCircular);
motor->step = steps;
}
/*
else
steps = (motor->step % motor->stepCircular);
motor->step = steps;
*/
return motor->step;
}
static void stepper_enableCheckHome(struct Stepper *motor, bool bDirPositive)
{
enum MotorHomeSensorCheck value = MOTOR_HOMESENSOR_NOCHECK; // default
motor->stepsTollMin = 0;
if((motor->stepToReach != STEPS_INFINITE_POSITIVE) &&
(motor->stepToReach != STEPS_INFINITE_NEGATIVE) )
{
if(bDirPositive) // else if(motor->dir == DIR_POSITIVE)
{
/* if the direction is positive (movement from 0 position),
* if the starting position is inside home and the target position
* is outside home -> the motor has to cross the home sensor -> enable the control
*/
if (motor->step < motor->cfg->stepsInHome - motor->cfg->stepsTollOutHome &&
motor->stepToReach > motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome)
{
value = MOTOR_HOMESENSOR_OUTCHECK;
// home sensor out max position
motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollOutHome + MOTOR_CONSECUTIVE_ERROR_STEPS;
// home sensor in max position
if(motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS > motor->cfg->stepsTollOutHome)
motor->stepsTollMin = motor->cfg->stepsInHome + MOTOR_CONSECUTIVE_ERROR_STEPS - motor->cfg->stepsTollOutHome;
}
}
else // if(motor->dir == DIR_NEGATIVE)
{
/*
* if the direction is negative (movement to 0 position),
* if the starting position is far from home and the target position
* is inside home -> the motor has to cross the home sensor -> enable the control
*/
if (motor->step > motor->cfg->stepsInHome + motor->cfg->stepsTollInHome &&
motor->stepToReach < motor->cfg->stepsInHome - motor->cfg->stepsTollInHome)
{
value = MOTOR_HOMESENSOR_INCHECK;
// home sensor out max position
motor->stepsTollMax = motor->cfg->stepsInHome + motor->cfg->stepsTollInHome - MOTOR_CONSECUTIVE_ERROR_STEPS;
// home sensor in max position
if(motor->cfg->stepsInHome > motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS)
motor->stepsTollMin = motor->cfg->stepsInHome - (motor->cfg->stepsTollInHome + MOTOR_CONSECUTIVE_ERROR_STEPS);
}
}
}
setCheckSensor(motor, value);
}
/**
* Move motor to absolute position at specified speed
*
* \arg steps position to reach in steps
* \arg speed speed in timer ticks (use TIME2CLOCKS() to convert)
*/
int16_t stepper_move(struct Stepper *motor, int16_t steps, uint16_t speed, int16_t deafstep)
{
// if the stepper already is in the desired position -> nothing to do
if (motor->step == steps)
return 0;
stepper_disable_irq(motor);
// final position
motor->stepToReach = steps;
// clear error steps
motor->stepsErrorHome = 0;
// position to start level check
motor->stepsDeaf = deafstep;
// clear level position
motor->stepsLevel = 0;
if (speed < motor->cfg->ramp.clocksMinWL)
{
ASSERT2(0, "speed too fast (small number)");
speed = motor->cfg->ramp.clocksMinWL;
}
motor->rampClock = motor->cfg->ramp.clocksMaxWL;
motor->rampValue = motor->cfg->ramp.clocksMaxWL;
// TODO: find the exact value for motor->speed searching in the ramp array.
motor->speed = speed;
stepper_enable_irq(motor);
return 0;
}
/**
* Stop motor gracefully
*/
void stepper_stop(struct Stepper *motor)
{
/*
* The best way is to set the target of the movement to the minimum
* distance needed to decelerate. The logic in FSM_run will do the rest.
*/
if(stepper_idle(motor))
return;
stepper_disable_irq(motor);
motor->stepToReach = motor->step + motor->rampStep * motor->dir;
stepper_enable_irq(motor);
}
/**
* Stop motor immediately, changing the status
*/
void stepper_break(struct Stepper *motor, enum StepperState state)
{
// The best way to abort any operation is to go back to pre-idle mode
stepper_disable_irq(motor);
// Set of Speed disabled and Steps reached so that the function
// stepper_idle() succeeds
motor->speed = SPEED_STOPPED;
motor->stepToReach = motor->step;
stepper_changeState(motor, state);
stepper_enable_irq(motor);
}
///< Returns true if the stepper is in idle at the final position or in error:
// this means anyway that the motor is not moving
bool stepper_idle(struct Stepper *motor)
{
return (stepper_isState(motor, MSTS_ERROR) ||
(stepper_isState(motor, MSTS_IDLE) && motor->step == motor->stepToReach) );
}
///< Returns true if the stepper is in error mode
bool stepper_error(struct Stepper *motor)
{
return (stepper_isState(motor, MSTS_ERROR));
}
///< check the home sensor in zero position
bool stepper_inhome(struct Stepper *motor)
{
return(stepper_getStep(motor) == 0 &&
!stepper_readHome(motor) );
}
Reference in New Issue View Git Blame Copy Permalink