You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
133 lines
3.6 KiB
133 lines
3.6 KiB
//
|
|
// 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);
|
|
}
|
|
|