ESP-IDF-Treiber für den CP2119/CP3119-Motortreiber

#pragma once
#include <cstdio>
#include <cstdint>
#include <cmath>
#include <driver/gpio.h>
#include <driver/ledc.h>

class CP3119M {
public:
    CP3119M(gpio_num_t pwmA, gpio_num_t pwmB, ledc_channel_t channelA, ledc_channel_t channelB);

    void Start(uint16_t speed, bool reverse=false);
    void Stop();

    /**
     * @brief Setzt die Geschwindigkeit und Richtung des Motors.
     *
     * Diese Funktion passt die Motorgeschwindigkeit anhand des übergebenen Wertes an. Wenn die Geschwindigkeit null oder ungültig (NaN) ist,
     * wird der Motor gestoppt. Andernfalls wird die Geschwindigkeit in einen duty cycle im Bereich 0-1023 umgewandelt und der Motor
     * mit der angegebenen Richtung gestartet.
     *
     * @param speed Die gewünschte Geschwindigkeit des Motors als Gleitkommawert. Positive Werte bedeuten Vorwärts-
     *              Richtung, negative Werte bedeuten Rückwärts-Richtung. Der Wert wird für die PWM-Steuerung auf einen Bereich
     *              von 0-1023 skaliert.
     * @param reverse Optionaler Parameter, um die Rückwärts-Richtung explizit festzulegen. Standard ist false. Wenn die Geschwindigkeit
     *                negativ ist, wird die Rückwärts-Richtung automatisch gesetzt.
     */
    inline void SetSpeed(float speed, bool reverse=false) {
        if(speed == 0.0f || isnanf(speed)) {
            Stop();
            return;
        }
        // Geschwindigkeit in den Bereich 0-1023 umwandeln
        uint16_t duty = static_cast<uint16_t>(fabs(1.0-speed) * 1023.0f);
        Start(duty, speed < 0.0f ? true : false);
    }
private:
    gpio_num_t pwmA;
    gpio_num_t pwmB;
    ledc_channel_t channelA;
    ledc_channel_t channelB;
};

#include "CP3119M.hpp"
#include <algorithm>


CP3119M::CP3119M(gpio_num_t pwmA, gpio_num_t pwmB, ledc_channel_t channelA, ledc_channel_t channelB) {
    // Pin-Nummern speichern
    this->pwmA = pwmA;
    this->pwmB = pwmB;
    this->channelA = channelA;
    this->channelB = channelB;

    // LEDC-Timer für PWM-Erzeugung konfigurieren
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .duty_resolution = LEDC_TIMER_10_BIT,  // 8-Bit-Auflösung (0-255)
        .timer_num = LEDC_TIMER_0,
        .freq_hz = 20000,  // Frequenz auf 20kHz setzen
        .clk_cfg = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    // LEDC-Channel für pwmA konfigurieren
    ledc_channel_config_t ledc_channel_a = {
        .gpio_num = pwmA,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = channelA,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,  // Mit duty cycle 0 initialisieren
        .hpoint = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_a));

    // LEDC-Channel für pwmB konfigurieren
    ledc_channel_config_t ledc_channel_b = {
        .gpio_num = pwmB,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = channelB,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,  // Mit duty cycle 0 initialisieren
        .hpoint = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_b));

}

void CP3119M::Start(uint16_t speed, bool reverse) {
    uint16_t duty = std::min(speed, (uint16_t)1023);  // Geschwindigkeit auf den Bereich 0-4095 begrenzen
    // Richtung und duty cycle setzen
    if (reverse) {
        ledc_set_duty(LEDC_LOW_SPEED_MODE, channelB, duty);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, channelB);
        ledc_set_duty(LEDC_LOW_SPEED_MODE, channelA, 1023);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, channelA);
    } else {
        ledc_set_duty(LEDC_LOW_SPEED_MODE, channelA, duty);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, channelA);
        ledc_set_duty(LEDC_LOW_SPEED_MODE, channelB, 1023); // 100%
        ledc_update_duty(LEDC_LOW_SPEED_MODE, channelB);
    }
}

void CP3119M::Stop() {
    // Beide Channels auf 100% duty cycle setzen (also bremsen)
    ledc_set_duty(LEDC_LOW_SPEED_MODE, channelA, 1023);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, channelA);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, channelB, 1023);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, channelB);
}

#include "CP3119M.hpp"
#include <driver/gpio.h>

extern "C" void app_main() {
    // CP3119-Motortreiber an GPIO 18 und 19 mit LEDC-Channels 0 und 1 initialisieren
    CP3119M motor(gpio_num_t::GPIO_NUM_18, gpio_num_t::GPIO_NUM_19, LEDC_CHANNEL_0, LEDC_CHANNEL_1);

    // Motor mit voller Geschwindigkeit in Vorwärts-Richtung starten
    motor.SetSpeed(1.0f);

    // Eine Weile laufen lassen
    vTaskDelay(pdMS_TO_TICKS(5000));

    // Motor stoppen
    motor.Stop();

    return 0;
}