Add motor control files
This commit is contained in:
712
main/motor_control.c
Normal file
712
main/motor_control.c
Normal file
@ -0,0 +1,712 @@
|
||||
#include <string.h>
|
||||
#include <sys/time.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "freertos/timers.h"
|
||||
#include "freertos/semphr.h"
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/ledc.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "nvs_flash.h"
|
||||
#include "nvs.h"
|
||||
#include "motor_control.h"
|
||||
|
||||
static const char *TAG = "MOTOR_CONTROL";
|
||||
|
||||
// Motor control structure
|
||||
typedef struct {
|
||||
motor_state_t state;
|
||||
motor_dir_t direction;
|
||||
uint8_t speed_percent;
|
||||
uint8_t target_speed;
|
||||
uint32_t max_runtime_ms;
|
||||
uint32_t min_interval_ms;
|
||||
uint8_t min_speed_percent;
|
||||
uint8_t max_speed_percent;
|
||||
|
||||
// Runtime tracking
|
||||
int64_t start_time;
|
||||
int64_t last_stop_time;
|
||||
TimerHandle_t safety_timer;
|
||||
TimerHandle_t soft_start_timer;
|
||||
|
||||
// Statistics
|
||||
motor_stats_t stats;
|
||||
|
||||
// GPIO pins
|
||||
gpio_num_t in1_gpio;
|
||||
gpio_num_t in2_gpio;
|
||||
ledc_channel_t pwm_channel;
|
||||
} motor_t;
|
||||
|
||||
// Global state
|
||||
static motor_t s_motors[MOTOR_PUMP_MAX];
|
||||
static SemaphoreHandle_t s_motor_mutex = NULL;
|
||||
static bool s_initialized = false;
|
||||
|
||||
// Callbacks
|
||||
static motor_state_callback_t s_state_callback = NULL;
|
||||
static motor_error_callback_t s_error_callback = NULL;
|
||||
|
||||
// NVS namespace
|
||||
#define MOTOR_NVS_NAMESPACE "motor_stats"
|
||||
|
||||
// Forward declarations
|
||||
static esp_err_t motor_set_direction(motor_id_t id, motor_dir_t dir);
|
||||
static esp_err_t motor_update_pwm(motor_id_t id, uint8_t duty);
|
||||
static void motor_safety_timer_callback(TimerHandle_t xTimer);
|
||||
static void motor_soft_start_timer_callback(TimerHandle_t xTimer);
|
||||
static esp_err_t motor_save_stats(motor_id_t id);
|
||||
static esp_err_t motor_load_stats(motor_id_t id);
|
||||
static void motor_update_state(motor_id_t id, motor_state_t new_state);
|
||||
|
||||
// Utility functions
|
||||
static int64_t get_time_ms(void)
|
||||
{
|
||||
return esp_timer_get_time() / 1000;
|
||||
}
|
||||
|
||||
static bool is_valid_motor_id(motor_id_t id)
|
||||
{
|
||||
return (id == MOTOR_PUMP_1 || id == MOTOR_PUMP_2);
|
||||
}
|
||||
|
||||
esp_err_t motor_control_init(void)
|
||||
{
|
||||
if (s_initialized) {
|
||||
ESP_LOGW(TAG, "Motor control already initialized");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t ret = ESP_OK;
|
||||
|
||||
// Create mutex
|
||||
s_motor_mutex = xSemaphoreCreateMutex();
|
||||
if (s_motor_mutex == NULL) {
|
||||
ESP_LOGE(TAG, "Failed to create mutex");
|
||||
return ESP_ERR_NO_MEM;
|
||||
}
|
||||
|
||||
// Configure standby pin (active low, so HIGH = enabled)
|
||||
gpio_config_t io_conf = {
|
||||
.mode = GPIO_MODE_OUTPUT,
|
||||
.pin_bit_mask = (1ULL << MOTOR_STBY_GPIO),
|
||||
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
||||
.pull_up_en = GPIO_PULLUP_DISABLE,
|
||||
};
|
||||
ret = gpio_config(&io_conf);
|
||||
if (ret != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Failed to configure STBY pin");
|
||||
goto error;
|
||||
}
|
||||
|
||||
// Disable motors initially
|
||||
gpio_set_level(MOTOR_STBY_GPIO, 0);
|
||||
|
||||
// Configure direction pins for both motors
|
||||
io_conf.pin_bit_mask = (1ULL << MOTOR_AIN1_GPIO) | (1ULL << MOTOR_AIN2_GPIO) |
|
||||
(1ULL << MOTOR_BIN1_GPIO) | (1ULL << MOTOR_BIN2_GPIO);
|
||||
ret = gpio_config(&io_conf);
|
||||
if (ret != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Failed to configure direction pins");
|
||||
goto error;
|
||||
}
|
||||
|
||||
// Configure LEDC for PWM
|
||||
ledc_timer_config_t ledc_timer = {
|
||||
.speed_mode = LEDC_LOW_SPEED_MODE,
|
||||
.timer_num = LEDC_TIMER_0,
|
||||
.duty_resolution = MOTOR_PWM_RESOLUTION,
|
||||
.freq_hz = MOTOR_PWM_FREQ_HZ,
|
||||
.clk_cfg = LEDC_AUTO_CLK
|
||||
};
|
||||
ret = ledc_timer_config(&ledc_timer);
|
||||
if (ret != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Failed to configure LEDC timer");
|
||||
goto error;
|
||||
}
|
||||
|
||||
// Configure PWM channels
|
||||
ledc_channel_config_t ledc_channel[2] = {
|
||||
{
|
||||
.speed_mode = LEDC_LOW_SPEED_MODE,
|
||||
.channel = LEDC_CHANNEL_0,
|
||||
.timer_sel = LEDC_TIMER_0,
|
||||
.gpio_num = MOTOR_PWMA_GPIO,
|
||||
.duty = 0,
|
||||
.hpoint = 0
|
||||
},
|
||||
{
|
||||
.speed_mode = LEDC_LOW_SPEED_MODE,
|
||||
.channel = LEDC_CHANNEL_1,
|
||||
.timer_sel = LEDC_TIMER_0,
|
||||
.gpio_num = MOTOR_PWMB_GPIO,
|
||||
.duty = 0,
|
||||
.hpoint = 0
|
||||
}
|
||||
};
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
ret = ledc_channel_config(&ledc_channel[i]);
|
||||
if (ret != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Failed to configure LEDC channel %d", i);
|
||||
goto error;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize motor structures
|
||||
memset(s_motors, 0, sizeof(s_motors));
|
||||
|
||||
// Motor 1 (Pump 1)
|
||||
s_motors[0].in1_gpio = MOTOR_AIN1_GPIO;
|
||||
s_motors[0].in2_gpio = MOTOR_AIN2_GPIO;
|
||||
s_motors[0].pwm_channel = LEDC_CHANNEL_0;
|
||||
s_motors[0].max_runtime_ms = MOTOR_MAX_RUNTIME_MS;
|
||||
s_motors[0].min_interval_ms = MOTOR_MIN_INTERVAL_MS;
|
||||
s_motors[0].min_speed_percent = MOTOR_MIN_SPEED;
|
||||
s_motors[0].max_speed_percent = 100;
|
||||
s_motors[0].state = MOTOR_STATE_STOPPED;
|
||||
s_motors[0].direction = MOTOR_DIR_FORWARD;
|
||||
|
||||
// Motor 2 (Pump 2)
|
||||
s_motors[1].in1_gpio = MOTOR_BIN1_GPIO;
|
||||
s_motors[1].in2_gpio = MOTOR_BIN2_GPIO;
|
||||
s_motors[1].pwm_channel = LEDC_CHANNEL_1;
|
||||
s_motors[1].max_runtime_ms = MOTOR_MAX_RUNTIME_MS;
|
||||
s_motors[1].min_interval_ms = MOTOR_MIN_INTERVAL_MS;
|
||||
s_motors[1].min_speed_percent = MOTOR_MIN_SPEED;
|
||||
s_motors[1].max_speed_percent = 100;
|
||||
s_motors[1].state = MOTOR_STATE_STOPPED;
|
||||
s_motors[1].direction = MOTOR_DIR_FORWARD;
|
||||
|
||||
// Create safety timers
|
||||
for (int i = 0; i < MOTOR_PUMP_MAX - 1; i++) {
|
||||
s_motors[i].safety_timer = xTimerCreate("motor_safety",
|
||||
pdMS_TO_TICKS(1000),
|
||||
pdFALSE,
|
||||
(void*)(i + 1),
|
||||
motor_safety_timer_callback);
|
||||
if (s_motors[i].safety_timer == NULL) {
|
||||
ESP_LOGE(TAG, "Failed to create safety timer for motor %d", i + 1);
|
||||
ret = ESP_ERR_NO_MEM;
|
||||
goto error;
|
||||
}
|
||||
|
||||
s_motors[i].soft_start_timer = xTimerCreate("motor_soft_start",
|
||||
pdMS_TO_TICKS(50),
|
||||
pdTRUE,
|
||||
(void*)(i + 1),
|
||||
motor_soft_start_timer_callback);
|
||||
if (s_motors[i].soft_start_timer == NULL) {
|
||||
ESP_LOGE(TAG, "Failed to create soft start timer for motor %d", i + 1);
|
||||
ret = ESP_ERR_NO_MEM;
|
||||
goto error;
|
||||
}
|
||||
}
|
||||
|
||||
// Load statistics from NVS
|
||||
for (int i = 0; i < MOTOR_PUMP_MAX - 1; i++) {
|
||||
motor_load_stats(i + 1);
|
||||
}
|
||||
|
||||
// Enable motor driver
|
||||
gpio_set_level(MOTOR_STBY_GPIO, 1);
|
||||
|
||||
s_initialized = true;
|
||||
ESP_LOGI(TAG, "Motor control initialized successfully");
|
||||
return ESP_OK;
|
||||
|
||||
error:
|
||||
if (s_motor_mutex) {
|
||||
vSemaphoreDelete(s_motor_mutex);
|
||||
s_motor_mutex = NULL;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
esp_err_t motor_control_deinit(void)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// Stop all motors
|
||||
motor_stop_all();
|
||||
|
||||
// Disable motor driver
|
||||
gpio_set_level(MOTOR_STBY_GPIO, 0);
|
||||
|
||||
// Delete timers
|
||||
for (int i = 0; i < MOTOR_PUMP_MAX - 1; i++) {
|
||||
if (s_motors[i].safety_timer) {
|
||||
xTimerDelete(s_motors[i].safety_timer, 0);
|
||||
}
|
||||
if (s_motors[i].soft_start_timer) {
|
||||
xTimerDelete(s_motors[i].soft_start_timer, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Delete mutex
|
||||
if (s_motor_mutex) {
|
||||
vSemaphoreDelete(s_motor_mutex);
|
||||
s_motor_mutex = NULL;
|
||||
}
|
||||
|
||||
s_initialized = false;
|
||||
ESP_LOGI(TAG, "Motor control deinitialized");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t motor_set_direction(motor_id_t id, motor_dir_t dir)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
if (dir == MOTOR_DIR_FORWARD) {
|
||||
gpio_set_level(motor->in1_gpio, 1);
|
||||
gpio_set_level(motor->in2_gpio, 0);
|
||||
} else {
|
||||
gpio_set_level(motor->in1_gpio, 0);
|
||||
gpio_set_level(motor->in2_gpio, 1);
|
||||
}
|
||||
|
||||
motor->direction = dir;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t motor_update_pwm(motor_id_t id, uint8_t duty)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
esp_err_t ret = ledc_set_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel, duty);
|
||||
if (ret != ESP_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return ledc_update_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel);
|
||||
}
|
||||
|
||||
static void motor_update_state(motor_id_t id, motor_state_t new_state)
|
||||
{
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
motor_state_t old_state = motor->state;
|
||||
|
||||
motor->state = new_state;
|
||||
|
||||
if (old_state != new_state && s_state_callback) {
|
||||
s_state_callback(id, new_state);
|
||||
}
|
||||
}
|
||||
|
||||
esp_err_t motor_start(motor_id_t id, uint8_t speed_percent)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
|
||||
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
// Check if already running
|
||||
if (motor->state == MOTOR_STATE_RUNNING) {
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
ESP_LOGW(TAG, "Motor %d already running", id);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// Check cooldown period
|
||||
int64_t now = get_time_ms();
|
||||
if (motor->last_stop_time > 0) {
|
||||
int64_t elapsed = now - motor->last_stop_time;
|
||||
if (elapsed < motor->min_interval_ms) {
|
||||
motor_update_state(id, MOTOR_STATE_COOLDOWN);
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
if (s_error_callback) {
|
||||
s_error_callback(id, "Cooldown period not elapsed");
|
||||
}
|
||||
ESP_LOGW(TAG, "Motor %d in cooldown, %lld ms remaining",
|
||||
id, motor->min_interval_ms - elapsed);
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
}
|
||||
|
||||
// Clamp speed to configured limits
|
||||
if (speed_percent < motor->min_speed_percent) {
|
||||
speed_percent = motor->min_speed_percent;
|
||||
}
|
||||
if (speed_percent > motor->max_speed_percent) {
|
||||
speed_percent = motor->max_speed_percent;
|
||||
}
|
||||
|
||||
// Set direction
|
||||
motor_set_direction(id, motor->direction);
|
||||
|
||||
// Store target speed for soft start
|
||||
motor->target_speed = speed_percent;
|
||||
motor->speed_percent = 0;
|
||||
|
||||
// Start with 0 PWM for soft start
|
||||
motor_update_pwm(id, 0);
|
||||
|
||||
// Record start time
|
||||
motor->start_time = now;
|
||||
motor_update_state(id, MOTOR_STATE_RUNNING);
|
||||
|
||||
// Start soft start timer
|
||||
xTimerStart(motor->soft_start_timer, 0);
|
||||
|
||||
// Start safety timer
|
||||
xTimerChangePeriod(motor->safety_timer, pdMS_TO_TICKS(motor->max_runtime_ms), 0);
|
||||
xTimerStart(motor->safety_timer, 0);
|
||||
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
|
||||
ESP_LOGI(TAG, "Motor %d started at %d%% speed", id, speed_percent);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t motor_stop(motor_id_t id)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
|
||||
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
// Stop timers
|
||||
xTimerStop(motor->safety_timer, 0);
|
||||
xTimerStop(motor->soft_start_timer, 0);
|
||||
|
||||
// Set PWM to 0
|
||||
motor_update_pwm(id, 0);
|
||||
motor->speed_percent = 0;
|
||||
|
||||
// Update runtime statistics
|
||||
if (motor->state == MOTOR_STATE_RUNNING) {
|
||||
int64_t runtime = get_time_ms() - motor->start_time;
|
||||
motor->stats.last_run_duration_ms = runtime;
|
||||
motor->stats.total_runtime_ms += runtime;
|
||||
motor->stats.last_run_timestamp = motor->start_time;
|
||||
motor->stats.run_count++;
|
||||
|
||||
// Save stats to NVS periodically (every 10 runs)
|
||||
if (motor->stats.run_count % 10 == 0) {
|
||||
motor_save_stats(id);
|
||||
}
|
||||
}
|
||||
|
||||
motor->last_stop_time = get_time_ms();
|
||||
motor_update_state(id, MOTOR_STATE_STOPPED);
|
||||
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
|
||||
ESP_LOGI(TAG, "Motor %d stopped", id);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t motor_stop_all(void)
|
||||
{
|
||||
esp_err_t ret = ESP_OK;
|
||||
|
||||
for (motor_id_t id = MOTOR_PUMP_1; id < MOTOR_PUMP_MAX; id++) {
|
||||
esp_err_t err = motor_stop(id);
|
||||
if (err != ESP_OK) {
|
||||
ret = err;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
esp_err_t motor_emergency_stop(void)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// Disable motor driver immediately
|
||||
gpio_set_level(MOTOR_STBY_GPIO, 0);
|
||||
|
||||
// Stop all motors
|
||||
motor_stop_all();
|
||||
|
||||
// Re-enable motor driver
|
||||
gpio_set_level(MOTOR_STBY_GPIO, 1);
|
||||
|
||||
ESP_LOGW(TAG, "Emergency stop executed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t motor_start_timed(motor_id_t id, uint8_t speed_percent, uint32_t duration_ms)
|
||||
{
|
||||
if (duration_ms > MOTOR_MAX_RUNTIME_MS) {
|
||||
duration_ms = MOTOR_MAX_RUNTIME_MS;
|
||||
}
|
||||
|
||||
esp_err_t ret = motor_start(id, speed_percent);
|
||||
if (ret != ESP_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Override the safety timer with the requested duration
|
||||
xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
|
||||
xTimerChangePeriod(s_motors[id - 1].safety_timer, pdMS_TO_TICKS(duration_ms), 0);
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t motor_set_speed(motor_id_t id, uint8_t speed_percent)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
|
||||
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
if (motor->state != MOTOR_STATE_RUNNING) {
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// Clamp speed
|
||||
if (speed_percent < motor->min_speed_percent) {
|
||||
speed_percent = motor->min_speed_percent;
|
||||
}
|
||||
if (speed_percent > motor->max_speed_percent) {
|
||||
speed_percent = motor->max_speed_percent;
|
||||
}
|
||||
|
||||
motor->speed_percent = speed_percent;
|
||||
motor->target_speed = speed_percent;
|
||||
|
||||
uint8_t duty = (speed_percent * MOTOR_PWM_MAX_DUTY) / 100;
|
||||
motor_update_pwm(id, duty);
|
||||
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
motor_state_t motor_get_state(motor_id_t id)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return MOTOR_STATE_ERROR;
|
||||
}
|
||||
|
||||
return s_motors[id - 1].state;
|
||||
}
|
||||
|
||||
bool motor_is_running(motor_id_t id)
|
||||
{
|
||||
return motor_get_state(id) == MOTOR_STATE_RUNNING;
|
||||
}
|
||||
|
||||
bool motor_is_cooldown(motor_id_t id)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
if (motor->last_stop_time == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
int64_t elapsed = get_time_ms() - motor->last_stop_time;
|
||||
return elapsed < motor->min_interval_ms;
|
||||
}
|
||||
|
||||
uint32_t motor_get_runtime_ms(motor_id_t id)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
if (motor->state == MOTOR_STATE_RUNNING) {
|
||||
return get_time_ms() - motor->start_time;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
esp_err_t motor_get_stats(motor_id_t id, motor_stats_t *stats)
|
||||
{
|
||||
if (!is_valid_motor_id(id) || stats == NULL) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
|
||||
memcpy(stats, &s_motors[id - 1].stats, sizeof(motor_stats_t));
|
||||
xSemaphoreGive(s_motor_mutex);
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t motor_set_max_runtime(motor_id_t id, uint32_t max_runtime_ms)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
s_motors[id - 1].max_runtime_ms = max_runtime_ms;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t motor_set_min_interval(motor_id_t id, uint32_t min_interval_ms)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
s_motors[id - 1].min_interval_ms = min_interval_ms;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t motor_set_speed_limits(motor_id_t id, uint8_t min_speed, uint8_t max_speed)
|
||||
{
|
||||
if (!is_valid_motor_id(id)) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
if (min_speed > max_speed || max_speed > 100) {
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
s_motors[id - 1].min_speed_percent = min_speed;
|
||||
s_motors[id - 1].max_speed_percent = max_speed;
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
void motor_register_state_callback(motor_state_callback_t callback)
|
||||
{
|
||||
s_state_callback = callback;
|
||||
}
|
||||
|
||||
void motor_register_error_callback(motor_error_callback_t callback)
|
||||
{
|
||||
s_error_callback = callback;
|
||||
}
|
||||
|
||||
esp_err_t motor_test_run(motor_id_t id, uint32_t duration_ms)
|
||||
{
|
||||
ESP_LOGI(TAG, "Starting test run for motor %d, duration: %lu ms", id, duration_ms);
|
||||
return motor_start_timed(id, MOTOR_DEFAULT_SPEED, duration_ms);
|
||||
}
|
||||
|
||||
// Timer callbacks
|
||||
static void motor_safety_timer_callback(TimerHandle_t xTimer)
|
||||
{
|
||||
motor_id_t id = (motor_id_t)(intptr_t)pvTimerGetTimerID(xTimer);
|
||||
|
||||
ESP_LOGW(TAG, "Safety timer expired for motor %d", id);
|
||||
motor_stop(id);
|
||||
|
||||
if (s_error_callback) {
|
||||
s_error_callback(id, "Maximum runtime exceeded");
|
||||
}
|
||||
}
|
||||
|
||||
static void motor_soft_start_timer_callback(TimerHandle_t xTimer)
|
||||
{
|
||||
motor_id_t id = (motor_id_t)(intptr_t)pvTimerGetTimerID(xTimer);
|
||||
motor_t *motor = &s_motors[id - 1];
|
||||
|
||||
if (motor->state != MOTOR_STATE_RUNNING) {
|
||||
xTimerStop(xTimer, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
// Ramp up speed
|
||||
if (motor->speed_percent < motor->target_speed) {
|
||||
motor->speed_percent += 5; // 5% increment
|
||||
if (motor->speed_percent > motor->target_speed) {
|
||||
motor->speed_percent = motor->target_speed;
|
||||
}
|
||||
|
||||
uint8_t duty = (motor->speed_percent * MOTOR_PWM_MAX_DUTY) / 100;
|
||||
motor_update_pwm(id, duty);
|
||||
|
||||
// Stop timer when target reached
|
||||
if (motor->speed_percent >= motor->target_speed) {
|
||||
xTimerStop(xTimer, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// NVS functions
|
||||
static esp_err_t motor_save_stats(motor_id_t id)
|
||||
{
|
||||
nvs_handle_t nvs_handle;
|
||||
esp_err_t ret;
|
||||
char key[16];
|
||||
|
||||
ret = nvs_open(MOTOR_NVS_NAMESPACE, NVS_READWRITE, &nvs_handle);
|
||||
if (ret != ESP_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
snprintf(key, sizeof(key), "motor%d_stats", id);
|
||||
ret = nvs_set_blob(nvs_handle, key, &s_motors[id - 1].stats, sizeof(motor_stats_t));
|
||||
|
||||
if (ret == ESP_OK) {
|
||||
nvs_commit(nvs_handle);
|
||||
}
|
||||
|
||||
nvs_close(nvs_handle);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static esp_err_t motor_load_stats(motor_id_t id)
|
||||
{
|
||||
nvs_handle_t nvs_handle;
|
||||
esp_err_t ret;
|
||||
char key[16];
|
||||
size_t length = sizeof(motor_stats_t);
|
||||
|
||||
ret = nvs_open(MOTOR_NVS_NAMESPACE, NVS_READONLY, &nvs_handle);
|
||||
if (ret != ESP_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
snprintf(key, sizeof(key), "motor%d_stats", id);
|
||||
ret = nvs_get_blob(nvs_handle, key, &s_motors[id - 1].stats, &length);
|
||||
|
||||
nvs_close(nvs_handle);
|
||||
|
||||
if (ret == ESP_OK) {
|
||||
ESP_LOGI(TAG, "Loaded stats for motor %d: total runtime %lu ms, %lu runs",
|
||||
id, s_motors[id - 1].stats.total_runtime_ms, s_motors[id - 1].stats.run_count);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
Reference in New Issue
Block a user