Final Project

#include <stdio.h>
#include <string.h>
#include <math.h> 
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c.h"
#include "driver/ledc.h"
#include "esp_timer.h"
#include "esp_err.h"
#include "esp_wifi.h"
#include "esp_now.h"
#include "nvs_flash.h"

#define I2C_MASTER_SCL_IO           23
#define I2C_MASTER_SDA_IO           22
#define I2C_MASTER_NUM              0
#define I2C_MASTER_FREQ_HZ          100000
#define SENSOR_ADDR                 0x68

#define ESC_FL_GPIO                 21
#define ESC_FR_GPIO                 1 
#define ESC_RL_GPIO                 0 
#define ESC_RR_GPIO                 2 

#define LEDC_MODE                   LEDC_LOW_SPEED_MODE
#define LEDC_TIMER                  LEDC_TIMER_0
#define LEDC_DUTY_RES               LEDC_TIMER_14_BIT 
#define LEDC_FREQUENCY              50                

#define MIN_DUTY                    819   
#define MAX_DUTY                    1638  

#define MIN_FLIGHT_THROTTLE         0.01f 
#define MAX_FLIGHT_THROTTLE         0.75f 
#define RAMP_DURATION_SEC           2.0f  

float HOVER_POWER_FL = 0.37f;
float HOVER_POWER_FR = 0.45f;
float HOVER_POWER_RL = 0.32f; 
float HOVER_POWER_RR = 0.33f;

#define RAD_TO_DEG                  57.29577951f
#define ALPHA                       0.98f   
#define GYRO_SCALE                  16.4f   

float Kp_pitch = 0.00036f; 
float Ki_pitch = 0.0000003f;
float Kd_pitch = 0.00018f; // Punto de partida para el amortiguamiento

// Mismos valores para Roll por simetría, aunque estés probando Pitch
float Kp_roll = 0.00250f;
float Ki_roll = 0.0000003f;
float Kd_roll = 0.000008f;

float prev_error_pitch = 0.0f, integral_pitch = 0.0f;
float prev_error_roll = 0.0f,  integral_roll = 0.0f;

float prev_derivative_pitch = 0.0f;
float prev_derivative_roll = 0.0f;
float LPF_BETA = 0.4f; 

typedef struct struct_message {
    float joystick_x;       
    float joystick_y;       
    bool kill_switch_btn;   
} struct_message;

struct_message incoming_data;
volatile bool remote_button_pressed = false;
volatile bool flight_active = false; 

void on_data_recv(const esp_now_recv_info_t * esp_now_info, const uint8_t *data, int data_len) {
    if (data_len == sizeof(incoming_data)) {
        memcpy(&incoming_data, data, sizeof(incoming_data));
        remote_button_pressed = incoming_data.kill_switch_btn;
    }
}

static esp_err_t i2c_master_init(void) {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };
    i2c_param_config(I2C_MASTER_NUM, &conf);
    return i2c_driver_install(I2C_MASTER_NUM, conf.mode, 0, 0, 0);
}

void set_motor_speed(ledc_channel_t channel, float throttle) {
    uint32_t duty = MIN_DUTY + (uint32_t)(throttle * (MAX_DUTY - MIN_DUTY));
    ledc_set_duty(LEDC_MODE, channel, duty);
    ledc_update_duty(LEDC_MODE, channel);
}

float clamp_throttle(float value) {
    if (value < MIN_FLIGHT_THROTTLE) return MIN_FLIGHT_THROTTLE;
    if (value > MAX_FLIGHT_THROTTLE) return MAX_FLIGHT_THROTTLE;
    return value;
}

void app_main(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());
        nvs_flash_init();
    }
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    esp_wifi_init(&cfg);
    esp_wifi_set_mode(WIFI_MODE_STA);
    esp_wifi_start();
    esp_now_init();
    esp_now_register_recv_cb((esp_now_recv_cb_t)on_data_recv);
    i2c_master_init();

    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_MODE, .timer_num = LEDC_TIMER,
        .duty_resolution = LEDC_DUTY_RES, .freq_hz = LEDC_FREQUENCY, .clk_cfg = LEDC_AUTO_CLK
    };
    ledc_timer_config(&ledc_timer);

    int esc_pins[4] = {ESC_FL_GPIO, ESC_FR_GPIO, ESC_RL_GPIO, ESC_RR_GPIO};
    ledc_channel_t esc_channels[4] = {LEDC_CHANNEL_0, LEDC_CHANNEL_1, LEDC_CHANNEL_2, LEDC_CHANNEL_3};

    for (int i = 0; i < 4; i++) {
        ledc_channel_config_t ledc_channel = {
            .speed_mode = LEDC_MODE, .channel = esc_channels[i], .timer_sel = LEDC_TIMER,
            .intr_type = LEDC_INTR_DISABLE, .gpio_num = esc_pins[i], .duty = MIN_DUTY, .hpoint = 0
        };
        ledc_channel_config(&ledc_channel);
    }

    for (int i = 0; i < 4; i++) set_motor_speed(esc_channels[i], 0.0f);
    
    printf("[1/4] H.E.R.M.E.S. online. Waiting 2 seconds.\n");
    vTaskDelay(pdMS_TO_TICKS(2000));
    
    uint8_t cmd_reg = 0x7E;
    uint8_t write_accel[2] = {cmd_reg, 0x11};
    uint8_t write_gyro[2]  = {cmd_reg, 0x15};
    i2c_master_write_to_device(I2C_MASTER_NUM, SENSOR_ADDR, write_accel, 2, 1000 / portTICK_PERIOD_MS);
    vTaskDelay(50 / portTICK_PERIOD_MS);
    i2c_master_write_to_device(I2C_MASTER_NUM, SENSOR_ADDR, write_gyro, 2, 1000 / portTICK_PERIOD_MS);
    vTaskDelay(100 / portTICK_PERIOD_MS);

    printf("[2/4] Preparing IMU offset calibration. KEEP H.E.R.M.E.S. STILL.\n");
    printf("Starting scan in 2 seconds...\n");
    vTaskDelay(pdMS_TO_TICKS(2000)); 
    
    printf("Scanning for 7 seconds (700 samples)... Do not touch.\n");
    float offset_x = 0.0f, offset_y = 0.0f;
    int samples = 700; 
    uint8_t reg = 0x0C, memoria[12];

    for (int i = 0; i < samples; i++) {
        if (i2c_master_write_read_device(I2C_MASTER_NUM, SENSOR_ADDR, ®, 1, memoria, 12, 100 / portTICK_PERIOD_MS) == ESP_OK) {
            int16_t accel_x_raw = (memoria[7] << 8) | memoria[6];
            int16_t accel_y_raw = (memoria[9] << 8) | memoria[8];
            int16_t accel_z_raw = (memoria[11] << 8) | memoria[10];
            offset_x += atan2f(accel_x_raw, accel_z_raw) * RAD_TO_DEG;
            offset_y += atan2f(accel_y_raw, accel_z_raw) * RAD_TO_DEG;
        }
        vTaskDelay(10 / portTICK_PERIOD_MS); 
    }
    offset_x /= samples; offset_y /= samples;
    printf("Calibration complete. Offsets stored.\n");

    printf("[3/4] Sending armed idle pulse to ESCs... Waiting 3 seconds.\n");
    vTaskDelay(pdMS_TO_TICKS(3000)); 
    printf("[4/4] H.E.R.M.E.S. PID CONTROL ACTIVE.\n");
    
    float angle_x = 0.0f, angle_y = 0.0f;
    uint64_t last_time = esp_timer_get_time();
    
    uint64_t last_print_time = esp_timer_get_time();
    const uint64_t PRINT_INTERVAL_US = 250000; 
    
    bool last_btn_state = false;
    float ramp_progress = 0.0f; 
    
    while (1) {
        bool current_btn_state = remote_button_pressed;
        if (current_btn_state && !last_btn_state) {
            flight_active = !flight_active; 
            if (flight_active) {
                ramp_progress = 0.0f; 
                integral_pitch = 0.0f;
                integral_roll = 0.0f;
                prev_derivative_pitch = 0.0f;
                prev_derivative_roll = 0.0f;
            } else {
                printf("\n>>> STOP <<<\n\n");
            }
        }
        last_btn_state = current_btn_state;

        if (i2c_master_write_read_device(I2C_MASTER_NUM, SENSOR_ADDR, ®, 1, memoria, 12, 100 / portTICK_PERIOD_MS) == ESP_OK) {
            
            uint64_t current_time = esp_timer_get_time();
            float dt = (current_time - last_time) / 1000000.0f;
            if (dt <= 0.0f) dt = 0.001f; 
            last_time = current_time;

            int16_t gyro_x_raw = (memoria[1] << 8) | memoria[0];
            int16_t gyro_y_raw = (memoria[3] << 8) | memoria[2];
            int16_t accel_x_raw = (memoria[7] << 8) | memoria[6];
            int16_t accel_y_raw = (memoria[9] << 8) | memoria[8];
            int16_t accel_z_raw = (memoria[11] << 8) | memoria[10];

            float accel_angle_x = atan2f(accel_x_raw, accel_z_raw) * RAD_TO_DEG;
            float accel_angle_y = atan2f(accel_y_raw, accel_z_raw) * RAD_TO_DEG;
            float gyro_rate_x = gyro_x_raw / GYRO_SCALE;
            float gyro_rate_y = gyro_y_raw / GYRO_SCALE;

            angle_x = ALPHA * (angle_x + gyro_rate_x * dt) + (1.0f - ALPHA) * accel_angle_x;
            angle_y = ALPHA * (angle_y + gyro_rate_y * dt) + (1.0f - ALPHA) * accel_angle_y;

            float calibrated_pitch = angle_x - offset_x; 
            float calibrated_roll = angle_y - offset_y;  

            if (current_time - last_print_time >= PRINT_INTERVAL_US) {
                printf("Pitch: %6.2f° | Roll: %6.2f°\n", calibrated_pitch, calibrated_roll);
                last_print_time = current_time;
            }

            if (flight_active) {
                if (ramp_progress < 1.0f) {
                    ramp_progress += (dt / RAMP_DURATION_SEC);
                    if (ramp_progress > 1.0f) ramp_progress = 1.0f;
                }

                float base_FL = MIN_FLIGHT_THROTTLE + ((HOVER_POWER_FL - MIN_FLIGHT_THROTTLE) * ramp_progress);
                float base_FR = MIN_FLIGHT_THROTTLE + ((HOVER_POWER_FR - MIN_FLIGHT_THROTTLE) * ramp_progress);
                float base_RL = MIN_FLIGHT_THROTTLE + ((HOVER_POWER_RL - MIN_FLIGHT_THROTTLE) * ramp_progress);
                float base_RR = MIN_FLIGHT_THROTTLE + ((HOVER_POWER_RR - MIN_FLIGHT_THROTTLE) * ramp_progress);

                float error_pitch = 0.0f - calibrated_pitch;
                float error_roll = 0.0f - calibrated_roll;

                integral_pitch += error_pitch * dt;
                if(integral_pitch > 50.0f) integral_pitch = 50.0f; else if(integral_pitch < -50.0f) integral_pitch = -50.0f;
                
                integral_roll += error_roll * dt;
                if(integral_roll > 50.0f) integral_roll = 50.0f; else if(integral_roll < -50.0f) integral_roll = -50.0f;

                float raw_derivative_pitch = (error_pitch - prev_error_pitch) / dt;
                float raw_derivative_roll = (error_roll - prev_error_roll) / dt;

                float derivative_pitch = (LPF_BETA * raw_derivative_pitch) + ((1.0f - LPF_BETA) * prev_derivative_pitch);
                float derivative_roll = (LPF_BETA * raw_derivative_roll) + ((1.0f - LPF_BETA) * prev_derivative_roll);

                prev_derivative_pitch = derivative_pitch;
                prev_derivative_roll = derivative_roll;
                prev_error_pitch = error_pitch;
                prev_error_roll = error_roll;

                float pid_pitch_output = (Kp_pitch * error_pitch) + (Ki_pitch * integral_pitch) + (Kd_pitch * derivative_pitch);
                float pid_roll_output = (Kp_roll * error_roll) + (Ki_roll * integral_roll) + (Kd_roll * derivative_roll);

                float target_FL = base_FL - pid_pitch_output - pid_roll_output;
                float target_FR = base_FR - pid_pitch_output + pid_roll_output;
                float target_RL = base_RL + pid_pitch_output - pid_roll_output;
                float target_RR = base_RR + pid_pitch_output + pid_roll_output;

                target_FL = clamp_throttle(target_FL);
                target_FR = clamp_throttle(target_FR);
                target_RL = clamp_throttle(target_RL);
                target_RR = clamp_throttle(target_RR);

                set_motor_speed(esc_channels[0], target_FL);
                set_motor_speed(esc_channels[1], target_FR);
                set_motor_speed(esc_channels[2], target_RL);
                set_motor_speed(esc_channels[3], target_RR);

            } else {
                for (int i = 0; i < 4; i++) set_motor_speed(esc_channels[i], 0.0f);
            }
        }
        vTaskDelay(10 / portTICK_PERIOD_MS); 
    }
}

#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_wifi.h"
#include "esp_now.h"
#include "nvs_flash.h"

#define BUTTON_PIN GPIO_NUM_20
#define LED_PIN    GPIO_NUM_15

uint8_t hermes_mac[] = {0x98, 0xA3, 0x16, 0x8D, 0xF3, 0x28};

typedef struct struct_message {
    float joystick_x;       
    float joystick_y;       
    bool kill_switch_btn;   
} struct_message;

struct_message control_data;

void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
    if (status == ESP_NOW_SEND_SUCCESS) {
    } else {
        printf("Delivery failed. Is the drone powered off?\n");
    }
}

void app_main(void) {
    gpio_config_t btn_conf = {
        .pin_bit_mask = (1ULL << BUTTON_PIN),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = 1,    
        .pull_down_en = 0,
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&btn_conf);

    gpio_config_t led_conf = {
        .pin_bit_mask = (1ULL << LED_PIN),
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = 0,
        .pull_down_en = 0,
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&led_conf);
    
    gpio_set_level(LED_PIN, 1);

    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);

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_start());

    ESP_ERROR_CHECK(esp_wifi_set_max_tx_power(80));

    if (esp_now_init() != ESP_OK) {
        printf("Fatal Error: Could not initialize ESP-NOW\n");
        return;
    }
    ESP_ERROR_CHECK(esp_now_register_send_cb((esp_now_send_cb_t)on_data_sent));

    esp_now_peer_info_t peerInfo = {};
    memcpy(peerInfo.peer_addr, hermes_mac, 6);
    peerInfo.channel = 0; 
    peerInfo.encrypt = false;
    
    if (esp_now_add_peer(&peerInfo) != ESP_OK) {
        printf("Error: Could not add H.E.R.M.E.S. to the network\n");
        return;
    }

    printf("======================================\n");
    printf("XIAO REMOTE CONTROL INITIATED\n");
    printf("Searching for drone with MAC: 98:A3:16:8D:F3:28\n");
    printf("======================================\n");

    while (1) {
        int button_state = gpio_get_level(BUTTON_PIN);
        
        control_data.kill_switch_btn = (button_state == 0) ? true : false;
        
        gpio_set_level(LED_PIN, (button_state == 0) ? 0 : 1);

        control_data.joystick_x = 0.0f;
        control_data.joystick_y = 0.0f;

        esp_now_send(hermes_mac, (uint8_t *) &control_data, sizeof(control_data));

        if(control_data.kill_switch_btn) {
            printf("KILL SWITCH PRESSED! Sending emergency signal...\n");
            vTaskDelay(pdMS_TO_TICKS(100)); 
        } else {
            vTaskDelay(pdMS_TO_TICKS(20));
        }
    }
}