# ESP-IDF_Robot Project Layout

/main
    app_main.c
    system_init.c
    system_init.h
    scheduler.c
    scheduler.h

/subsystems
    /motors
        motors.c
        motors.h
    /adc
        adc.c
        adc.h
        joystick.c
        joystick.h
    /sensors
        temp_sensor.c
        temp_sensor.h
        ina219_sensor.c
        ina219_sensor.h
        ultrasonic_sensor.c
        ultrasonic_sensor.h
    /connectivity
        wifi_sys.c
        wifi_sys.h
        espnow_sys.c
        espnow_sys.h
        mqtt_sys.c
        mqtt_sys.h
    /ui
        ui_led.c
        ui_led.h
        ui_buttons.c
        ui_buttons.h

/include
    config.h
    controls.h
    espnow_config.h

CMakeLists.txt
/main/CMakeLists.txt
/subsystems/CMakeLists.txt

### Top-level CMakeLists.txt

``` text
cmake_minimum_required(VERSION 3.16)
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
set(EXTRA_COMPONENT_DIRS "subsystems")
project(robot_firmware)
```

### /main/CMakeLists.txt

``` cmake
idf_component_register(
    SRCS "app_main.c" "system_init.c" "scheduler.c"
    INCLUDE_DIRS "."
    REQUIRES motors adc sensors connectivity ui
)
```

### /subsystems/CMakeLists.txt

``` text
# subsystems/motors/CMakeLists.txt
idf_component_register(
    SRCS "motors.c"
    INCLUDE_DIRS "."
    REQUIRES driver
)
```

## CORE

### system_init.h

``` c
#pragma once

void system_init(void);
```

### system_init.c

``` c
#include "system_init.h"
#include "nvs_flash.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "esp_log.h"

void system_init(void) {
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    ESP_ERROR_CHECK(esp_event_loop_create_default());
    ESP_ERROR_CHECK(esp_netif_init());
}
```

### scheduler.h

``` c
#pragma once
#include "freertos/FreeRTOS.h"

typedef struct motor_system_t      motor_system_t;
typedef struct adc_system_t        adc_system_t;
typedef struct mqtt_system_t       mqtt_system_t;
typedef struct temp_sensor_system_t temp_sensor_system_t;
typedef struct ina219_system_t     ina219_system_t;
typedef struct ultrasonic_system_t ultrasonic_system_t;

typedef struct scheduler_t {
    motor_system_t       *motors;
    adc_system_t         *adc;
    mqtt_system_t        *mqtt;
    temp_sensor_system_t *temp;
    ina219_system_t      *ina;
    ultrasonic_system_t  *ultra;
} scheduler_t;

void scheduler_init(scheduler_t *sched);
void scheduler_start(scheduler_t *sched);
```

### scheduler.c

``` c
#include "scheduler.h"
#include "freertos/task.h"

static void scheduler_task(void *arg) {
    scheduler_t *sched = (scheduler_t *)arg;
    TickType_t last = xTaskGetTickCount();

    while (1) {
        TickType_t now = xTaskGetTickCount();

        if (sched->adc && sched->adc->update)       sched->adc->update(sched->adc, now);
        if (sched->motors && sched->motors->update) sched->motors->update(sched->motors, now);
        if (sched->mqtt && sched->mqtt->update)     sched->mqtt->update(sched->mqtt, now);
        if (sched->temp && sched->temp->update)     sched->temp->update(sched->temp, now);
        if (sched->ina && sched->ina->update)       sched->ina->update(sched->ina, now);
        if (sched->ultra && sched->ultra->update)   sched->ultra->update(sched->ultra, now);

        vTaskDelayUntil(&last, pdMS_TO_TICKS(10)); // 100 Hz
    }
}

void scheduler_init(scheduler_t *sched) {
    (void)sched;
}

void scheduler_start(scheduler_t *sched) {
    xTaskCreate(scheduler_task, "scheduler", 4096, sched, 5, NULL);
}
```

### app_main.c

``` c
#include "system_init.h"
#include "scheduler.h"

#include "motors.h"
#include "adc.h"
#include "temp_sensor.h"
#include "ina219_sensor.h"
#include "ultrasonic_sensor.h"
#include "wifi_sys.h"
#include "espnow_sys.h"
#include "mqtt_sys.h"
#include "ui_led.h"
#include "ui_buttons.h"

void app_main(void) {
    system_init();

    static motor_system_t       motors;
    static adc_system_t         adc;
    static temp_sensor_system_t temp;
    static ina219_system_t      ina;
    static ultrasonic_system_t  ultra;
    static mqtt_system_t        mqtt;
    static scheduler_t          sched;

    motor_system_init(&motors);
    adc_system_init(&adc);
    temp_sensor_system_init(&temp);
    ina219_system_init(&ina);
    ultrasonic_system_init(&ultra);

    wifi_system_init();      // no struct needed
    espnow_system_init(&adc, &motors); // pass references if needed
    mqtt_system_init(&mqtt);

    ui_led_init();
    ui_buttons_init();

    sched.motors = &motors;
    sched.adc    = &adc;
    sched.temp   = &temp;
    sched.ina    = &ina;
    sched.ultra  = &ultra;
    sched.mqtt   = &mqtt;

    scheduler_init(&sched);
    scheduler_start(&sched);
}
```

## Motors Subsystem

### motors.h

``` c
#pragma once
#include "freertos/FreeRTOS.h"
#include "driver/ledc.h"

typedef struct motor_system_t motor_system_t;

struct motor_system_t {
    int pwm_left;
    int pwm_right;

    void (*set)(motor_system_t *self, int left, int right);
    void (*stop)(motor_system_t *self);
    void (*update)(motor_system_t *self, TickType_t now);
};

void motor_system_init(motor_system_t *sys);
```

### motors.c

``` c
#include "motors.h"
#include "config.h"
#include "esp_log.h"

static const char *TAG = "MOTORS";

static void motor_hw_init(void) {
    // LEDC timers + channels using your config.h constants
    // (MTR_FREQUENCY, MTR_MODE, MTR_DUTY_RES, etc.)
}

static void motor_apply_pwm(motor_system_t *self) {
    // Map self->pwm_left / pwm_right to LEDC duty
    // Use ledc_set_duty + ledc_update_duty
}

static void motor_set_impl(motor_system_t *self, int left, int right) {
    self->pwm_left  = left;
    self->pwm_right = right;
    motor_apply_pwm(self);
}

static void motor_stop_impl(motor_system_t *self) {
    motor_set_impl(self, 0, 0);
}

static void motor_update_impl(motor_system_t *self, TickType_t now) {
    (void)now;
    // Optional: ramping, safety, timeouts
}

void motor_system_init(motor_system_t *sys) {
    motor_hw_init();
    sys->pwm_left  = 0;
    sys->pwm_right = 0;
    sys->set    = motor_set_impl;
    sys->stop   = motor_stop_impl;
    sys->update = motor_update_impl;
}

```

### adc.h

``` c
#pragma once
#include "freertos/FreeRTOS.h"

typedef struct adc_system_t adc_system_t;

struct adc_system_t {
    int x_raw;
    int y_raw;

    void (*read)(adc_system_t *self);
    void (*update)(adc_system_t *self, TickType_t now);
};

void adc_system_init(adc_system_t *sys);

```

### acd.c

``` c
#include "adc.h"
#include "driver/adc.h"
#include "esp_adc/adc_oneshot.h"
#include "esp_log.h"

static const char *TAG = "ADC";

static adc_oneshot_unit_handle_t adc1_handle;

static void adc_hw_init(void) {
    adc_oneshot_unit_init_cfg_t init_config = {
        .unit_id = ADC_UNIT_1,
    };
    ESP_ERROR_CHECK(adc_oneshot_new_unit(&init_config, &adc1_handle));

    adc_oneshot_chan_cfg_t cfg = {
        .bitwidth = SOC_ADC_DIGI_MAX_BITWIDTH,
        .atten = ADC_ATTEN_DB_11,
    };
    ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, ADC_CHANNEL_0, &cfg));
    ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, ADC_CHANNEL_1, &cfg));
}

static void adc_read_impl(adc_system_t *self) {
    int x = 0, y = 0;
    ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, ADC_CHANNEL_0, &x));
    ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, ADC_CHANNEL_1, &y));
    self->x_raw = x;
    self->y_raw = y;
}

static void adc_update_impl(adc_system_t *self, TickType_t now) {
    (void)now;
    adc_read_impl(self);
}

void adc_system_init(adc_system_t *sys) {
    adc_hw_init();
    sys->x_raw = 0;
    sys->y_raw = 0;
    sys->read   = adc_read_impl;
    sys->update = adc_update_impl;
}

```

### joystick.h

``` c
#pragma once

void joystick_mix(int x_raw, int y_raw, int *pwm_left, int *pwm_right);

```

### joystick.c

``` c
#include "joystick.h"
#include <math.h>

static float clampf(float v, float min, float max) {
    return v < min ? min : (v > max ? max : v);
}

void joystick_mix(int X_raw, int Y_raw, int *pwm_a, int *pwm_b) {
    float x = (float)(X_raw - 1020) / 1020.0f;
    float y = (float)(Y_raw - 1020) / 1020.0f;

    const float k = 0.4f;

    float L0 = y + k * x;
    float R0 = y - k * x;

    float m = fmaxf(1.0f, fmaxf(fabsf(L0), fabsf(R0)));
    float L = L0 / m;
    float R = R0 / m;

    float L_scaled = L * 8190.0f;
    float R_scaled = R * 8190.0f;

    *pwm_a = (int)clampf(L_scaled, -8191.0f, 8190.0f);
    *pwm_b = (int)clampf(R_scaled, -8191.0f, 8190.0f);
}

```

### wifi_sys.h

``` c
#pragma once

void wifi_system_init(void);
```

### wifi_sys.c

``` c
#include "wifi_sys.h"
#include "esp_wifi.h"
#include "esp_netif.h"
#include "esp_log.h"
#include "mqtt_sys.h" // for SSID/PASS if you keep them there or in config.h

static const char *TAG = "WIFI_SYS";

void wifi_system_init(void) {
    esp_netif_create_default_wifi_sta();
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
    wifi_config_t wifi_config = {
        .sta = {
            .ssid = WIFI_SSID,
            .password = WIFI_PASSWORD,
        },
    };
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
    ESP_ERROR_CHECK(esp_wifi_start());
    ESP_ERROR_CHECK(esp_wifi_connect());
}
```

### espnow_sys.h

``` c
#pragma once
#include <stdint.h>

typedef struct adc_system_t adc_system_t;
typedef struct motor_system_t motor_system_t;

void espnow_system_init(adc_system_t *adc, motor_system_t *motors);
```

### espnow_sys.c

``` c
#include "espnow_sys.h"
#include "esp_now.h"
#include "esp_wifi.h"
#include "esp_log.h"
#include "controls.h" // for sensors_data_t, etc.

static const char *TAG = "ESPNOW_SYS";

static adc_system_t   *s_adc = NULL;
static motor_system_t *s_motors = NULL;

static void espnow_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len) {
    if (len < sizeof(sensors_data_t)) return;
    sensors_data_t buf;
    memcpy(&buf, data, sizeof(buf));

    if (s_adc) {
        s_adc->x_raw = buf.x_axis;
        s_adc->y_raw = buf.y_axis;
    }

    if (s_motors && s_motors->set) {
        // if you still want direct PWM from RC:
        s_motors->set(s_motors, buf.motor1_rpm_pcm, buf.motor2_rpm_pcm);
    }
}

void espnow_system_init(adc_system_t *adc, motor_system_t *motors) {
    s_adc = adc;
    s_motors = motors;

    ESP_ERROR_CHECK(esp_now_init());
    ESP_ERROR_CHECK(esp_now_register_recv_cb(espnow_recv_cb));

    // add peer config here if needed
}
```

### mqtt_sys.h

``` c
#pragma once
#include "freertos/FreeRTOS.h"

typedef struct mqtt_system_t mqtt_system_t;

struct mqtt_system_t {
    float temp;
    float battery_voltage;
    float sys_current;
    float sys_power;
    int pwm_1;
    int pwm_2;

    void (*publish_temp)(mqtt_system_t *self, float temp);
    void (*publish_pwm)(mqtt_system_t *self, int pwm1, int pwm2);
    void (*update)(mqtt_system_t *self, TickType_t now);
};

void mqtt_system_init(mqtt_system_t *sys);

#define WIFI_SSID     "IoT_bots"
#define WIFI_PASSWORD "208208208"
```

### mqtt_sys.c

``` c
#include "mqtt_sys.h"
#include "mqtt_client.h"
#include "esp_log.h"

static const char *TAG = "MQTT_SYS";
static const char *MQTT_BROKER_URI = "mqtt://74.14.210.168";

static esp_mqtt_client_handle_t s_client = NULL;

static void mqtt_publish_temp_impl(mqtt_system_t *self, float temp) {
    self->temp = temp;
    if (!s_client) return;
    char buf[16];
    snprintf(buf, sizeof(buf), "%.02f", temp);
    esp_mqtt_client_publish(s_client, "/bitrider/temp", buf, 0, 1, 0);
}

static void mqtt_publish_pwm_impl(mqtt_system_t *self, int pwm1, int pwm2) {
    self->pwm_1 = pwm1;
    self->pwm_2 = pwm2;
    if (!s_client) return;
    char b1[8], b2[8];
    snprintf(b1, sizeof(b1), "%d", pwm1);
    snprintf(b2, sizeof(b2), "%d", pwm2);
    esp_mqtt_client_publish(s_client, "/bitrider/pwm-1", b1, 0, 1, 0);
    esp_mqtt_client_publish(s_client, "/bitrider/pwm-2", b2, 0, 1, 0);
}

static void mqtt_event_handler(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data) {
    esp_mqtt_event_handle_t event = event_data;
    switch (event->event_id) {
        case MQTT_EVENT_CONNECTED:
            ESP_LOGI(TAG, "MQTT connected");
            s_client = event->client;
            break;
        case MQTT_EVENT_DISCONNECTED:
            ESP_LOGI(TAG, "MQTT disconnected");
            s_client = NULL;
            break;
        default:
            break;
    }
}

static void mqtt_update_impl(mqtt_system_t *self, TickType_t now) {
    (void)self;
    (void)now;
    // could periodically republish or handle queued data
}

void mqtt_system_init(mqtt_system_t *sys) {
    sys->temp = 0.0f;
    sys->battery_voltage = 0.0f;
    sys->sys_current = 0.0f;
    sys->sys_power = 0.0f;
    sys->pwm_1 = 0;
    sys->pwm_2 = 0;

    sys->publish_temp = mqtt_publish_temp_impl;
    sys->publish_pwm  = mqtt_publish_pwm_impl;
    sys->update       = mqtt_update_impl;

    esp_mqtt_client_config_t cfg = {
        .broker.address.uri = MQTT_BROKER_URI,
    };
    esp_mqtt_client_handle_t client = esp_mqtt_client_init(&cfg);
    esp_mqtt_client_register_event(client, ESP_EVENT_ANY_ID, mqtt_event_handler, NULL);
    esp_mqtt_client_start(client);
}
```

### temp_sensor.h

``` c
#pragma once

#include "freertos/FreeRTOS.h"

typedef struct temp_sensor_system_t temp_sensor_system_t;

struct temp_sensor_system_t {
    float last_celsius;

    // API
    void (*update)(temp_sensor_system_t *self, TickType_t now);
};

void temp_sensor_system_init(temp_sensor_system_t *sys);
```

### temp_sensor.c

``` c
#include "temp_sensor.h"
#include "driver/temperature_sensor.h"
#include "esp_log.h"

static const char *TAG = "TEMP_SENSOR";

static temperature_sensor_handle_t s_temp_handle = NULL;

static void temp_hw_init(void) {
    temperature_sensor_config_t cfg = TEMPERATURE_SENSOR_CONFIG_DEFAULT(10, 50);
    ESP_ERROR_CHECK(temperature_sensor_install(&cfg, &s_temp_handle));
    ESP_ERROR_CHECK(temperature_sensor_enable(s_temp_handle));
    ESP_LOGI(TAG, "Temperature sensor initialized");
}

static void temp_update_impl(temp_sensor_system_t *self, TickType_t now) {
    (void)now;
    if (!s_temp_handle) return;

    float c = 0.0f;
    esp_err_t err = temperature_sensor_get_celsius(s_temp_handle, &c);
    if (err == ESP_OK) {
        self->last_celsius = c;
        ESP_LOGD(TAG, "Temperature: %.02f C", c);
    } else {
        ESP_LOGW(TAG, "Failed to read temperature sensor: %s", esp_err_to_name(err));
    }
}

void temp_sensor_system_init(temp_sensor_system_t *sys) {
    temp_hw_init();
    sys->last_celsius = 0.0f;
    sys->update = temp_update_impl;
}
```

## blink_example_main.c

/* Robot Controls
    Generate PWM signals to control motors.

    By:         Alexander Bobkov
    Date:       Dec 21, 2024

    Updated:    Jan 10, 2025
                Jun 26, 2025
                Jul 26, 2025 (ESP-IDF + MQTT + WiFi)
                Aug 6 , 2025 Continous interpolation of joystick x- and y- values
                Jan 3,  2026 Revised motor control logic

    built-in LED GPIO:          10
    build-in push button GPIO:  3

    ESP-PDF: v5.4.1

    SPECS

    Voltage, DevBoard:  5V
    Voltage, Robot:     7.4V (2S LiPo battery)
    Current, standby:   300mA
    Current, motors:    850mA
*/

#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "driver/temperature_sensor.h"
//#include "driver/mcpwm.h"
#include "esp_log.h"
#include "led_strip.h"
#include "sdkconfig.h"
/* ADC */
#include "rc.h"
/* Motors Controls */
#include "motor_controls.h"
/* System-wide controls */
#include "controls.h"
#include "esp_adc/adc_continuous.h"

/* ESP-NOW */
#include <string.h>
#include <assert.h>
#include "freertos/semphr.h"
#include "freertos/timers.h"
#include "nvs_flash.h"
#include "esp_random.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_crc.h"
#include "esp_netif.h"
#include "esp_now.h"
#include "esp_mac.h"
#include "esp_wifi.h"
#include "mqtt.h"
#include "esp_system.h"
#include "espnow_config.h"

#include "ultrasonic.h"
#include "ina219.h"

#include "config.h"

static const char *TAG = "ESP IDF Robot";

#define I2C_PORT 0
#define I2C_ADDR 0x40
#define CONFIG_EXAMPLE_I2C_MASTER_SDA 3
#define CONFIG_EXAMPLE_I2C_MASTER_SCL 2
#define CONFIG_EXAMPLE_SHUNT_RESISTOR_MILLI_OHM 100


/* Use project configuration menu (idf.py menuconfig) to choose the GPIO to blink,
   or you can edit the following line and set a number here.
*/

// Retrieve values from configuration menu
#define BLINK_GPIO                  CONFIG_BLINK_GPIO       // 10 GPIO of on-board LED
#define PUSH_BTN_GPIO               CONFIG_BUTTON_GPIO      // 8, not 3 GPIO of on-board push-button
#define MTR_FL_GPIO                 0 //CONFIG_MOTOR_FRONT_LEFT_GPIO
// ADC
#define ADC_ATTEN                   ADC_ATTEN_DB_11
#define ADC_BIT_WIDTH               SOC_ADC_DIGI_MAX_BITWIDTH
#define ADC_UNIT                    ADC_UNIT_1
#define ADC_CONV_MODE               ADC_CONV_BOTH_UNIT// ADC_CONV_SINGLE_UNIT_1
#define ADC_OUTPUT_TYPE             ADC_DIGI_OUTPUT_FORMAT_TYPE2    // ESP32C3
#define READ_LEN                    1024//256
//#define ADC_GET_CHANNEL(p_data)     ((p_data)->type2.channel)
//#define ADC_GET_DATA(p_data)        ((p_data)->type2.data)
#define PROJ_X                      (1)                     // ADC1_CH1; 0 GPIO joystick, x-axis
#define PROJ_Y                      (0)                     // ADC1_CH0; 1 GPIO joystick, y-axis
#define NAV_BTN                     (8)                     // 8 GPIO joystick button
#define _ADC_UNIT_STR(unit)         #unit
#define ADC_UNIT_STR(unit)          _ADC_UNIT_STR(unit)
uint32_t x_avg = 0, y_avg = 0;
static TaskHandle_t led_task_handle;
static TaskHandle_t s_task_handle;
static TaskHandle_t m_task_handle;  // Task for controlling motors PWMs
static adc_channel_t channel[2] = {ADC_CHANNEL_0, ADC_CHANNEL_1};
static sensors_data_t buf;

#define ESP_INTR_FLAG_DEFAULT 0

#define GPIO_INPUT_PIN_SEL ((1ULL<<PUSH_BTN_GPIO) | (1ULL<<NAV_BTN))
#define GPIO_OUTPUT_PIN_SEL ((1ULL<<BLINK_GPIO))

/* ESPNOW can work in both station and softap mode. It is configured in menuconfig. */
#if CONFIG_ESPNOW_WIFI_MODE_STATION
#define ESPNOW_WIFI_MODE WIFI_MODE_STA
#define ESPNOW_WIFI_IF   ESP_IF_WIFI_STA
#else
#define ESPNOW_WIFI_MODE WIFI_MODE_AP
#define ESPNOW_WIFI_IF   ESP_IF_WIFI_AP
#endif

static temperature_sensor_handle_t temp_sensor;
static temperature_sensor_config_t temp_sensor_config;
float tsens_value;
static QueueHandle_t gpio_evt_queue = NULL;
static uint8_t s_led_state = 1;


/*  ============================
            ESP NOW
    ============================

    ESP32-C3 Blue board MAC:        54:32:04:46:71:80
    ESP32-C3 SuperMini MAC:         34:b7:da:f9:33:8d
    ESP32-C3 Breadboard #1 MAC:     e4:b0:63:17:9e:45
    ESP32-C3 Breadboard #2 MAC:     9c:9e:6e:14:b5:54 (rapid blink)
    ESP32-C3 zenBoard MAC:          dc:da:0c:8e:b3:70
*/
static uint8_t broadcast_mac[ESP_NOW_ETH_ALEN] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
//static uint8_t broadcast_mac[ESP_NOW_ETH_ALEN]  = {0x54, 0x32, 0x04, 0x46, 0x71, 0x80};     // MAC address of troubleshooting Dev board
//static uint8_t broadcast_mac[ESP_NOW_ETH_ALEN]  = {0xE4, 0xB0, 0x63, 0x17, 0x9E, 0x45};
static uint8_t robot_mac[ESP_NOW_ETH_ALEN]      = {0xE4, 0xB0, 0x63, 0x17, 0x9E, 0x45};     // MAC address of Robot
static uint8_t rc_mac[ESP_NOW_ETH_ALEN]         = {0x34, 0xB7, 0xDA, 0xF9, 0x33, 0x8D};     // MAC address of Remote Control
static uint8_t espnow_seq[ESPNOW_DATA_MAX]      = {0, 0};

static int rc_x = 0, rc_y = 0;
static int pwm_motor_1 = 0;
static int pwm_motor_2 = 0;
static int pwm_motor_3 = 0;
static int pwm_motor_4 = 0;

//uint8_t broadcastAddress[] = {};
//struct_message controlData;
esp_now_peer_info_t peerInfo;
static void espnow_deinit(espnow_send_param_t *send_param);

static void blink_led(void) {
    /* Set the GPIO level according to the state (LOW or HIGH)*/
    gpio_set_level(BLINK_GPIO, s_led_state);
}

static void IRAM_ATTR gpio_isr_handler (void* arg) {
    uint32_t gpio_num = (uint32_t) arg;
    xQueueSendFromISR(gpio_evt_queue, &gpio_num, NULL);
}
// Push button interrupt task
static void gpio_task (void* arg) {
    uint32_t io_num;
    for (;;) {
        if (xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY)) {
            printf("GPIO[%"PRIu32"] intr, val: %d\n", io_num, gpio_get_level(io_num));
        }
    }
}
static void nav_key_task (void* arg) {
    uint32_t io_num;
    for (;;) {
        if (xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY)) {
            //printf("GPIO[%"PRIu32"] intr, val: %d\n", io_num, gpio_get_level(io_num));
        }
    }
}

static void configure_button (void) {
    ESP_LOGI(TAG, "Configured on-board push button");
    //gpio_reset_pin(PUSH_BTN_GPIO);
    //gpio_set_direction(PUSH_BTN_GPIO, GPIO_MODE_INPUT);
}

// Struct for storing DC Motors PWM values
static void motors_init (void) {
    m.motor1_rpm_pcm = 0;
    m.motor2_rpm_pcm = 0;
    m.motor3_rpm_pcm = 0;
    m.motor4_rpm_pcm = 0;
}

// Initialize DC Motors PWM timers and channels
static void ledc_init (void) {

    // MOTOR FRONT RIGHT, FORWARD
    ledc_timer_config_t ledc_timer_1 = {
        .speed_mode =       MTR_MODE,// LEDC_MODE,
        .duty_resolution =  MTR_DUTY_RES,// LEDC_DUTY_RES,
        .timer_num =        MTR_FRONT_RIGHT_TMR,// LEDC_TIMER,
        .freq_hz =          MTR_FREQUENCY,// LEDC_FREQUENCY,
        .clk_cfg =          LEDC_APB_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer_1));
    ledc_channel_config_t ledc_channel_1 = {
        .speed_mode =       MTR_MODE,
        .channel =          MTR_FRONT_RIGHT,// LEDC_CHANNEL_0,// MTR_FRONT_RIGHT,
        .timer_sel =        MTR_FRONT_RIGHT_TMR,// LEDC_TIMER,
        .intr_type =        LEDC_INTR_DISABLE,
        .gpio_num =         MTR_FRONT_RIGHT_IO,
        .duty =             MTR_FRONT_RIGHT_DUTY,
        .hpoint =           0,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_1));
    // MOTOR FRONT LEFT, FORWARD
    ledc_timer_config_t ledc_timer_2 = {
        .speed_mode =       MTR_MODE,
        .duty_resolution =  MTR_DUTY_RES,
        .timer_num =        MTR_FRONT_LEFT_TMR,
        .freq_hz =          MTR_FREQUENCY,
        .clk_cfg =          LEDC_APB_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer_2));
    ledc_channel_config_t ledc_channel_2 = {
        .speed_mode =       MTR_MODE,
        .channel =          MTR_FRONT_LEFT,
        .timer_sel =        MTR_FRONT_LEFT_TMR,
        .intr_type =        LEDC_INTR_DISABLE,
        .gpio_num =         MTR_FRONT_LEFT_IO,
        .duty =             MTR_FRONT_LEFT_DUTY,
        .hpoint =           0,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_2));

    // MOTOR FRONT RIGHT, REVERSE
    ledc_timer_config_t ledc_timer_3 = {
        .speed_mode =       MTR_MODE,// LEDC_MODE,
        .duty_resolution =  MTR_DUTY_RES,// LEDC_DUTY_RES,
        .timer_num =        MTR_FRONT_RIGHT_REV_TMR,// LEDC_TIMER,
        .freq_hz =          MTR_FREQUENCY,// LEDC_FREQUENCY,
        .clk_cfg =          LEDC_APB_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer_3));
    ledc_channel_config_t ledc_channel_3 = {
        .speed_mode =       MTR_MODE,
        .channel =          MTR_FRONT_RIGHT_REV,// LEDC_CHANNEL_0,// MTR_FRONT_RIGHT,
        .timer_sel =        MTR_FRONT_RIGHT_REV_TMR,// LEDC_TIMER,
        .intr_type =        LEDC_INTR_DISABLE,
        .gpio_num =         MTR_FRONT_RIGHT_REV_IO,
        .duty =             MTR_FRONT_RIGHT_REV_DUTY,
        .hpoint =           0,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_3));
    // MOTOR FRONT LEFT, REVERSE
    ledc_timer_config_t ledc_timer_4 = {
        .speed_mode =       MTR_MODE,
        .duty_resolution =  MTR_DUTY_RES,
        .timer_num =        MTR_FRONT_LEFT_REV_TMR,
        .freq_hz =          MTR_FREQUENCY,
        .clk_cfg =          LEDC_APB_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer_4));
    ledc_channel_config_t ledc_channel_4 = {
        .speed_mode =       MTR_MODE,
        .channel =          MTR_FRONT_LEFT_REV,
        .timer_sel =        MTR_FRONT_LEFT_REV_TMR,
        .intr_type =        LEDC_INTR_DISABLE,
        .gpio_num =         MTR_FRONT_LEFT_REV_IO,
        .duty =             MTR_FRONT_LEFT_REV_DUTY,
        .hpoint =           0,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_4));
}

/* ESP-NOW */
// Wi-Fi should start before using ESP-NOW
static void wifi_init()
{
    /*
    * STAND-ALONE
    */
    /*ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK( esp_wifi_init(&cfg) );
    ESP_ERROR_CHECK( esp_wifi_set_storage(WIFI_STORAGE_RAM) );
    ESP_ERROR_CHECK( esp_wifi_set_mode(WIFI_MODE_STA));//ESPNOW_WIFI_MODE));
    ESP_ERROR_CHECK( esp_wifi_start());
    ESP_ERROR_CHECK( esp_wifi_set_channel(CONFIG_ESPNOW_CHANNEL, WIFI_SECOND_CHAN_NONE));
    #if CONFIG_ESPNOW_ENABLE_LONG_RANGE
    ESP_ERROR_CHECK( esp_wifi_set_protocol(ESPNOW_WIFI_IF, WIFI_PROTOCOL_11B|WIFI_PROTOCOL_11G|WIFI_PROTOCOL_11N|WIFI_PROTOCOL_LR) );
    #endif*/

    /*
    * WI-FI
    */
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());
    esp_netif_create_default_wifi_sta();
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK( esp_wifi_init(&cfg) );
    ESP_ERROR_CHECK( esp_wifi_set_storage(WIFI_STORAGE_RAM) );
    ESP_ERROR_CHECK( esp_wifi_set_mode(WIFI_MODE_STA));//ESPNOW_WIFI_MODE));
    wifi_config_t wifi_config = {
        .sta = {
            .ssid = WIFI_SSID,//"IoT_bots2",
            .password = WIFI_PASSWORD,// "208208208",
        },
    };
    ESP_ERROR_CHECK (esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
    //ESP_ERROR_CHECK( esp_wifi_set_channel(CONFIG_ESPNOW_CHANNEL, WIFI_SECOND_CHAN_NONE));
    ESP_ERROR_CHECK( esp_wifi_start());
    //ESP_ERROR_CHECK( esp_wifi_set_channel(CONFIG_ESPNOW_CHANNEL, WIFI_SECOND_CHAN_NONE));
    
    ESP_ERROR_CHECK( esp_wifi_connect() );
}

static void led_task (void *arg) {
    while(1) {
        //ESP_LOGI(TAG, "Turning the LED %s!", s_led_state == true ? "ON" : "OFF");
	    gpio_set_level(BLINK_GPIO, s_led_state);
	    vTaskDelay(CONFIG_BLINK_PERIOD / portTICK_PERIOD_MS);
        s_led_state = !s_led_state;
	}
}
static void temp_sensor_task (void *arg) {
    while (true) {
        ESP_LOGI("ESP32-C3", "Reading sensor temperature");
        float tsens_value;
        ESP_ERROR_CHECK(temperature_sensor_get_celsius(temp_sensor, &tsens_value));
        ESP_LOGW("ESP32-C3", "Temperature value %.02f ℃", tsens_value);
        mqtt_update_temp (tsens_value); 
        vTaskDelay(5000 / portTICK_PERIOD_MS);
    }
}
/* UPDATED MOTOR LOGIC */
static float clampf (float val, float min, float max) {
    return (val < min) ? min : (val > max) ? max : val;
}
void joystick_mix (int X_raw, int Y_raw, int *pwm_a, int *pwm_b) {
    // 1. Normalize joystick to [-1 .. +1]
    float x = (float)(X_raw - 1020) / 1020.0f; float y = (float)(Y_raw - 1020) / 1020.0f;
    
    // 2. Steering gain for smooth arcs
    const float k = 0.4f;
    
    // 3. Raw differential mix
    float L0 = y + k * x;
    float R0 = y - k * x;
    
    // 4. Normalize pair so neither exceeds magnitude 1
    float m = fmaxf(1.0f, fmaxf(fabsf(L0), fabsf(R0)));
    float L = L0 / m;
    float R = R0 / m;
    
    // 5. Scale to signed PWM range [-8191 .. +8190]
    float L_scaled = L * 8190.0f;
    float R_scaled = R * 8190.0f;
    
    // 6. Clamp and output as integers
    *pwm_a = (int)clampf(L_scaled, -8191.0f, 8190.0f);
    *pwm_b = (int)clampf(R_scaled, -8191.0f, 8190.0f);
}
// Task function to read joystick values and update motors rotation speeds.
static void rc_task (void *arg) {
    while (true) {
        // update_pwm (rc_x, rc_y);     // Orginal motor update logic
        joystick_mix (rc_y, rc_x, &pwm_motor_1, &pwm_motor_2);
        update_motors_pwm (pwm_motor_1, pwm_motor_2);   // Revised motor update logic
        //ESP_LOGI("x,y", "( %d, %d ) [ %d, %d] ", rc_x, rc_y, x, y);
        vTaskDelay (100 / portTICK_PERIOD_MS);  // Determines responsiveness
        //vTaskDelay (1000 / portTICK_PERIOD_MS);
    }
}
static void display_xy() {
    while (true) {
        ESP_LOGI("x,y", "( %d, %d ) [ %d, %d] ", rc_x, rc_y, x, y);
        ESP_LOGI("PWM", "M1: %d, M2: %d, M3: %d, M4: %d", 
            m.motor1_rpm_pcm, m.motor2_rpm_pcm, 
            m.motor3_rpm_pcm, m.motor4_rpm_pcm);
        joystick_mix (rc_y, rc_x, &pwm_motor_1, &pwm_motor_2);
        ESP_LOGI("Converted PWM", "M1+M2: %d, M3+M4: %d", pwm_motor_1, pwm_motor_2);
        uint8_t channel;
        wifi_band_t band;
        esp_wifi_get_channel(&channel, NULL);
        esp_wifi_get_band(&band);
        ESP_LOGI(TAG, "Wi-Fi Channel: %d, Band: %d", channel, band);

        vTaskDelay (1000 / portTICK_PERIOD_MS);
    }
}

/*
    EXP32-C3 Chip built-in temprature sensor
    Read & display the temperature value
*/
static void chip_sensor_init () {
    temp_sensor = NULL;
    temperature_sensor_config_t temp_sensor_config = TEMPERATURE_SENSOR_CONFIG_DEFAULT(10, 50);
    ESP_ERROR_CHECK(temperature_sensor_install(&temp_sensor_config, &temp_sensor));

    ESP_LOGI(TAG, "Enable temperature sensor");
    ESP_ERROR_CHECK(temperature_sensor_enable(temp_sensor));
}
static void display_chip_temperature () {
    ESP_LOGI("ESP32-C3", "Reading sensor temperature");
    ESP_ERROR_CHECK(temperature_sensor_get_celsius(temp_sensor, &tsens_value));
    ESP_LOGW("ESP32-C3", "Temperature value %.02f ℃", tsens_value);
}

// ESP-NOW callback on data received
void onDataReceived (const uint8_t *mac_addr, const uint8_t *data, uint8_t data_len) {

    memcpy(&buf, data, sizeof(buf));    // Write buffer into the struct
    rc_x = buf.x_axis;                  // Save joystic x-axis value
    rc_y = buf.y_axis;                  // Save joystic y-axis value
    // Update motors PWM values from received data
    m.motor1_rpm_pcm = buf.motor1_rpm_pcm;
    m.motor2_rpm_pcm = buf.motor2_rpm_pcm;
    m.motor3_rpm_pcm = buf.motor3_rpm_pcm;
    m.motor4_rpm_pcm = buf.motor4_rpm_pcm;
    // Update motors PWM values using joystick x- and y-axis values
    update_pwm(rc_x, rc_y);
    mqtt_update_pwm_1(rc_x);            // Publish PWM-1 on MQTT Broker
    mqtt_update_pwm_2(rc_y);            // Publish PWM-2 on MQTT Broker
}

void ultrasonic_task (void *arg) {
    ultrasonic_sensor_t sensor = {
        .trigger_gpio = GPIO_NUM_4,  // Example GPIO for trigger
        .echo_gpio = GPIO_NUM_5       // Example GPIO for echo
    };
    ESP_ERROR_CHECK(ultrasonic_init(&sensor));

    uint32_t time_us;
    while (true) {
        ESP_ERROR_CHECK(ultrasonic_measure_raw(&sensor, PING_TIMEOUT, &time_us));
        float distance_cm = (float)time_us / ROUNDTRIP_CM;
        ESP_LOGI(TAG, "Distance: %.2f cm", distance_cm);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}

void task(void *pvParameters)
{
    ina219_t dev;
    memset(&dev, 0, sizeof(ina219_t));

    assert(CONFIG_EXAMPLE_SHUNT_RESISTOR_MILLI_OHM > 0);
    ESP_ERROR_CHECK(ina219_init_desc(&dev, I2C_ADDR, I2C_PORT, CONFIG_EXAMPLE_I2C_MASTER_SDA, CONFIG_EXAMPLE_I2C_MASTER_SCL));
    ESP_LOGI(TAG, "Initializing INA219");
    ESP_ERROR_CHECK(ina219_init(&dev));

    ESP_LOGI(TAG, "Configuring INA219");
    ESP_ERROR_CHECK(ina219_configure(&dev, INA219_BUS_RANGE_16V, INA219_GAIN_0_125,
            INA219_RES_12BIT_1S, INA219_RES_12BIT_1S, INA219_MODE_CONT_SHUNT_BUS));

    ESP_LOGI(TAG, "Calibrating INA219");

    ESP_ERROR_CHECK(ina219_calibrate(&dev, (float)CONFIG_EXAMPLE_SHUNT_RESISTOR_MILLI_OHM / 1000.0f));

    float bus_voltage, shunt_voltage, current, power;

    ESP_LOGI(TAG, "Starting the loop");
    while (1)
    {
        ESP_ERROR_CHECK(ina219_get_bus_voltage(&dev, &bus_voltage));
        ESP_ERROR_CHECK(ina219_get_shunt_voltage(&dev, &shunt_voltage));
        ESP_ERROR_CHECK(ina219_get_current(&dev, &current));
        ESP_ERROR_CHECK(ina219_get_power(&dev, &power));
        /* Using float in printf() requires non-default configuration in
         * sdkconfig. See sdkconfig.defaults.esp32 and
         * sdkconfig.defaults.esp8266  */
        printf("VBUS: %.04f V, VSHUNT: %.04f mV, IBUS: %.04f mA, PBUS: %.04f mW\n",
                bus_voltage, shunt_voltage * 1000, current * 1000, power * 1000);

        uint8_t channel;
        wifi_band_t band;
        esp_wifi_get_channel(&channel, NULL);
        esp_wifi_get_band(&band);
        printf("Wi-Fi Channel: %d, Band:%d\n", channel, band);
        //ESP_LOGE(TAG, "Wi-Fi Channel: %d, Band: %d", channel, band);
        mqtt_update_battery_voltage(bus_voltage);
        mqtt_update_sys_current(1000.00*current);
        mqtt_update_sys_power(power);

        vTaskDelay(pdMS_TO_TICKS(2500));
    }
}

void app_main(void)
{
    // Initialize NVS before Wi-Fi
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    // MOTORS
    motors_init();

    // Use wifi_init() for ESP-NOW and Wi-Fi setup
    wifi_init();

    // Initialize internal temperature sensor
    chip_sensor_init();
    xTaskCreate(temp_sensor_task, "ESP32C3 Sensor", 2048, NULL, 15, NULL);

    // Initialize LED
    ledc_init();
    int var = 8191;
    gpio_config_t io_conf = {};

    // Configure on-board LED
    gpio_reset_pin(BLINK_GPIO);
    gpio_set_direction(BLINK_GPIO, GPIO_MODE_OUTPUT);
    xTaskCreate(led_task, "LED", 2048, NULL, 15, NULL);

    // Configure on-board push button
    io_conf.intr_type = GPIO_INTR_POSEDGE;
    io_conf.pin_bit_mask = GPIO_INPUT_PIN_SEL;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_up_en = 1;
    gpio_config(&io_conf);
    gpio_set_intr_type(PUSH_BTN_GPIO, GPIO_INTR_NEGEDGE);
    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(gpio_task, "GPIO task", 2048, NULL, 10, NULL);
    gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
    gpio_isr_handler_add(PUSH_BTN_GPIO, gpio_isr_handler, (void*) PUSH_BTN_GPIO);

    // Configure navigation button
    io_conf.intr_type = GPIO_INTR_NEGEDGE;
    io_conf.pin_bit_mask = GPIO_INPUT_PIN_SEL;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_up_en = 1;
    gpio_config(&io_conf);
    gpio_set_intr_type(NAV_BTN, GPIO_INTR_NEGEDGE);
    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(nav_key_task, "NAV Keys task", 2048, NULL, 10, NULL);
    gpio_isr_handler_add(NAV_BTN, gpio_isr_handler, (void*) NAV_BTN);

    configure_button();
    printf("Added button interrupt");

    mqtt_app_start();
    // Initialize buffer with 0s
    buf.x_axis = 0;
    buf.y_axis = 0;
    buf.motor1_rpm_pcm = 0;
    esp_now_init();
    esp_now_register_recv_cb((void*)onDataReceived);

    // ADC
    rc_adc_init();
    xTaskCreate(rc_task, "RC", 2048, NULL, 5, NULL);

    ESP_ERROR_CHECK(i2cdev_init());
    xTaskCreate(task, "test", configMINIMAL_STACK_SIZE * 8, NULL, 5, NULL);
    xTaskCreate(display_xy, "coordinates", configMINIMAL_STACK_SIZE * 8, NULL, 4, NULL);
}

## config.h

#ifndef CONFIG_H
#define CONFIG_H

// MOTORS PWM CONFIG
#define MTR_FREQUENCY               (7000)                  // 1000
#define MTR_MODE                    LEDC_LOW_SPEED_MODE
#define MTR_DUTY_RES                LEDC_TIMER_13_BIT       // 13-bit resolution supports maximum duty value 8192 (8)
// LEFT SIDE MOTORS, FORWARD
#define MTR_FRONT_LEFT_IO           (6)
#define MTR_FRONT_LEFT_TMR          LEDC_TIMER_0
#define MTR_FRONT_LEFT              LEDC_CHANNEL_1
#define MTR_FRONT_LEFT_DUTY         (3361)
// RIGHT SIDE MOTORS, FORWARD
#define MTR_FRONT_RIGHT_IO          (5)
#define MTR_FRONT_RIGHT_TMR         LEDC_TIMER_1
#define MTR_FRONT_RIGHT             LEDC_CHANNEL_0
#define MTR_FRONT_RIGHT_DUTY        (3361)
// LEFT SIDE MOTORS, REVERSE
#define MTR_FRONT_LEFT_REV_IO       (4)
#define MTR_FRONT_LEFT_REV_TMR      LEDC_TIMER_2
#define MTR_FRONT_LEFT_REV          LEDC_CHANNEL_2
#define MTR_FRONT_LEFT_REV_DUTY     (3361)
// RIGHT SIDE MOTORS, REVERSE
#define MTR_FRONT_RIGHT_REV_IO      (7)
#define MTR_FRONT_RIGHT_REV_TMR     LEDC_TIMER_3
#define MTR_FRONT_RIGHT_REV         LEDC_CHANNEL_3
#define MTR_FRONT_RIGHT_REV_DUTY    (3361)


//#define LEDC_DUTY                   (3361) //7820) // 8068, 7944, 7820, 7696, 7572, *7680*, 7424, 7168, 6144, 512, 768
//#define LEDC_FREQUENCY              (2500) //8192) //4000) // For LED the freuqncy of 500Hz seems to be sufficient. // Frequency in Hertz. For DC motor, set frequency at 5 kHz; try 1kHz @ 14 bits resolution


#endif

### mqtt.c

#include "mqtt_client.h"
#include "esp_wifi.h"
#include "esp_log.h"
#include "esp_event.h"

#include "mqtt.h"

//static const char* MQTT_BROKER_URI = "mqtt://10.100.50.16:1883";//74.14.210.168";//"mqtt://mqtt.techquadbit.net";
static const char* MQTT_BROKER_URI = "mqtt://74.14.210.168";//"mqtt://mqtt.techquadbit.net";

static const char* MQTT_TAG = "MQTT_Robot";
static esp_mqtt_client_handle_t mqtt_client = NULL;

static float temp_value = 0.0f;
static float battery_voltage = 0.0f;
static float sys_current = 0.0f;
static float sys_power = 0.0f;
static int pwm_1 = 0, pwm_2 = 0, pwm_3 = 0, pwm_4 = 0;

static void mqtt_publish_task(void *arg) {
    esp_mqtt_client_handle_t client = (esp_mqtt_client_handle_t)arg;
    
    while (1) {
        //float tsens_value = 0.0f;
        //temperature_sensor_get_celsius(temp_sensor, &tsens_value);
        ESP_LOGW("ESP32-C3", "Temperature value %.02f ℃", temp_value);
        char temp_str[6], battery_voltage_str[8], sys_current_str[8], sys_power_str[8];
        char pwm_1_str[5], pwm_2_str[5], pwm_3_str[5], pwm_4_str[5];
        snprintf(temp_str, sizeof(temp_str), "%.02f", temp_value);
        snprintf(battery_voltage_str, sizeof(battery_voltage_str), "%.02f", battery_voltage);
        snprintf(sys_current_str, sizeof(sys_current_str), "%.02f", sys_current);
        snprintf(sys_power_str, sizeof(sys_power_str), "%.02f", sys_power);
        snprintf(pwm_1_str, sizeof(pwm_1_str), "%d", pwm_1);
        snprintf(pwm_2_str, sizeof(pwm_2_str), "%d", pwm_2);
        snprintf(pwm_3_str, sizeof(pwm_3_str), "%d", pwm_3);
        snprintf(pwm_4_str, sizeof(pwm_4_str), "%d", pwm_4);

        // Publish a message every 5 seconds
        esp_mqtt_client_publish(mqtt_client, "/bitrider/temp", temp_str, 0, 1, 0);
        esp_mqtt_client_publish(mqtt_client, "/bitrider/battery_voltage", battery_voltage_str, 0, 1, 0);
        esp_mqtt_client_publish(mqtt_client, "/bitrider/sys_current", sys_current_str, 0, 1, 0);
        esp_mqtt_client_publish(mqtt_client, "/bitrider/sys_power", sys_power_str, 0, 1, 0);
        esp_mqtt_client_publish(mqtt_client, "/bitrider/pwm-1", pwm_1_str, 0, 1, 0);
        esp_mqtt_client_publish(mqtt_client, "/bitrider/pwm-2", pwm_2_str, 0, 1, 0);
        /*esp_mqtt_client_publish(mqtt_client, "/bitrider/pwm-3", pwm_3_str, 0, 1, 0);
        esp_mqtt_client_publish(mqtt_client, "/bitrider/pwm-4", pwm_4_str, 0, 1, 0);*/
        vTaskDelay(pdMS_TO_TICKS(1000));
        ESP_LOGI(MQTT_TAG, "Called task to publish topic /bitrider/temp");
    }
}

void mqtt_update_temp (float temp) { temp_value = temp; }
void mqtt_update_battery_voltage (float voltage) { battery_voltage = voltage; }
void mqtt_update_sys_current (float current) { sys_current = current; }
void mqtt_update_sys_power (float power) { sys_power = power; }
void mqtt_update_pwm_1 (int pwm) { pwm_1 = pwm; }
void mqtt_update_pwm_2 (int pwm) { pwm_2 = pwm; }
void mqtt_update_pwm_3 (int pwm) { pwm_3 = pwm; }
void mqtt_update_pwm_4 (int pwm) { pwm_4 = pwm; }

static void mqtt_event_handler(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data) {
    
    esp_mqtt_event_handle_t event = event_data;
    esp_mqtt_client_handle_t client = event->client;

    switch ((esp_mqtt_event_id_t)event_id) {
        case MQTT_EVENT_CONNECTED:
            ESP_LOGI(MQTT_TAG, "MQTT_EVENT_CONNECTED");
            esp_mqtt_client_publish(client, "/esp/test", "Hello from ESP32-C3!", 0, 1, 0);
            mqtt_client = client;
            xTaskCreate(mqtt_publish_task, "mqtt_publish_task", 8192, NULL, 5, NULL);
            break;
        case MQTT_EVENT_DISCONNECTED:
            ESP_LOGI(MQTT_TAG, "MQTT_EVENT_DISCONNECTED");
            break;
        default:
            break;
    }
}

void mqtt_publish() {
    esp_mqtt_client_handle_t client = esp_mqtt_client_init(NULL);
    esp_mqtt_client_register_event(client, ESP_EVENT_ANY_ID, mqtt_event_handler, NULL);
    
    esp_mqtt_client_start(client);
    
    // Publish a message
    esp_mqtt_client_publish(client, "/esp/test", "Hello from Alex!", 0, 1, 0);
    
    // Stop the client
    esp_mqtt_client_stop(client);
}

void mqtt_app_start(void) {
    esp_mqtt_client_config_t mqtt_cfg = {
        .broker.address.uri = MQTT_BROKER_URI,
    };

    esp_mqtt_client_handle_t client = esp_mqtt_client_init(&mqtt_cfg);
    esp_mqtt_client_register_event(client, ESP_EVENT_ANY_ID, mqtt_event_handler, NULL);
    esp_mqtt_client_start(client);
}

/*void sta_wifi_init(void) {
    esp_netif_init();
    esp_event_loop_create_default();
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    esp_wifi_init(&cfg);

    wifi_config_t wifi_config = {
        .sta = {
            .ssid = WIFI_SSID,
            .password = WIFI_PASSWORD,
        },
    };

    esp_wifi_set_mode(WIFI_MODE_STA);
    esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
    esp_wifi_start();
    esp_wifi_connect();
}*/


## mqtt.h

#ifndef __MQTT_H__
#define __MQTT_H__

#include "mqtt_client.h"
#include "esp_wifi.h"

//static const char WIFI_SSID;
#define WIFI_SSID "IoT_bots"
//static const char WIFI_PASSWORD;
#define WIFI_PASSWORD "208208208"
static const char* MQTT_BROKER_URI;
static const char* MQTT_TAG;
static esp_mqtt_client_handle_t mqtt_client;;

static void mqtt_event_handler(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data);
static void mqtt_publish_task(void *arg);
void mqtt_app_start(void);
void mqtt_publish(void);
static float temp_value, battery_voltage, sys_current, sys_power;
void mqtt_update_temp (float temp);
void mqtt_update_battery_voltage (float voltage);
void mqtt_update_sys_current (float current);
void mqtt_update_sys_power (float power);
void mqtt_update_pwm_1 (int pwm);
void mqtt_update_pwm_2 (int pwm);
void mqtt_update_pwm_3 (int pwm);
void mqtt_update_pwm_4 (int pwm);
void sta_wifi_init(void);

#endif

## rc.h

#ifndef RC_H
#define RC_H

#include "driver/adc.h"
#include "esp_adc_cal.h"
#include "esp_adc/adc_oneshot.h"
#include "esp_adc/adc_continuous.h"
#include "esp_adc/adc_cali.h"
#include "esp_adc/adc_cali_scheme.h"

#include "controls.h"
#include "config.h"

#define ADC_CHNL            ADC_CHANNEL_1
#define ADC_ATTEN           ADC_ATTEN_DB_11
#define ADC1_CHAN0          ADC1_CHANNEL_0
#define ADC1_CHAN1          ADC1_CHANNEL_1

//static const char *TAG = "ESP IDF Robot"
struct motors_rpm m;
static int adc_raw[2][10];
static int voltage[2][10];
static int s = 0, sample = 5, x = 0, y = 0, x_sum = 0, y_sum = 0;

bool do_calibration1_chan0, do_calibration1_chan1;

adc_cali_handle_t adc1_cali_chan0_handle, adc1_cali_chan1_handle;
adc_oneshot_unit_handle_t adc1_handle;
static bool adc_calibration_init(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle);
static void adc_calibration_deinit(adc_cali_handle_t handle);
static int interpolate_raw_val (int raw);
static int rescale_raw_val (int raw);

static esp_err_t rc_adc_init (void) {
    
    adc_oneshot_unit_init_cfg_t init_config1 = {
        .unit_id = ADC_UNIT_1,
    };

    ESP_ERROR_CHECK( adc_oneshot_new_unit(&init_config1, &adc1_handle));

    adc_oneshot_chan_cfg_t config = {
        .bitwidth = SOC_ADC_DIGI_MAX_BITWIDTH,//ADC_BITWIDTH_DEFAULT,
        .atten = ADC_ATTEN,
    };
    ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, ADC1_CHAN0, &config));
    ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, ADC1_CHAN1, &config));

    //-------------ADC1 Calibration Init---------------//    
    adc1_cali_chan0_handle = NULL;
    adc1_cali_chan1_handle = NULL;
    do_calibration1_chan0 = adc_calibration_init(ADC_UNIT_1, ADC1_CHAN0, ADC_ATTEN, &adc1_cali_chan0_handle);
    do_calibration1_chan1 = adc_calibration_init(ADC_UNIT_1, ADC1_CHAN1, ADC_ATTEN, &adc1_cali_chan1_handle);

    return ESP_OK;
}

static int check_motor_pcm(int x) {
    int lim = 7440;
    if (x > lim)
        return 8190;//lim;
    else if (x < -lim)
        return -8190;//-lim;
    else
        return x;
}

// Update PWM based on received values
// IMPORTANT: x and y values correspod to the PWM!
static void update_pwm (int rc_x, int rc_y) {
    
    x = check_motor_pcm(rescale_raw_val(rc_x));
    y = check_motor_pcm(rescale_raw_val(rc_y));
    //ESP_LOGI("x,y", "( %d, %d ) [ %d, %d] ", rc_x, rc_y, x, y);

    /*if (s < sample) {
        x_sum += check_motor_pcm(rescale_raw_val(x));
        y_sum += check_motor_pcm(rescale_raw_val(y));
        s ++;
    }
    else if (s == sample) {
        //x = check_motor_pcm(rescale_raw_val(adc_raw[0][0]));
        //y = check_motor_pcm(rescale_raw_val(adc_raw[0][1]));
        x = x_sum / sample;
        y = y_sum / sample;
        s++;*/

    /*
    
    (+, +0)     (+, +)      (+0, +)
    (-, +)      (0, 0)      (+, -)
    (-, -0)     (-, -)      (-0, -)
    
    if (1024 < x < 2048 && 1024 < y < 2048) {}
    */

    // ADDED ON AUG 6, 2025: to be tested!
    // CONTINOUS UPDATE
    int x_val = x;
    int y_val = y;
    int x_centered = x_val - 2048;
    int y_centered = x_val - 2048;
    // Map joystick to motor direction from y-axis
    int motor_a_dir = y_centered >= 0 ? 1 : -1;
    int motor_b_dir = y_centered >= 0 ? 1 : -1;
    int motor_a_speed = abs(y_centered) * 8192 / 2048;
    int motor_b_speed = abs(y_centered) * 8192 / 2048;
    // Add turning effect from x-axis
    motor_a_speed -= x_centered * 8192 / 2048;
    motor_b_speed += x_centered * 8192 / 2048;
    // Clamp speeds
    if (motor_a_speed < 0) motor_a_speed = 0;
    if (motor_b_speed < 0) motor_b_speed = 0;
    if (motor_a_speed > 8192) motor_a_speed = 8192;
    if (motor_b_speed > 8192) motor_b_speed = 8192;
    //set_motor_direction();
    //set_motor_speed();
    // Pass PWM values to the proper DC motors depending on the joystick y-axis position
    // Forward
    /*if (y_val > y_centered) {
        m.motor1_rpm_pcm = motor_a_speed;
        m.motor2_rpm_pcm = motor_b_speed;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }
    // Reverse
    if (y_val < y_centered) {
        m.motor1_rpm_pcm = motor_a_speed;
        m.motor2_rpm_pcm = motor_b_speed;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }*/


    /*
    // Turn Left
    if (x == 8190 && y == -8190) {
        m.motor1_rpm_pcm = 6172;
        m.motor2_rpm_pcm = 8190;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }
    else if (x == 8190 && y == 8190) {
        m.motor1_rpm_pcm = 8190;
        m.motor2_rpm_pcm = 6172;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }
    else if (x == -8190 && y == -8190) {
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = 6172;
        m.motor4_rpm_pcm = 8190;
    }
    else if (x == -8190 && y == 8190) {
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = 8190;
        m.motor4_rpm_pcm = 6172;
    }*/
    // FORWARD AND REVERSE
    //if ((x > 1500) && (y > 700 && y < 850)) {
    
    else if ((x > 1500) && (y > -2500 && y < 2500)) {
        //ESP_LOGW("ESP-NOW", "FORWARD");
        // Both sides rotate in forward direction.
        m.motor1_rpm_pcm = x;   // left, forward
        m.motor2_rpm_pcm = x;   // right, forward
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }
    //else if ((x < 0) && (y > 700 && y < 850)) {
    else if ((x < 0) && (y > -2500 && y < 2500)) {
        //ESP_LOGW("ESP-NOW", "REVERSE");
        // Both sides rotate in reverse direction.
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = -x;
        m.motor4_rpm_pcm = -x;
    }
    // ROTATE CLOCKWISE AND COUNTER CLOCKWISE
    else if ((x > -2500 && x < 2500) && (y < 0)) {
        //ESP_LOGW("ESP-NOW", "LEFT");
        // Left side rotates in forward direction, right side rotates in reverse direction.
        m.motor1_rpm_pcm = 0;//-y;
        m.motor2_rpm_pcm = -y;//0;
        m.motor3_rpm_pcm = 0;//-y;
        m.motor4_rpm_pcm = -y;//0;
    }
    else if ((x > -2500 && x < 2500) && (y > 900)) {
        //ESP_LOGW("ESP-NOW", "RIGHT");
        // Right side rotates in forward direction, left side rotates in reverse direction.
        m.motor1_rpm_pcm = y;//0;
        m.motor2_rpm_pcm = 0;//y; 
        m.motor3_rpm_pcm = y;//0;
        m.motor4_rpm_pcm = 0;//y; 
    }
    else {
        //ESP_LOGW("ESP-NOW", "STAND STILL");
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }

    ledc_set_duty(MTR_MODE, MTR_FRONT_LEFT, m.motor1_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_LEFT);
    ledc_set_duty(MTR_MODE, MTR_FRONT_RIGHT, m.motor2_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_RIGHT);

    ledc_set_duty(MTR_MODE, MTR_FRONT_LEFT_REV, m.motor3_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_LEFT_REV);
    ledc_set_duty(MTR_MODE, MTR_FRONT_RIGHT_REV, m.motor4_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_RIGHT_REV);

}

static void update_motors_pwm (int pwm_motor_1, int pwm_motor_2) {

    /* UPDATED MOTOR LOGIC */
    if (pwm_motor_1 >= 0 && pwm_motor_2 >= 0) {
        m.motor1_rpm_pcm = pwm_motor_1;
        m.motor2_rpm_pcm = pwm_motor_2;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }
    if (pwm_motor_1 > 0 && pwm_motor_2 < 0) {
        m.motor1_rpm_pcm = pwm_motor_1;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = -pwm_motor_2;
        m.motor4_rpm_pcm = 0;
    }
    if (pwm_motor_1 < 0 && pwm_motor_2 > 0) {
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = -pwm_motor_1;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = pwm_motor_2;
    }
    if (pwm_motor_1 < 0 && pwm_motor_2 < 0) {
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = -pwm_motor_1;
        m.motor4_rpm_pcm = -pwm_motor_2;
    }

    ledc_set_duty(MTR_MODE, MTR_FRONT_LEFT, m.motor1_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_LEFT);
    ledc_set_duty(MTR_MODE, MTR_FRONT_RIGHT, m.motor2_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_RIGHT);

    ledc_set_duty(MTR_MODE, MTR_FRONT_LEFT_REV, m.motor3_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_LEFT_REV);
    ledc_set_duty(MTR_MODE, MTR_FRONT_RIGHT_REV, m.motor4_rpm_pcm);
    ledc_update_duty(MTR_MODE, MTR_FRONT_RIGHT_REV);

    ESP_LOGW("MTR LGC", "M1: %d, M2: %d, M3: %d, M4: %d",
        m.motor1_rpm_pcm,
        m.motor2_rpm_pcm,
        m.motor3_rpm_pcm,
        m.motor4_rpm_pcm);
}

/*static void rc_get_raw_data() {

    ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, ADC1_CHAN0, &adc_raw[0][0]));
    ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, ADC1_CHAN1, &adc_raw[0][1]));
    ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Raw Data: %d", ADC_UNIT_1 + 1, ADC1_CHAN0, adc_raw[0][0]);
    ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Raw Data: %d", ADC_UNIT_1 + 1, ADC1_CHAN1, adc_raw[0][1]);
    ESP_LOGI("Joystick L/R", "Position: %d (%d)", rescale_raw_val(adc_raw[0][0]),  check_motor_pcm(rescale_raw_val(adc_raw[0][0])));
    ESP_LOGI("Joystick F", "Position: %d (%d)", rescale_raw_val(adc_raw[0][1]), check_motor_pcm(rescale_raw_val(adc_raw[0][1])));
    ESP_LOGW("Joystick", " sample %d, (x,y): (%d, %d)", sample, x, y);

    if (s < sample) {
        x_sum += check_motor_pcm(rescale_raw_val(adc_raw[0][0]));
        y_sum += check_motor_pcm(rescale_raw_val(adc_raw[0][1]));
        s ++;
    }
    else if (s == sample) {
        //x = check_motor_pcm(rescale_raw_val(adc_raw[0][0]));
        //y = check_motor_pcm(rescale_raw_val(adc_raw[0][1]));
        x = x_sum / sample;
        y = y_sum / sample;
    //x = buf.x_axis;
    //y = buf.y_axis;
    

    if ((x > 0 && x < 500) && (y > 500)) {
        ESP_LOGW("RC", "FORWARD");
        // Both sides rotate in forward direction.
        m.motor1_rpm_pcm = y;   // left, forward
        m.motor2_rpm_pcm = y;   // right, forward
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }
    else if ((x > 0 && x < 500) && (y < -200)) {
        ESP_LOGW("RC", "REVERSE");
        // Both sides rotate in reverse direction.
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = -y;
        m.motor4_rpm_pcm = -y;
    }
    else if ((y < 0 && y > -200) && (x < -1000)) {
        ESP_LOGW("RC", "LEFT");
        // Left side rotates in forward direction, right side rotates in reverse direction.
        m.motor1_rpm_pcm = -x;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = -x;
        m.motor4_rpm_pcm = 0;
    }
    else if ((y < 0 && y > -200) && (x > 1000)) {
        ESP_LOGW("RC", "RIGHT");
        // Right side rotates in forward direction, left side rotates in reverse direction.
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = x; 
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = x; 
    }
    else {
        ESP_LOGW("RC", "STAND STILL");
        m.motor1_rpm_pcm = 0;
        m.motor2_rpm_pcm = 0;
        m.motor3_rpm_pcm = 0;
        m.motor4_rpm_pcm = 0;
    }
    s++;
    }
    else {
        x_sum = 0;
        y_sum = 0;
        s = 0;
    }

    ESP_LOGI("PWM", "Motor 1 PWM: %d", m.motor1_rpm_pcm);
    ESP_LOGI("PWM", "Motor 2 PWM: %d", m.motor2_rpm_pcm);
    ESP_LOGI("PWM", "Motor 3 PWM: %d", m.motor3_rpm_pcm);
    ESP_LOGI("PWM", "Motor 4 PWM: %d", m.motor4_rpm_pcm);

    if (do_calibration1_chan0) {
        ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_chan0_handle, adc_raw[0][0], &voltage[0][0]));
        ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Cali Voltage: %d mV", ADC_UNIT_1 + 1, ADC1_CHAN0, voltage[0][0]);
    }
    if (do_calibration1_chan1) {
        ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_chan1_handle, adc_raw[0][1], &voltage[0][1]));
        ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Cali Voltage: %d mV", ADC_UNIT_1 + 1, ADC1_CHAN1, voltage[0][1]);
    }
}*/

static bool adc_calibration_init(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle)
{
    adc_cali_handle_t handle = NULL;
    esp_err_t ret = ESP_FAIL;
    bool calibrated = false;

    #if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED
    if (!calibrated) {
        ESP_LOGI("ESP IDF Robot", "calibration scheme version is %s", "Curve Fitting");
        adc_cali_curve_fitting_config_t cali_config = {
            .unit_id = unit,
            .chan = channel,
            .atten = atten,
            .bitwidth = ADC_BITWIDTH_DEFAULT,
        };
        ret = adc_cali_create_scheme_curve_fitting(&cali_config, &handle);
        if (ret == ESP_OK) {
            calibrated = true;
        }
    }
    #endif

    #if ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED
    if (!calibrated) {
        ESP_LOGI("ESP IDF Robot", "calibration scheme version is %s", "Line Fitting");
        adc_cali_line_fitting_config_t cali_config = {
            .unit_id = unit,
            .atten = atten,
            .bitwidth = ADC_BITWIDTH_DEFAULT,
        };
        ret = adc_cali_create_scheme_line_fitting(&cali_config, &handle);
        if (ret == ESP_OK) {
            calibrated = true;
        }
    }
    #endif

    *out_handle = handle;
    if (ret == ESP_OK) {
        ESP_LOGI("ESP IDF Robot", "Calibration Success");
    } else if (ret == ESP_ERR_NOT_SUPPORTED || !calibrated) {
        ESP_LOGW("ESP IDF Robot", "eFuse not burnt, skip software calibration");
    } else {
        ESP_LOGE("ESP IDF Robot", "Invalid arg or no memory");
    }

    return calibrated;
}

static void adc_calibration_deinit(adc_cali_handle_t handle)
{
    #if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED
    ESP_LOGI("ESP IDF Robot", "deregister %s calibration scheme", "Curve Fitting");
    ESP_ERROR_CHECK(adc_cali_delete_scheme_curve_fitting(handle));

    #elif ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED
    ESP_LOGI("ESP IDF Robot", "deregister %s calibration scheme", "Line Fitting");
    ESP_ERROR_CHECK(adc_cali_delete_scheme_line_fitting(handle));
    #endif
}

#endif