| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #include "EspServoOutput.h" | ||
| 2 | #include "esp_log.h" | ||
| 3 | #include "esp_idf_version.h" | ||
| 4 | |||
| 5 | static const char* TAG = "EspServoOutput"; | ||
| 6 | |||
| 7 | ✗ | EspServoOutput::EspServoOutput(gpio_num_t pin, ledc_channel_t ledcChannel, bool initialLevel) | |
| 8 | ✗ | : m_pin(pin) | |
| 9 | ✗ | , m_channel(ledcChannel) | |
| 10 | ✗ | , m_currentLevel(initialLevel) { | |
| 11 | // Configure LEDC timer for servo PWM (50Hz) | ||
| 12 | ✗ | ledc_timer_config_t timer_conf = { | |
| 13 | .speed_mode = LEDC_LOW_SPEED_MODE, | ||
| 14 | .duty_resolution = LEDC_TIMER_14_BIT, // 14-bit resolution for precise control | ||
| 15 | .timer_num = LEDC_TIMER_0, | ||
| 16 | .freq_hz = SERVO_FREQ_HZ, | ||
| 17 | .clk_cfg = LEDC_AUTO_CLK, | ||
| 18 | .deconfigure = false}; | ||
| 19 | |||
| 20 | ✗ | esp_err_t ret = ledc_timer_config(&timer_conf); | |
| 21 | ✗ | if (ret != ESP_OK) { | |
| 22 | ✗ | ESP_LOGE(TAG, "Failed to configure LEDC timer: %s", esp_err_to_name(ret)); | |
| 23 | ✗ | return; | |
| 24 | } | ||
| 25 | |||
| 26 | // Configure LEDC channel for the servo | ||
| 27 | ✗ | ledc_channel_config_t channel_conf = { | |
| 28 | ✗ | .gpio_num = static_cast<int>(m_pin), | |
| 29 | .speed_mode = LEDC_LOW_SPEED_MODE, | ||
| 30 | ✗ | .channel = m_channel, | |
| 31 | .intr_type = LEDC_INTR_DISABLE, | ||
| 32 | .timer_sel = LEDC_TIMER_0, | ||
| 33 | .duty = 0, | ||
| 34 | .hpoint = 0, | ||
| 35 | #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 5, 0) | ||
| 36 | .sleep_mode = LEDC_SLEEP_MODE_NO_ALIVE_NO_PD, | ||
| 37 | #endif | ||
| 38 | .flags = { | ||
| 39 | ✗ | .output_invert = 0}}; | |
| 40 | |||
| 41 | ✗ | ret = ledc_channel_config(&channel_conf); | |
| 42 | ✗ | if (ret != ESP_OK) { | |
| 43 | ✗ | ESP_LOGE(TAG, "Failed to configure LEDC channel: %s", esp_err_to_name(ret)); | |
| 44 | ✗ | return; | |
| 45 | } | ||
| 46 | |||
| 47 | // Set initial position | ||
| 48 | ✗ | uint32_t initialAngle = initialLevel ? SERVO_ANGLE_OPEN : SERVO_ANGLE_CLOSED; | |
| 49 | ✗ | uint32_t pulseWidthUs = SERVO_MIN_PULSE_US + (initialAngle * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US)) / 180; | |
| 50 | ✗ | uint32_t duty = (pulseWidthUs * ((1 << 14) - 1)) / SERVO_PERIOD_US; | |
| 51 | ✗ | ledc_set_duty(LEDC_LOW_SPEED_MODE, m_channel, duty); | |
| 52 | ✗ | ledc_update_duty(LEDC_LOW_SPEED_MODE, m_channel); | |
| 53 | |||
| 54 | ✗ | ESP_LOGI(TAG, "Servo on GPIO %d (channel %d) configured (initial: %s)", | |
| 55 | pin, ledcChannel, initialLevel ? "OPEN" : "CLOSED"); | ||
| 56 | ✗ | } | |
| 57 | |||
| 58 | ✗ | EspServoOutput::~EspServoOutput() { | |
| 59 | // Stop PWM and fade to 0 | ||
| 60 | ✗ | ledc_stop(LEDC_LOW_SPEED_MODE, m_channel, 0); | |
| 61 | ✗ | } | |
| 62 | |||
| 63 | ✗ | void EspServoOutput::setLevel(bool high) { | |
| 64 | ✗ | m_currentLevel = high; | |
| 65 | ✗ | setAngle(high ? SERVO_ANGLE_OPEN : SERVO_ANGLE_CLOSED); | |
| 66 | |||
| 67 | ✗ | ESP_LOGD(TAG, "Servo GPIO %d: %s (angle: %lu°)", | |
| 68 | m_pin, | ||
| 69 | high ? "OPEN" : "CLOSED", | ||
| 70 | (unsigned long) (high ? SERVO_ANGLE_OPEN : SERVO_ANGLE_CLOSED)); | ||
| 71 | ✗ | } | |
| 72 | |||
| 73 | ✗ | bool EspServoOutput::getLevel() const { | |
| 74 | ✗ | return m_currentLevel; | |
| 75 | } | ||
| 76 | |||
| 77 | ✗ | void EspServoOutput::setAngle(uint32_t angleDegrees) { | |
| 78 | // Clamp angle to 0-180 | ||
| 79 | ✗ | if (angleDegrees > 180) { | |
| 80 | ✗ | angleDegrees = 180; | |
| 81 | } | ||
| 82 | |||
| 83 | // Calculate pulse width in microseconds | ||
| 84 | // 0° = 1000us, 180° = 2000us | ||
| 85 | ✗ | uint32_t pulseWidthUs = SERVO_MIN_PULSE_US + | |
| 86 | ✗ | (angleDegrees * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US)) / 180; | |
| 87 | |||
| 88 | // Calculate duty cycle for 14-bit resolution | ||
| 89 | // duty = (pulse_width_us / period_us) * (2^14 - 1) | ||
| 90 | ✗ | uint32_t duty = (pulseWidthUs * ((1 << 14) - 1)) / SERVO_PERIOD_US; | |
| 91 | |||
| 92 | // Set the duty cycle directly | ||
| 93 | ✗ | ledc_set_duty(LEDC_LOW_SPEED_MODE, m_channel, duty); | |
| 94 | ✗ | ledc_update_duty(LEDC_LOW_SPEED_MODE, m_channel); | |
| 95 | ✗ | } | |
| 96 |