411 lines
14 KiB
C
411 lines
14 KiB
C
#include "motor_control.h"
|
|
#include "config.h"
|
|
#include "esp_log.h"
|
|
#include "driver/gpio.h"
|
|
#include "driver/ledc.h"
|
|
#include <string.h>
|
|
|
|
// Private variables
|
|
static motor_state_t motor_state = {
|
|
.mode = MOTOR_OFF,
|
|
.pending_mode = MOTOR_OFF,
|
|
.target_speed = 0,
|
|
.pending_speed = 0,
|
|
.current_speed = 0,
|
|
.state = MOTOR_STATE_IDLE,
|
|
.ramping = false,
|
|
.ramp_timer = NULL,
|
|
.cooldown_timer = NULL,
|
|
.cooldown_remaining_ms = 0,
|
|
.last_on_mode = MOTOR_EXHAUST, // Default to exhaust for ON button
|
|
.last_on_speed = 50, // Default to 50% for ON button
|
|
.user_turned_off = false
|
|
};
|
|
|
|
// Forward declarations for private functions
|
|
static void apply_motor_pwm(int speed_percent);
|
|
static void start_motor_operation(motor_mode_t mode, int speed_percent);
|
|
static void save_last_on_state(motor_mode_t mode, int speed);
|
|
static void motor_ramp_timer_callback(TimerHandle_t xTimer);
|
|
static void motor_cooldown_timer_callback(TimerHandle_t xTimer);
|
|
|
|
// Private function: Apply PWM to motor based on current mode and speed
|
|
static void apply_motor_pwm(int speed_percent) {
|
|
// Clamp speed to valid range using config macro
|
|
speed_percent = CLAMP_SPEED(speed_percent);
|
|
|
|
uint32_t duty = SPEED_TO_DUTY(speed_percent);
|
|
|
|
if (motor_state.mode == MOTOR_OFF || speed_percent == 0) {
|
|
gpio_set_level(LED_PIN, 0);
|
|
gpio_set_level(MOTOR_R_EN, 0);
|
|
gpio_set_level(MOTOR_L_EN, 0);
|
|
ledc_set_duty(PWM_SPEED_MODE, PWM_R_CHANNEL, 0);
|
|
ledc_set_duty(PWM_SPEED_MODE, PWM_L_CHANNEL, 0);
|
|
ledc_update_duty(PWM_SPEED_MODE, PWM_R_CHANNEL);
|
|
ledc_update_duty(PWM_SPEED_MODE, PWM_L_CHANNEL);
|
|
|
|
} else if (motor_state.mode == MOTOR_EXHAUST) {
|
|
gpio_set_level(LED_PIN, 1);
|
|
gpio_set_level(MOTOR_R_EN, 1);
|
|
gpio_set_level(MOTOR_L_EN, 1);
|
|
ledc_set_duty(PWM_SPEED_MODE, PWM_R_CHANNEL, duty);
|
|
ledc_set_duty(PWM_SPEED_MODE, PWM_L_CHANNEL, 0);
|
|
ledc_update_duty(PWM_SPEED_MODE, PWM_R_CHANNEL);
|
|
ledc_update_duty(PWM_SPEED_MODE, PWM_L_CHANNEL);
|
|
|
|
} else if (motor_state.mode == MOTOR_INTAKE) {
|
|
gpio_set_level(LED_PIN, 1);
|
|
gpio_set_level(MOTOR_R_EN, 1);
|
|
gpio_set_level(MOTOR_L_EN, 1);
|
|
ledc_set_duty(PWM_SPEED_MODE, PWM_R_CHANNEL, 0);
|
|
ledc_set_duty(PWM_SPEED_MODE, PWM_L_CHANNEL, duty);
|
|
ledc_update_duty(PWM_SPEED_MODE, PWM_R_CHANNEL);
|
|
ledc_update_duty(PWM_SPEED_MODE, PWM_L_CHANNEL);
|
|
}
|
|
}
|
|
|
|
// Private function: Motor ramp timer callback
|
|
static void motor_ramp_timer_callback(TimerHandle_t xTimer) {
|
|
if (motor_state.state != MOTOR_STATE_RAMPING) {
|
|
return;
|
|
}
|
|
|
|
int speed_diff = motor_state.target_speed - motor_state.current_speed;
|
|
|
|
if (abs(speed_diff) <= RAMP_STEP_SIZE) {
|
|
// Close enough to target, finish ramping
|
|
motor_state.current_speed = motor_state.target_speed;
|
|
motor_state.ramping = false;
|
|
motor_state.state = MOTOR_STATE_IDLE;
|
|
|
|
// Stop the timer
|
|
xTimerStop(motor_state.ramp_timer, 0);
|
|
|
|
ESP_LOGI(SYSTEM_TAG, "Ramping complete - Final speed: %d%%", motor_state.current_speed);
|
|
} else {
|
|
// Continue ramping
|
|
if (speed_diff > 0) {
|
|
motor_state.current_speed += RAMP_STEP_SIZE;
|
|
} else {
|
|
motor_state.current_speed -= RAMP_STEP_SIZE;
|
|
}
|
|
|
|
MOTOR_LOGD(SYSTEM_TAG, "Ramping: %d%% (target: %d%%)", motor_state.current_speed, motor_state.target_speed);
|
|
}
|
|
|
|
apply_motor_pwm(motor_state.current_speed);
|
|
}
|
|
|
|
// Private function: Motor cooldown timer callback
|
|
static void motor_cooldown_timer_callback(TimerHandle_t xTimer) {
|
|
ESP_LOGI(SYSTEM_TAG, "Cooldown complete - Starting motor in %s mode at %d%%",
|
|
motor_state.pending_mode == MOTOR_EXHAUST ? "EXHAUST" : "INTAKE",
|
|
motor_state.pending_speed);
|
|
|
|
// Reset cooldown tracking
|
|
motor_state.cooldown_remaining_ms = 0;
|
|
|
|
// Start the motor in the pending mode
|
|
start_motor_operation(motor_state.pending_mode, motor_state.pending_speed);
|
|
}
|
|
|
|
// Private function: Save the last ON state (for ON button functionality)
|
|
static void save_last_on_state(motor_mode_t mode, int speed) {
|
|
if (mode != MOTOR_OFF && speed > 0) {
|
|
motor_state.last_on_mode = mode;
|
|
motor_state.last_on_speed = speed;
|
|
ESP_LOGI(SYSTEM_TAG, "Last ON state updated: %s @ %d%%",
|
|
mode == MOTOR_EXHAUST ? "EXHAUST" : "INTAKE", speed);
|
|
}
|
|
}
|
|
|
|
// Private function: Start motor operation (internal function)
|
|
static void start_motor_operation(motor_mode_t mode, int speed_percent) {
|
|
// Clamp speed using config macro
|
|
speed_percent = CLAMP_SPEED(speed_percent);
|
|
|
|
motor_state.mode = mode;
|
|
motor_state.target_speed = speed_percent;
|
|
motor_state.state = MOTOR_STATE_RAMPING;
|
|
motor_state.ramping = true;
|
|
|
|
if (mode == MOTOR_OFF || speed_percent == 0) {
|
|
// Immediate stop
|
|
motor_state.current_speed = 0;
|
|
motor_state.target_speed = 0;
|
|
motor_state.state = MOTOR_STATE_IDLE;
|
|
motor_state.ramping = false;
|
|
apply_motor_pwm(0);
|
|
ESP_LOGI(SYSTEM_TAG, "Motor stopped immediately");
|
|
} else {
|
|
// Save last ON state for future ON button use
|
|
save_last_on_state(mode, speed_percent);
|
|
|
|
// Start from minimum speed if currently off
|
|
if (motor_state.current_speed == 0) {
|
|
int start_speed = (speed_percent < MIN_MOTOR_SPEED) ? speed_percent : MIN_MOTOR_SPEED;
|
|
motor_state.current_speed = start_speed;
|
|
apply_motor_pwm(start_speed);
|
|
ESP_LOGI(SYSTEM_TAG, "Motor starting at %d%%, ramping to %d%%", start_speed, speed_percent);
|
|
}
|
|
|
|
// Start ramping if needed
|
|
if (motor_state.current_speed != motor_state.target_speed) {
|
|
xTimerStart(motor_state.ramp_timer, 0);
|
|
} else {
|
|
motor_state.state = MOTOR_STATE_IDLE;
|
|
motor_state.ramping = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Public API Implementation
|
|
|
|
esp_err_t motor_control_init(void) {
|
|
ESP_LOGI(SYSTEM_TAG, "Initializing motor control system...");
|
|
|
|
// Configure GPIO pins
|
|
ESP_LOGI(SYSTEM_TAG, "Configuring GPIO pins...");
|
|
|
|
uint64_t pin_mask = (1ULL << LED_PIN) |
|
|
(1ULL << MOTOR_R_EN) |
|
|
(1ULL << MOTOR_L_EN);
|
|
|
|
gpio_config_t io_conf = {
|
|
.pin_bit_mask = pin_mask,
|
|
.mode = GPIO_MODE_OUTPUT,
|
|
.pull_up_en = GPIO_PULLUP_DISABLE,
|
|
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
|
.intr_type = GPIO_INTR_DISABLE
|
|
};
|
|
|
|
esp_err_t ret = gpio_config(&io_conf);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGE(SYSTEM_TAG, "Failed to configure GPIO pins: %s", esp_err_to_name(ret));
|
|
return ret;
|
|
}
|
|
|
|
// Set initial pin states
|
|
gpio_set_level(LED_PIN, 0);
|
|
gpio_set_level(MOTOR_R_EN, 0);
|
|
gpio_set_level(MOTOR_L_EN, 0);
|
|
|
|
ESP_LOGI(SYSTEM_TAG, "GPIO pins configured");
|
|
|
|
// Configure PWM
|
|
ESP_LOGI(SYSTEM_TAG, "Configuring PWM...");
|
|
|
|
ledc_timer_config_t timer_conf = {
|
|
.speed_mode = PWM_SPEED_MODE,
|
|
.timer_num = PWM_TIMER,
|
|
.duty_resolution = PWM_RESOLUTION,
|
|
.freq_hz = PWM_FREQUENCY,
|
|
.clk_cfg = LEDC_AUTO_CLK
|
|
};
|
|
ret = ledc_timer_config(&timer_conf);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGE(SYSTEM_TAG, "Failed to configure PWM timer: %s", esp_err_to_name(ret));
|
|
return ret;
|
|
}
|
|
|
|
ledc_channel_config_t channel_conf = {
|
|
.channel = PWM_R_CHANNEL,
|
|
.duty = 0,
|
|
.gpio_num = PWM_R_PIN,
|
|
.speed_mode = PWM_SPEED_MODE,
|
|
.hpoint = 0,
|
|
.timer_sel = PWM_TIMER
|
|
};
|
|
ret = ledc_channel_config(&channel_conf);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGE(SYSTEM_TAG, "Failed to configure PWM right channel: %s", esp_err_to_name(ret));
|
|
return ret;
|
|
}
|
|
|
|
channel_conf.channel = PWM_L_CHANNEL;
|
|
channel_conf.gpio_num = PWM_L_PIN;
|
|
ret = ledc_channel_config(&channel_conf);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGE(SYSTEM_TAG, "Failed to configure PWM left channel: %s", esp_err_to_name(ret));
|
|
return ret;
|
|
}
|
|
|
|
ESP_LOGI(SYSTEM_TAG, "PWM configured");
|
|
|
|
// Create timers
|
|
motor_state.ramp_timer = xTimerCreate(
|
|
"MotorRampTimer", // Timer name
|
|
pdMS_TO_TICKS(RAMP_STEP_MS), // Timer period
|
|
pdTRUE, // Auto-reload
|
|
(void*)0, // Timer ID
|
|
motor_ramp_timer_callback // Callback function
|
|
);
|
|
|
|
motor_state.cooldown_timer = xTimerCreate(
|
|
"MotorCooldownTimer", // Timer name
|
|
pdMS_TO_TICKS(DIRECTION_CHANGE_COOLDOWN_MS), // Timer period
|
|
pdFALSE, // One-shot
|
|
(void*)0, // Timer ID
|
|
motor_cooldown_timer_callback // Callback function
|
|
);
|
|
|
|
if (motor_state.ramp_timer == NULL || motor_state.cooldown_timer == NULL) {
|
|
ESP_LOGE(SYSTEM_TAG, "Failed to create motor timers");
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
ESP_LOGI(SYSTEM_TAG, "Motor control system initialized with direction change safety");
|
|
return ESP_OK;
|
|
}
|
|
|
|
void motor_set_speed(motor_mode_t mode, int speed_percent) {
|
|
// Clamp speed to valid range using config macro
|
|
speed_percent = CLAMP_SPEED(speed_percent);
|
|
|
|
ESP_LOGI(SYSTEM_TAG, "Motor command: %s - Speed: %d%% (Current mode: %s, Current speed: %d%%, State: %s)",
|
|
motor_mode_to_string(mode), speed_percent,
|
|
motor_mode_to_string(motor_state.mode), motor_state.current_speed,
|
|
motor_state_to_string(motor_state.state));
|
|
|
|
// Track if user manually turned off
|
|
if (mode == MOTOR_OFF && motor_state.mode != MOTOR_OFF) {
|
|
motor_state.user_turned_off = true;
|
|
ESP_LOGI(SYSTEM_TAG, "User manually turned OFF - will stay off after restart");
|
|
} else if (mode != MOTOR_OFF) {
|
|
motor_state.user_turned_off = false;
|
|
ESP_LOGI(SYSTEM_TAG, "Motor turned ON - will resume after power loss");
|
|
}
|
|
|
|
// If we're in cooldown, update the pending command
|
|
if (motor_state.state == MOTOR_STATE_COOLDOWN) {
|
|
motor_state.pending_mode = mode;
|
|
motor_state.pending_speed = speed_percent;
|
|
ESP_LOGI(SYSTEM_TAG, "Motor in cooldown - command queued for execution");
|
|
return;
|
|
}
|
|
|
|
// Check if this is a direction change that requires cooldown using config macro
|
|
bool requires_cooldown = false;
|
|
if (motor_state.current_speed > 0 && motor_state.mode != MOTOR_OFF) {
|
|
requires_cooldown = IS_DIRECTION_CHANGE(motor_state.mode, mode);
|
|
}
|
|
|
|
if (requires_cooldown) {
|
|
ESP_LOGI(SYSTEM_TAG, "Direction change detected - initiating safety cooldown sequence");
|
|
|
|
// Stop any current ramping
|
|
if (motor_state.ramping) {
|
|
xTimerStop(motor_state.ramp_timer, 0);
|
|
motor_state.ramping = false;
|
|
}
|
|
|
|
// Stop the motor immediately
|
|
motor_state.mode = MOTOR_OFF;
|
|
motor_state.current_speed = 0;
|
|
motor_state.target_speed = 0;
|
|
motor_state.state = MOTOR_STATE_COOLDOWN;
|
|
motor_state.cooldown_remaining_ms = DIRECTION_CHANGE_COOLDOWN_MS;
|
|
apply_motor_pwm(0);
|
|
|
|
// Store the pending command
|
|
motor_state.pending_mode = mode;
|
|
motor_state.pending_speed = speed_percent;
|
|
|
|
// Start cooldown timer
|
|
xTimerStart(motor_state.cooldown_timer, 0);
|
|
|
|
ESP_LOGI(SYSTEM_TAG, "Motor stopped for direction change - %d second cooldown started",
|
|
DIRECTION_CHANGE_COOLDOWN_MS / 1000);
|
|
} else {
|
|
// No direction change required, proceed normally
|
|
|
|
// Stop any current ramping
|
|
if (motor_state.ramping) {
|
|
xTimerStop(motor_state.ramp_timer, 0);
|
|
motor_state.ramping = false;
|
|
}
|
|
|
|
// Stop cooldown timer if running
|
|
if (motor_state.state == MOTOR_STATE_COOLDOWN) {
|
|
xTimerStop(motor_state.cooldown_timer, 0);
|
|
motor_state.cooldown_remaining_ms = 0;
|
|
}
|
|
|
|
start_motor_operation(mode, speed_percent);
|
|
}
|
|
}
|
|
|
|
const motor_state_t* motor_get_state(void) {
|
|
return &motor_state;
|
|
}
|
|
|
|
void motor_update_cooldown_time(void) {
|
|
if (motor_state.state == MOTOR_STATE_COOLDOWN && motor_state.cooldown_remaining_ms > 0) {
|
|
if (motor_state.cooldown_remaining_ms >= STATUS_UPDATE_INTERVAL_MS) {
|
|
motor_state.cooldown_remaining_ms -= STATUS_UPDATE_INTERVAL_MS;
|
|
} else {
|
|
motor_state.cooldown_remaining_ms = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
const char* motor_mode_to_string(motor_mode_t mode) {
|
|
switch (mode) {
|
|
case MOTOR_OFF: return "OFF";
|
|
case MOTOR_EXHAUST: return "EXHAUST";
|
|
case MOTOR_INTAKE: return "INTAKE";
|
|
default: return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
const char* motor_state_to_string(motor_state_enum_t state) {
|
|
switch (state) {
|
|
case MOTOR_STATE_IDLE: return "IDLE";
|
|
case MOTOR_STATE_RAMPING: return "RAMPING";
|
|
case MOTOR_STATE_STOPPING: return "STOPPING";
|
|
case MOTOR_STATE_COOLDOWN: return "COOLDOWN";
|
|
case MOTOR_STATE_RESTARTING: return "RESTARTING";
|
|
default: return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
bool motor_is_ramping(void) {
|
|
return motor_state.ramping;
|
|
}
|
|
|
|
bool motor_is_in_cooldown(void) {
|
|
return motor_state.state == MOTOR_STATE_COOLDOWN;
|
|
}
|
|
|
|
uint32_t motor_get_cooldown_remaining(void) {
|
|
return motor_state.cooldown_remaining_ms;
|
|
}
|
|
|
|
void motor_set_last_on_state(motor_mode_t mode, int speed) {
|
|
if (mode != MOTOR_OFF && IS_VALID_SPEED(speed) && speed > 0) {
|
|
motor_state.last_on_mode = mode;
|
|
motor_state.last_on_speed = speed;
|
|
ESP_LOGI(SYSTEM_TAG, "Last ON state set: %s @ %d%%",
|
|
motor_mode_to_string(mode), speed);
|
|
}
|
|
}
|
|
|
|
void motor_get_last_on_state(motor_mode_t* mode, int* speed) {
|
|
if (mode) *mode = motor_state.last_on_mode;
|
|
if (speed) *speed = motor_state.last_on_speed;
|
|
}
|
|
|
|
void motor_resume_last_state(void) {
|
|
ESP_LOGI(SYSTEM_TAG, "Resuming last state: %s @ %d%%",
|
|
motor_mode_to_string(motor_state.last_on_mode), motor_state.last_on_speed);
|
|
motor_set_speed(motor_state.last_on_mode, motor_state.last_on_speed);
|
|
}
|
|
|
|
void motor_set_user_turned_off(bool turned_off) {
|
|
motor_state.user_turned_off = turned_off;
|
|
}
|
|
|
|
bool motor_get_user_turned_off(void) {
|
|
return motor_state.user_turned_off;
|
|
} |