rusefi/firmware/hw_layer/stepper_dual_hbridge.cpp

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