mirror of https://github.com/rusefi/rusefi.git
111 lines
2.9 KiB
C++
111 lines
2.9 KiB
C++
|
|
#include "pch.h"
|
|
|
|
#if !EFI_UNIT_TEST
|
|
|
|
#include "stepper.h"
|
|
#include "dc_motor.h"
|
|
|
|
static const int8_t phaseA[] =
|
|
{
|
|
1,
|
|
1,
|
|
-1,
|
|
-1
|
|
};
|
|
|
|
static const int8_t phaseB[] =
|
|
{
|
|
-1,
|
|
1,
|
|
1,
|
|
-1
|
|
};
|
|
|
|
static const int8_t microPhases[] = {
|
|
0, 20, 38, 56, 71, 83, 92, 98,
|
|
100, 98, 92, 83, 71, 56, 38, 20,
|
|
0, -20, -38, -56, -71, -83, -92, -98,
|
|
-100, -98, -92, -83, -71, -56, -38, -20
|
|
};
|
|
|
|
static const int maxNumSteps = 8;
|
|
static constexpr int tableSizeMask = efi::size(microPhases) - 1;
|
|
static constexpr float phaseDutyCycleDivisor = 1.0f / (100.0f * 100.0f); // both the phase degrees and duty cycle settings are in %
|
|
|
|
|
|
void DualHBridgeStepper::initialize(DcMotor* motorPhaseA, DcMotor* motorPhaseB, float reactionTime)
|
|
{
|
|
setReactionTime(reactionTime);
|
|
|
|
m_motorPhaseA = motorPhaseA;
|
|
m_motorPhaseB = motorPhaseB;
|
|
|
|
efiAssertVoid(ObdCode::CUSTOM_ERR_ASSERT, engineConfiguration->stepperNumMicroSteps <= maxNumSteps, "stepperNumMicroSteps");
|
|
}
|
|
|
|
bool DualHBridgeStepper::step(bool positive) {
|
|
// Check that we've been initialized
|
|
if (!m_motorPhaseA || !m_motorPhaseB) {
|
|
return false;
|
|
}
|
|
|
|
if (engineConfiguration->stepperNumMicroSteps > 1) {
|
|
float dutyMult = engineConfiguration->stepperMaxDutyCycle * phaseDutyCycleDivisor;
|
|
int numStepIncr = maxNumSteps / engineConfiguration->stepperNumMicroSteps;
|
|
if (!positive)
|
|
numStepIncr = -numStepIncr;
|
|
for (int i = engineConfiguration->stepperNumMicroSteps; i > 0; i--) {
|
|
m_phase = (m_phase + numStepIncr) & tableSizeMask;
|
|
update(dutyMult);
|
|
// sleep 1/Nth of the pause time
|
|
pause(engineConfiguration->stepperNumMicroSteps);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// For the full-stepping mode, we use a traditional "two phase on" drive model
|
|
// because "wave drive" (one phase on) method has less torque.
|
|
// For explanation, pls see: https://github.com/rusefi/rusefi/pull/3213#discussion_r700746453
|
|
|
|
// step phase, wrapping
|
|
if (positive) {
|
|
m_phase = (m_phase + 1) & 0x03;
|
|
} else {
|
|
m_phase = (m_phase - 1) & 0x03;
|
|
}
|
|
|
|
update(1.0f);
|
|
pause();
|
|
|
|
return true;
|
|
}
|
|
|
|
bool DualHBridgeStepper::update(float dutyMult) {
|
|
if (!m_motorPhaseA || !m_motorPhaseB) {
|
|
return false;
|
|
}
|
|
|
|
if (engineConfiguration->stepperNumMicroSteps > 1) {
|
|
// phase B is 90 degrees shifted
|
|
uint8_t m_phaseB = (m_phase + engineConfiguration->stepperNumMicroSteps) & tableSizeMask;
|
|
|
|
// Set phases according to the table
|
|
m_motorPhaseA->set(dutyMult * microPhases[m_phase]);
|
|
m_motorPhaseB->set(dutyMult * microPhases[m_phaseB]);
|
|
return true;
|
|
}
|
|
// Set phases according to the table
|
|
m_motorPhaseA->set(phaseA[m_phase]);
|
|
m_motorPhaseB->set(phaseB[m_phase]);
|
|
return true;
|
|
}
|
|
|
|
void DualHBridgeStepper::sleep() {
|
|
float sleepingCoef = minI(engineConfiguration->stepperMinDutyCycle, engineConfiguration->stepperMaxDutyCycle) * phaseDutyCycleDivisor;
|
|
update(sleepingCoef);
|
|
pause();
|
|
}
|
|
|
|
#endif
|