diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index 1062dcc807..6e752420fc 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/firmware/controllers/actuators/electronic_throttle.cpp @@ -100,48 +100,62 @@ static LoggingWithStorage logger("ETB"); EXTERN_ENGINE; -class EtbControl; - -static void applyEtbPinState(int stateIndex, EtbControl *etb) /* pwm_gen_callback */; - class EtbControl { -public: - EtbControl() : etbPwmUp("etbUp"), dcMotor(&etbPwmUp, &outputDirectionOpen, &outputDirectionClose) {} - OutputPin outputDirectionOpen; - OutputPin outputDirectionClose; - OutputPin etbOutput; +private: + OutputPin m_pinEnable; + OutputPin m_pinDir1; + OutputPin m_pinDir2; + + SimplePwm m_pwmEnable; + SimplePwm m_pwmDir1; + SimplePwm m_pwmDir2; + SimplePwm etbPwmUp; + +public: + EtbControl() : etbPwmUp("etbUp"), dcMotor(&m_pwmEnable, &m_pwmDir1, &m_pwmDir2) {} + TwoPinDcMotor dcMotor; - void start(bool useTwoWires, brain_pin_e controlPin, - brain_pin_e directionPin1, - brain_pin_e directionPin2) { - etbPwmUp.arg = this; - dcMotor.useTwoWires = useTwoWires; - if (!dcMotor.useTwoWires) { - // this line used for PWM in case of three wire mode - etbOutput.initPin("etb", controlPin); - } + + void start(bool useTwoWires, + brain_pin_e pinEnable, + brain_pin_e pinDir1, + brain_pin_e pinDir2) { + dcMotor.SetType(useTwoWires ? TwoPinDcMotor::ControlType::TwoDirection : TwoPinDcMotor::ControlType::TwoDirectionAndEnable); + + m_pinEnable.initPin("ETB Enable", pinEnable); + m_pinDir1.initPin("ETB Dir 1", pinDir1); + m_pinDir2.initPin("ETB Dir 2", pinDir2); + + // Clamp to >100hz int freq = maxI(100, engineConfiguration->etbFreq); - startSimplePwm(&etbPwmUp, "etb1", + + startSimplePwm(&m_pwmEnable, "ETB Enable", &engine->executor, - &etbOutput, + &m_pinEnable, freq, - 0.80, - (pwm_gen_callback*)applyEtbPinState); - outputDirectionOpen.initPin("etb dir open", directionPin1); - outputDirectionClose.initPin("etb dir close", directionPin2); + 0, + (pwm_gen_callback*)applyPinState); + + startSimplePwm(&m_pwmDir1, "ETB Dir 1", + &engine->executor, + &m_pinDir1, + freq, + 0, + (pwm_gen_callback*)applyPinState); + + startSimplePwm(&m_pwmDir2, "ETB Dir 2", + &engine->executor, + &m_pinDir2, + freq, + 0, + (pwm_gen_callback*)applyPinState); } - - }; static EtbControl etb1; static float directPwmValue = NAN; -/* -CCM_OPTIONAL static SimplePwm etbPwmDown("etbDown"); -*/ - extern percent_t mockPedalPosition; @@ -149,16 +163,9 @@ static Pid pid(&engineConfiguration->etb); static percent_t currentEtbDuty; -//static bool wasEtbBraking = false; - -// looks like my H-bridge does not like 100% duty cycle and it just hangs up? -// so we use 98% to indicate that things are alive and never use PM_FULL of PWM generator -//#define ETB_DUTY_LIMIT FULL_PWM_THRESHOLD -#define ETB_DUTY_LIMIT 0.4 +#define ETB_DUTY_LIMIT 0.9 #define PERCENT_TO_DUTY(X) (maxF(minF((X / 100.0), ETB_DUTY_LIMIT - 0.01), 0.01 - ETB_DUTY_LIMIT)) -//#define PERCENT_TO_DUTY(X) ((X) / 100.0) - class EtbController : public PeriodicController { public: EtbController() : PeriodicController("ETB") { } @@ -228,28 +235,11 @@ private: pid.iTermMin = engineConfiguration->etb_iTermMin; pid.iTermMax = engineConfiguration->etb_iTermMax; -/* - if (absF(actualThrottlePosition - targetPosition) < 0.5) { - // we are pretty close to desired position, let's hold it - dcMotor.BrakeVcc(); - scheduleMsg(&logger, "VCC braking %f %f", targetPosition, actualThrottlePosition); - return; - } -*/ currentEtbDuty = feedForward + pid.getOutput(targetPosition, actualThrottlePosition); etb1.dcMotor.Set(PERCENT_TO_DUTY(currentEtbDuty)); -/* - if (CONFIGB(etbDirectionPin2) != GPIO_UNASSIGNED) { - bool needEtbBraking = absF(targetPosition - actualThrottlePosition) < 3; - if (needEtbBraking != wasEtbBraking) { - scheduleMsg(&logger, "need ETB braking: %d", needEtbBraking); - wasEtbBraking = needEtbBraking; - } - outputDirectionClose.setValue(needEtbBraking); - } -*/ + if (engineConfiguration->isVerboseETB) { pid.showPidStatus(&logger, "ETB"); } @@ -308,16 +298,12 @@ void setEtbPFactor(float value) { } static void etbReset() { - // TODO: what is this about? - // I am experiencing some weird instability with my H-bridge with my Monte Carlo attempts scheduleMsg(&logger, "etbReset"); - for (int i = 0;i < 5;i++) { - // this is some crazy code to remind H-bridge that we are alive - etb1.dcMotor.BrakeGnd(); - chThdSleepMilliseconds(10); - } - mockPedalPosition = MOCK_UNDEFINED; + + etb1.dcMotor.Set(0); pid.reset(); + + mockPedalPosition = MOCK_UNDEFINED; } /** @@ -453,20 +439,6 @@ void unregisterEtbPins() { } -static void applyEtbPinState(int stateIndex, EtbControl *etb) /* pwm_gen_callback */ { - efiAssertVoid(CUSTOM_ERR_6663, stateIndex < PWM_PHASE_MAX_COUNT, "invalid stateIndex"); - int value = etb->etbPwmUp.multiWave.getChannelState(0, stateIndex); - if (etb->dcMotor.useTwoWires) { - OutputPin *output = etb->dcMotor.twoWireModeControl; - if (output != NULL) { - output->setValue(value); - } - } else { - OutputPin *output = &etb->etbOutput; - output->setValue(value); - } -} - void initElectronicThrottle(void) { addConsoleAction("ethinfo", showEthInfo); addConsoleAction("etbreset", etbReset); diff --git a/firmware/controllers/system/dc_motor.cpp b/firmware/controllers/system/dc_motor.cpp index 541d98bc19..05d7812ab8 100644 --- a/firmware/controllers/system/dc_motor.cpp +++ b/firmware/controllers/system/dc_motor.cpp @@ -8,29 +8,19 @@ #include "dc_motor.h" -TwoPinDcMotor::TwoPinDcMotor(SimplePwm* pwm, OutputPin* dir1, OutputPin* dir2) - : m_pwm(pwm) +TwoPinDcMotor::TwoPinDcMotor(SimplePwm* enable, SimplePwm* dir1, SimplePwm* dir2) + : m_enable(enable) , m_dir1(dir1) , m_dir2(dir2) { } bool TwoPinDcMotor::isOpenDirection() { - return isPositiveOrZero; + return m_value >= 0; } float TwoPinDcMotor::Get() { - return value; -} - -void TwoPinDcMotor::BrakeGnd() { - m_dir1->setValue(false); - m_dir2->setValue(false); -} - -void TwoPinDcMotor::BrakeVcc() { - m_dir1->setValue(true); - m_dir2->setValue(true); + return m_value; } /** @@ -38,17 +28,15 @@ void TwoPinDcMotor::BrakeVcc() { */ bool TwoPinDcMotor::Set(float duty) { - this->value = duty; + m_value = duty; - if(duty < 0) + bool isPositive = duty > 0; + + if(!isPositive) { - isPositiveOrZero = false; duty = -duty; } - else - { - isPositiveOrZero = true; - } + // below here 'duty' is a not negative // Clamp to 100% @@ -62,23 +50,15 @@ bool TwoPinDcMotor::Set(float duty) duty = 0.0f; } - if(duty < 0.01f) - { - BrakeGnd(); - } - else - { - if (isPositiveOrZero) { - twoWireModeControl = m_dir1; - } else { - twoWireModeControl = m_dir2; - } + // If we're in two pin mode, force 100%, else use this pin to PWM + float enableDuty = m_type == ControlType::TwoDirection ? 1 : duty; - m_dir1->setValue(isPositiveOrZero); - m_dir2->setValue(!isPositiveOrZero); - } + // If we're in dir + en mode, force 100%, else use the dir pins for PMW + float dirDuty = m_type == ControlType::TwoDirection ? duty : 1; - m_pwm->setSimplePwmDutyCycle(duty); + m_enable->setSimplePwmDutyCycle(enableDuty); + m_dir1->setSimplePwmDutyCycle(isPositive ? dirDuty : 0); + m_dir2->setSimplePwmDutyCycle(isPositive ? 0 : dirDuty); // This motor has no fault detection, so always return false (indicate success). return false; diff --git a/firmware/controllers/system/dc_motor.h b/firmware/controllers/system/dc_motor.h index 5d724255f2..f0a6d044b2 100644 --- a/firmware/controllers/system/dc_motor.h +++ b/firmware/controllers/system/dc_motor.h @@ -37,25 +37,31 @@ public: */ class TwoPinDcMotor : public DcMotor { +public: + enum class ControlType + { + TwoDirection, + TwoDirectionAndEnable, + }; + private: - SimplePwm* const m_pwm; - OutputPin* const m_dir1; - OutputPin* const m_dir2; - float value = 0; - bool isPositiveOrZero = false; + SimplePwm* const m_enable; + SimplePwm* const m_dir1; + SimplePwm* const m_dir2; + float m_value = 0; + + ControlType m_type; public: /** - * @param pwm SimplePwm driver for enable pin, for PWM speed control. + * @param enable SimplePwm driver for enable pin, for PWM speed control. * @param dir1 Enable 1 or direction 1 pin. Gets set high to rotate forward. * @param dir2 Enable 2 or direction 2 pin. Gets set high to rotate backward. */ - TwoPinDcMotor(SimplePwm* pwm, OutputPin* dir1, OutputPin* dir2); + TwoPinDcMotor(SimplePwm* enable, SimplePwm* dir1, SimplePwm* dir2); - bool useTwoWires = false; - OutputPin* twoWireModeControl = NULL; virtual bool Set(float duty) override; float Get(); - void BrakeGnd(); - void BrakeVcc(); bool isOpenDirection(); + + void SetType(ControlType type) { m_type = type; } };