Sub_Spray_STM32/User/BSP/LED/pump_controller.cpp

//
// Created by iflyt on 2025/2/28.
//

#include "pump_controller.h"

#include <cmath>
#include <base/zbasic.h>
#include <bits/stl_algobase.h>

#include "tim_pwm.h"

PumpController::PumpController() = default;

PumpController::~PumpController() {}

void PumpController::init() {
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    // 使能 GPIOD 和 GPIOI 时钟
    __HAL_RCC_GPIOD_CLK_ENABLE();
    __HAL_RCC_GPIOI_CLK_ENABLE();

    // 配置 PUMP_EN_PIN 和 PUMP_DIR_PIN 为输出模式
    GPIO_InitStruct.Pin = PUMP_EN_PIN | PUMP_DIR_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(PUMP_EN_PORT, &GPIO_InitStruct);

    // 配置 PUMP_FG_PIN 为输入模式
    GPIO_InitStruct.Pin = PUMP_FG_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(PUMP_FG_PORT, &GPIO_InitStruct);

    // 初始状态下禁用注射泵
    powerOff();
}

void PumpController::powerOn() {
    isPowerOn = true;
    HAL_GPIO_WritePin(PUMP_EN_PORT, PUMP_EN_PIN, GPIO_PIN_RESET);
    this->openPwm();
}

void PumpController::powerOff() {
    isPowerOn = false;
    HAL_GPIO_WritePin(PUMP_EN_PORT, PUMP_EN_PIN, GPIO_PIN_SET);
    this->closePwm();
}

void PumpController::setFlowSpeed(double flow) {
    // 设置为零 自动关闭
    if(flow == 0.0) {
        this->powerOff();
        ZLOGI("【PUMP】", "SET FLOW POWER OFF");
        return;
    }
    bool is_forward = flow > 0;

    const double abs_flow = fabs(flow);
    this->freq_ = calculateFrequency(abs_flow);
    if(this->freq_ == 0) {
        this->freq_ = is_forward ? 1 : -1;
        ZLOGI("【PUMP】", "[ZERO LIMIT] FREQ  [%d] ", freq_);
    }

    ZLOGI("【PUMP】", "FINAL FREQ [%d] ", freq_);
    if(this->isPowerOn) {
        this->openPwm();
        this->powerOn();
    }
}

void PumpController::moveWithFlowSpeed(double flow) {
    // 设置为零 自动关闭
    if(flow == 0.0) {
        this->powerOff();
        ZLOGI("【PUMP】", "SET FLOW POWER OFF");
        return;
    }
    bool is_forward = flow > 0;

    const double abs_flow = fabs(flow);
    this->freq_ = calculateFrequency(abs_flow);
    if(this->freq_ == 0) {
        this->freq_ = is_forward ? 1 : -1;
        ZLOGI("【PUMP】", "[ZERO LIMIT] FREQ  [%d] ", freq_);
    }
    ZLOGI("【PUMP】", "FINAL FREQ [%d] ", freq_);
    this->setDirection(is_forward);
    this->openPwm();
    this->powerOn();
}

void PumpController::setDirection(bool forward) {
    if (forward) {
        HAL_GPIO_WritePin(PUMP_DIR_PORT, PUMP_DIR_PIN, GPIO_PIN_SET);
    } else {
        HAL_GPIO_WritePin(PUMP_DIR_PORT, PUMP_DIR_PIN, GPIO_PIN_RESET);
    }
}

bool PumpController::isNoraml() {
    // return  HAL_GPIO_ReadPin(PUMP_FG_PORT, PUMP_FG_PIN) == GPIO_PIN_SET;
    return true;
}

void PumpController::openPwm() {
    bsp_SetTIMOutPWM(PUMP_PUL_PORT, PUMP_PUL_PIN, PUMP_TIMER_HANDLE, PUMP_TIMER_CHANNEL, freq_, standardDutyCycle_);
}

void PumpController::closePwm() {
#if 0
    bsp_SetTIMOutPWM(PUMP_PUL_PORT, PUMP_PUL_PIN, PUMP_TIMER_HANDLE, PUMP_TIMER_CHANNEL, 0, 0);
#else
    bsp_SetTIMOutPWM(PUMP_PUL_PORT, PUMP_PUL_PIN, PUMP_TIMER_HANDLE, PUMP_TIMER_CHANNEL, 0, 10000);
#endif
}

int32_t PumpController::calculateFrequency(double flow) const{
    // 确保流量值在有效范围内
    flow = std::max(flow, static_cast<double>(MIN_FLOW));
    flow = std::min(flow, static_cast<double>(MAX_FLOW));

    // 计算斜率
    double slope = static_cast<double>(MAX_FREQUENCY - MIN_FREQUENCY) / (MAX_FLOW - MIN_FLOW);

    // 根据线性关系计算频率
    return MIN_FREQUENCY + (slope * (flow - MIN_FLOW) + 0.5);
}