rusefi-1/firmware/hw_layer/stepper.cpp

243 lines
6.4 KiB
C++
Raw Normal View History

2015-07-10 06:01:56 -07:00
/**
* @file stepper.cpp
*
* http://rusefi.com/wiki/index.php?title=Hardware:Stepper_motor
*
* @date Dec 24, 2014
2020-01-13 18:57:43 -08:00
* @author Andrey Belomutskiy, (c) 2012-2020
2015-07-10 06:01:56 -07:00
*/
#include "pch.h"
2019-01-03 20:51:29 -08:00
#include "stepper.h"
float StepperMotorBase::getTargetPosition() const {
return m_targetPosition;
}
void StepperMotorBase::setTargetPosition(float targetPositionSteps) {
// When the IAC position value change is insignificant (lower than this threshold), leave the poor valve alone
// When we get a larger change, actually update the target stepper position
if (absF(m_targetPosition - targetPositionSteps) >= 1) {
m_targetPosition = targetPositionSteps;
}
}
void StepperMotorBase::initialize(StepperHw *hardware, int totalSteps) {
m_totalSteps = maxI(3, totalSteps);
m_hw = hardware;
}
2019-01-03 20:51:29 -08:00
// todo: EFI_STEPPER macro
#if EFI_PROD_CODE || EFI_SIMULATOR
2015-07-10 06:01:56 -07:00
void StepperMotorBase::saveStepperPos(int pos) {
// use backup-power RTC registers to store the data
2019-04-12 17:52:51 -07:00
#if EFI_PROD_CODE
backupRamSave(BACKUP_STEPPER_POS, pos + 1);
2018-11-02 10:18:47 -07:00
#endif
postCurrentPosition();
}
int StepperMotorBase::loadStepperPos() {
2019-04-12 17:52:51 -07:00
#if EFI_PROD_CODE
return (int)backupRamLoad(BACKUP_STEPPER_POS) - 1;
2018-11-02 10:18:47 -07:00
#else
return 0;
#endif
}
void StepperMotorBase::changeCurrentPosition(bool positive) {
if (positive) {
m_currentPosition++;
} else {
m_currentPosition--;
}
postCurrentPosition();
}
void StepperMotorBase::postCurrentPosition(void) {
if (engineConfiguration->debugMode == DBG_STEPPER_IDLE_CONTROL) {
#if EFI_TUNER_STUDIO
engine->outputChannels.debugIntField5 = m_currentPosition;
#endif /* EFI_TUNER_STUDIO */
}
}
void StepperMotorBase::setInitialPosition(void) {
// try to get saved stepper position (-1 for no data)
2020-01-08 22:03:23 -08:00
m_currentPosition = loadStepperPos();
2019-04-12 17:52:51 -07:00
#if HAL_USE_ADC
// first wait until at least 1 slowADC sampling is complete
waitForSlowAdc();
2018-11-01 12:57:50 -07:00
#endif
2019-04-12 17:52:51 -07:00
#if EFI_SHAFT_POSITION_INPUT
bool isRunning = engine->rpmCalculator.isRunning();
#else
bool isRunning = false;
#endif /* EFI_SHAFT_POSITION_INPUT */
// now check if stepper motor re-initialization is requested - if the throttle pedal is pressed at startup
2021-10-05 16:59:07 -07:00
auto tpsPos = Sensor::getOrZero(SensorType::DriverThrottleIntent);
bool forceStepperParking = !isRunning && tpsPos > STEPPER_PARKING_TPS;
if (engineConfiguration->stepperForceParkingEveryRestart)
forceStepperParking = true;
efiPrintf("Stepper: savedStepperPos=%d forceStepperParking=%d (tps=%.2f)", m_currentPosition, (forceStepperParking ? 1 : 0), tpsPos);
2020-01-08 22:03:23 -08:00
if (m_currentPosition < 0 || forceStepperParking) {
efiPrintf("Stepper: starting parking...");
// reset saved value
saveStepperPos(-1);
/**
* let's park the motor in a known position to begin with
*
* I believe it's safer to retract the valve for parking - at least on a bench I've seen valves
* disassembling themselves while pushing too far out.
*
* Add extra steps to compensate step skipping by some old motors.
*/
int numParkingSteps = (int)efiRound((1.0f + (float)engineConfiguration->stepperParkingExtraSteps / PERCENT_MULT) * m_totalSteps, 1.0f);
for (int i = 0; i < numParkingSteps; i++) {
if (!m_hw->step(false)) {
initialPositionSet = false;
return;
}
changeCurrentPosition(false);
}
// set & save zero stepper position after the parking completion
2020-01-08 22:03:23 -08:00
m_currentPosition = 0;
saveStepperPos(m_currentPosition);
efiPrintf("Stepper: parking finished!");
} else {
// The initial target position should correspond to the saved stepper position.
// Idle thread starts later and sets a new target position.
2020-01-08 22:03:23 -08:00
setTargetPosition(m_currentPosition);
2015-07-10 06:01:56 -07:00
}
initialPositionSet = true;
}
void StepperMotorBase::doIteration() {
int targetPosition = efiRound(getTargetPosition(), 1);
int currentPosition = m_currentPosition;
// the stepper does not work if the main relay is turned off (it requires +12V)
if (!engine->isMainRelayEnabled()) {
m_hw->pause();
return;
}
if (!initialPositionSet) {
setInitialPosition();
return;
}
if (targetPosition == currentPosition) {
m_hw->sleep();
m_isBusy = false;
return;
}
m_isBusy = true;
bool isIncrementing = targetPosition > currentPosition;
if (m_hw->step(isIncrementing)) {
changeCurrentPosition(isIncrementing);
}
2020-01-08 22:03:23 -08:00
// save position to backup RTC register
2019-04-12 17:52:51 -07:00
#if EFI_PROD_CODE
saveStepperPos(m_currentPosition);
2018-11-02 10:18:47 -07:00
#endif
2015-07-10 06:01:56 -07:00
}
bool StepperMotorBase::isBusy() const {
return m_isBusy;
2015-07-10 06:01:56 -07:00
}
void StepDirectionStepper::setDirection(bool isIncrementing) {
2020-01-08 22:03:23 -08:00
if (isIncrementing != m_currentDirection) {
// compensate stepper motor inertia
pause();
2020-01-08 22:03:23 -08:00
m_currentDirection = isIncrementing;
}
directionPin.setValue(isIncrementing);
}
bool StepDirectionStepper::pulse() {
// we move the motor only of it is powered from the main relay
if (!engine->isMainRelayEnabled())
return false;
enablePin.setValue(false); // enable stepper
stepPin.setValue(true);
pause();
2019-07-13 06:38:37 -07:00
stepPin.setValue(false);
pause();
2015-07-10 06:01:56 -07:00
enablePin.setValue(true); // disable stepper
return true;
2015-07-10 06:01:56 -07:00
}
void StepperHw::sleep() {
pause();
}
void StepperHw::pause(int divisor) const {
// currently we can't sleep less than 1ms (see #3214)
chThdSleepMicroseconds(maxI(MS2US(1), (int)(MS2US(m_reactionTime)) / divisor));
}
void StepperHw::setReactionTime(float ms) {
m_reactionTime = maxF(1, ms);
}
bool StepDirectionStepper::step(bool positive) {
setDirection(positive);
return pulse();
}
void StepperMotor::initialize(StepperHw *hardware, int totalSteps) {
StepperMotorBase::initialize(hardware, totalSteps);
2022-07-21 12:17:32 -07:00
start();
}
void StepDirectionStepper::initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin_output_mode_e directionPinMode, float reactionTime, brain_pin_e enablePin, pin_output_mode_e enablePinMode) {
if (!isBrainPinValid(stepPin) || !isBrainPinValid(directionPin)) {
2015-07-10 06:01:56 -07:00
return;
}
setReactionTime(reactionTime);
2017-06-13 06:28:05 -07:00
this->directionPinMode = directionPinMode;
this->directionPin.initPin("Stepper DIR", directionPin, &this->directionPinMode);
2015-07-10 06:01:56 -07:00
this->stepPinMode = OM_DEFAULT; // todo: do we need configurable stepPinMode?
this->stepPin.initPin("Stepper step", stepPin, &this->stepPinMode);
this->enablePinMode = enablePinMode;
this->enablePin.initPin("Stepper EN", enablePin, &this->enablePinMode);
2015-07-10 06:01:56 -07:00
2018-01-22 14:48:12 -08:00
// All pins must be 0 for correct hardware startup (e.g. stepper auto-disabling circuit etc.).
this->enablePin.setValue(true); // disable stepper
this->stepPin.setValue(false);
2018-01-22 14:48:12 -08:00
this->directionPin.setValue(false);
2020-01-08 22:03:23 -08:00
m_currentDirection = false;
2015-07-10 06:01:56 -07:00
}
2019-01-03 20:51:29 -08:00
#endif
#if EFI_UNIT_TEST
void StepperHw::sleep() { }
#endif // EFI_UNIT_TEST