#include <Arduino.h>
#include <Adafruit_NeoPixel.h>

#define NEO_PIN     3      
#define NUM_PIXELS  1
#define BRIGHTNESS  150     
#define HUE_STEP    200    
#define FRAME_DELAY 10     

Adafruit_NeoPixel pixel(NUM_PIXELS, NEO_PIN, NEO_GRB + NEO_KHZ800);

void setup() {
  pixel.begin();
  pixel.setBrightness(BRIGHTNESS);
  pixel.clear();
  pixel.show();
}

void loop() {
  for (uint16_t hue = 0; hue < 65536; hue += HUE_STEP) {

    uint32_t color = pixel.gamma32(pixel.ColorHSV(hue));

    pixel.setPixelColor(0, color);
    pixel.show();
    delay(FRAME_DELAY);
  }
}
                                

Explanation

Libraries and Definitions

• #include <Adafruit_NeoPixel.h>: Library that handles the communication protocol of the NeoPixel LED (single-wire digital signal at 800KHz).
• NEO_PIN 3: GPIO pin where the data signal is sent to the LED.
• BRIGHTNESS 150: Global brightness applied to all colors (0-255).
• HUE_STEP 200: Hue increment per frame. It determines how fast the color changes.
• FRAME_DELAY 10: Pause in milliseconds between each LED update.

Setup

• pixel.begin(): Initialized the data pin and prepared the internal NeoPixel driver.
• pixel.setBrightness(150): Scaled all RGB values before sending them without altering the calculated color.
• pixel.clear() + pixel.show(): Turned off the LED at startup by sending zeros to all RGB channels.

Auxiliary Function: Hue to RGB Color

• ColorHSV(hue): Translated a hue value from 0-65535 into a 32-bit RGB color, cycling through the entire color wheel: red to green to blue to red.
• gamma32(color): Corrected the human eye's perception curve so the transition between colors looked visually uniform.

Main Loop

• For loop (0 to 65535, step 200): Cycled through 328 frames at 10ms each, resulting in a complete 3.3-second cycle.
• pixel.setPixelColor(0, color): Wrote the color to the internal buffer without sending it yet.
• pixel.show(): Sent the buffer to the LED via the 800KHz signal, updating the visible color.

#include <Arduino.h>
#include <Adafruit_NeoPixel.h>

#define PIN_NEOPIXEL   PIN_PA3
#define BAT_PIN        PIN_PA7
#define NUM_PIXELS     1

#define R1   100
#define R2   360

#define ADC_REF_MV   3300

#define V_GREEN    3800
#define V_YELLOW   3600
#define V_RED      3450

#define BRIGHTNESS     120
#define READ_EVERY_MS  2000
#define BLINK_MS       350

Adafruit_NeoPixel led(NUM_PIXELS, PIN_NEOPIXEL, NEO_GRB + NEO_KHZ800);

uint32_t lastRead  = 0;
uint32_t lastBlink = 0;
bool     blinkOn   = true;
uint16_t vbat_mV   = 4200;

uint16_t leerBateriaMv();

void setup() {
  led.begin();
  led.setBrightness(BRIGHTNESS);
  led.show();
  analogReference(VDD);
  analogSampleDuration(16);
  vbat_mV = leerBateriaMv();
}

void loop() {
  uint32_t ahora = millis();

  if (ahora - lastRead >= READ_EVERY_MS) {
    lastRead = ahora;
    vbat_mV = leerBateriaMv();
  }

  if (vbat_mV >= V_GREEN) {
    led.setPixelColor(0, led.Color(0, 255, 0));
    led.show();
  }
  else if (vbat_mV >= V_YELLOW) {
    led.setPixelColor(0, led.Color(255, 180, 0));
    led.show();
  }
  else if (vbat_mV >= V_RED) {
    led.setPixelColor(0, led.Color(255, 0, 0));
    led.show();
  }
  else {
    if (ahora - lastBlink >= BLINK_MS) {
      lastBlink = ahora;
      blinkOn = !blinkOn;
      led.setPixelColor(0, blinkOn ? led.Color(255, 0, 0) : 0);
      led.show();
    }
  }

  delay(20);
}

uint16_t leerBateriaMv() {
  analogRead(BAT_PIN);
  uint16_t suma = 0;
  for (uint8_t i = 0; i < 8; i++) {
    suma += analogRead(BAT_PIN);
    delay(2);
  }
  uint16_t raw   = suma / 8;
  uint32_t vnode = (uint32_t)raw * ADC_REF_MV / 1023;
  uint32_t vbat  = vnode * (R1 + R2) / R2;
  return (uint16_t)vbat;
}
                                

Explanation

Libraries and Definitions

• #include <Adafruit_NeoPixel.h>: Library that handled the NeoPixel LED communication protocol.
• PIN_PA3: NeoPixel data pin.
• PIN_PA7: Analog pin connected to the voltage divider node.
• R1 = 100, R2 = 360: Values in kΩ of the resistive divider. With this proportion, the maximum 4.2V of the LiPo battery dropped to 3.28V at the node, falling right within the ADC range.
• ADC_REF_MV 3300: ADC reference voltage in millivolts (VDD = 3.3V).

Battery Thresholds

• V_GREEN 3800: Above 3.80V, the LED turned green, meaning the battery was full.
• V_YELLOW 3600: Above 3.60V, the LED turned yellow, indicating half charge.
• V_RED 3450: Above 3.45V, the LED turned solid red, indicating 20% capacity.
• Below 3.45V, the LED blinked red, indicating a critical level.

Setup

• led.begin() and led.show(): Initialized the LED and turned it off at startup.
• analogReference(VDD): Set VDD (3.3V) as the ADC reference, which was required in megaTinyCore for accurate readings.
• analogSampleDuration(16): Increased the ADC sampling time. This was necessary because the 100kΩ/360kΩ divider had high impedance, and the ADC needed more time to stabilize before reading.

Auxiliary Function: leerBateriaMv()

• Discarded reading: The first call to analogRead() was ignored to stabilize the internal ADC multiplexer before taking real readings.
• 8-reading average: Reduced ADC noise by accumulating 8 samples and dividing by 8.
• Conversion to millivolts: Used uint32_t for intermediate arithmetic to prevent overflow, since raw x 3300 exceeded the limit of a 16-bit integer.
  • vnode: Voltage at the divider node in mV.
  • vbat: Actual battery voltage, reconstructed by applying the inverse proportion of the divider: vbat = vnode x (R1 + R2) / R2.

Main Loop

• Periodic reading: The voltage was measured every 2 seconds using millis() to avoid blocking the program while the LED blinked.
• Green, yellow, and solid red: The color was updated directly with led.setPixelColor() and led.show() depending on the corresponding threshold.
• Critical blinking: When the voltage dropped below V_RED, the LED alternated between red and off every 350ms using millis(), avoiding the use of delay() so the periodic battery reading wouldn't be interrupted.
• delay(20): A minimum pause at the end of the loop to stabilize the cycle without affecting the blinking response.

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <WiFi.h>               
#include <esp_now.h>            
#include "driver/gpio.h"       

#define Flex_1_Pin  2
#define Flex_2_Pin  3
#define Flex_3_Pin  4
#define Button_Pin  GPIO_NUM_8

#define FLEX_1_MIN 1000
#define FLEX_1_MAX 1900
#define FLEX_2_MIN 650
#define FLEX_2_MAX 1800
#define FLEX_3_MIN 350
#define FLEX_3_MAX 1900

#define NUM_SAMPLES   9
#define MIN_VALID     200
#define SMOOTH_FACTOR 0.3

#define ALPHA       0.98
#define CALIB_COUNT 500

#define DEBOUNCE_MS 250

uint8_t Receiver_Address[] = {0x58, 0xE6, 0xC5, 0x10, 0x43, 0xCC};

typedef struct Struct_Message {
  int Flex1;
  int Flex2;
  int Flex3;
  int Pitch;
  int Roll;
  int Yaw;
} Struct_Message;

Struct_Message Out_Going_Data;

Adafruit_MPU6050 mpu;

float EMA_Value[3]  = {FLEX_1_MIN, FLEX_2_MIN, FLEX_3_MIN};
int   Last_Valid[3] = {FLEX_1_MIN, FLEX_2_MIN, FLEX_3_MIN};

float Pitch = 0, Roll = 0, Yaw = 0;
float Gyro_Bias_X = 0, Gyro_Bias_Y = 0, Gyro_Bias_Z = 0;
unsigned long Last_Time = 0;
unsigned long Sample_Count = 0;

volatile bool Button_Flag        = false;
unsigned long Last_Debounce      = 0;
int           Button_Press_Count = 0;
bool          Paused             = false;

float Last_Angle1 = 0, Last_Angle2 = 0, Last_Angle3 = 0;
float Last_Pitch  = 0, Last_Roll   = 0, Last_Yaw    = 0;

void Sort_Array(int *arr, int n) {
  for (int i = 0; i < n - 1; i++)
    for (int j = 0; j < n - i - 1; j++)
      if (arr[j] > arr[j + 1]) {
        int tmp = arr[j]; arr[j] = arr[j + 1]; arr[j + 1] = tmp;
      }
}

int Median_Filter(int pin, int index) {
  int samples[NUM_SAMPLES];
  int valid = 0;
  for (int i = 0; i < NUM_SAMPLES; i++) {
    int val = analogRead(pin);
    delay(2);
    if (val >= MIN_VALID) samples[valid++] = val;
  }
  if (valid == 0) return Last_Valid[index];
  Sort_Array(samples, valid);
  Last_Valid[index] = samples[valid / 2];
  return Last_Valid[index];
}

float EMA_Filter(int median, int index) {
  EMA_Value[index] = EMA_Value[index] * (1 - SMOOTH_FACTOR) + median * SMOOTH_FACTOR;
  return EMA_Value[index];
}

float Read_Flex_Filtered(int pin, int index) {
  return EMA_Filter(Median_Filter(pin, index), index);
}

float To_Angle(float raw, int raw_min, int raw_max) {
  float n = (raw - raw_min) / (float)(raw_max - raw_min);
  n = constrain(n, 0.0, 1.0);
  return n * 135.0;
}

void Calibrate_Gyro() {
  Serial.println(">> Calibrando giroscopio, NO MUEVAS el sensor...");
  float sx = 0, sy = 0, sz = 0;
  sensors_event_t a, g, t;
  for (int i = 0; i < CALIB_COUNT; i++) {
    mpu.getEvent(&a, &g, &t);
    sx += g.gyro.x;
    sy += g.gyro.y;
    sz += g.gyro.z;
    delay(2);
  }
  Gyro_Bias_X = sx / CALIB_COUNT;
  Gyro_Bias_Y = sy / CALIB_COUNT;
  Gyro_Bias_Z = sz / CALIB_COUNT;
  Serial.printf("Bias -> X:%.4f  Y:%.4f  Z:%.4f\n",
                Gyro_Bias_X, Gyro_Bias_Y, Gyro_Bias_Z);
}

void Update_Orientation() {
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  unsigned long now = micros();
  float dt = (now - Last_Time) / 1000000.0;
  Last_Time = now;

  float gx = (gyro.gyro.x - Gyro_Bias_X) * 180.0 / PI;
  float gy = (gyro.gyro.y - Gyro_Bias_Y) * 180.0 / PI;
  float gz = (gyro.gyro.z - Gyro_Bias_Z) * 180.0 / PI;

  float acc_pitch = atan2(accel.acceleration.y,
  sqrt(accel.acceleration.x * accel.acceleration.x +
  accel.acceleration.z * accel.acceleration.z)) * 180.0 / PI;
  float acc_roll  = atan2(-accel.acceleration.x,
  accel.acceleration.z) * 180.0 / PI;

  Pitch = ALPHA * (Pitch + gx * dt) + (1 - ALPHA) * acc_pitch;
  Roll  = ALPHA * (Roll  + gy * dt) + (1 - ALPHA) * acc_roll;
  Yaw  += gz * dt;
}

void IRAM_ATTR Button_ISR() {
  unsigned long now = millis();
  if (now - Last_Debounce > DEBOUNCE_MS) {
    Button_Flag   = true;
    Last_Debounce = now;
  }
}

void Setup_Button() {
  gpio_config_t config = {
    .pin_bit_mask = (1ULL << Button_Pin),
    .mode         = GPIO_MODE_INPUT,
    .pull_up_en   = GPIO_PULLUP_DISABLE,
    .pull_down_en = GPIO_PULLDOWN_ENABLE,
    .intr_type    = GPIO_INTR_DISABLE
  };
  gpio_config(&config);

  attachInterrupt(8, Button_ISR, RISING);
}

void Check_Button() {
  if (Button_Flag) {
    Button_Flag = false;
    Paused = !Paused;
    Button_Press_Count++;

    if (Paused) {
      Last_Angle1 = To_Angle(EMA_Value[0], FLEX_1_MIN, FLEX_1_MAX);
      Last_Angle2 = To_Angle(EMA_Value[1], FLEX_2_MIN, FLEX_2_MAX);
      Last_Angle3 = To_Angle(EMA_Value[2], FLEX_3_MIN, FLEX_3_MAX);
      Last_Pitch  = Pitch;
      Last_Roll   = Roll;
      Last_Yaw    = Yaw;
      Serial.println("\n  [[ PAUSADO — Servos congelados ]]");
    } else {
      Serial.println("\n  [[ REANUDADO ]]");
    }
  }
}

void On_Data_Sent(const wifi_tx_info_t* info, esp_now_send_status_t status) {
}

void Setup_ESPNOW() {
  WiFi.mode(WIFI_STA);

  WiFi.setTxPower(WIFI_POWER_2dBm);

  if (esp_now_init() != ESP_OK) {
    Serial.println("Error al iniciar ESP-NOW");
    return;
  }

  esp_now_register_send_cb(On_Data_Sent);

  esp_now_peer_info_t peerInfo = {};
  memcpy(peerInfo.peer_addr, Receiver_Address, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Error al agregar el receptor");
    return;
  }

  Serial.println("ESP-NOW listo.");
}

void Send_Data() {
  Out_Going_Data.Flex1 = (int)round(Last_Angle1);
  Out_Going_Data.Flex2 = (int)round(Last_Angle2);
  Out_Going_Data.Flex3 = (int)round(Last_Angle3);
  Out_Going_Data.Pitch = (int)round(Last_Pitch);
  Out_Going_Data.Roll  = (int)round(Last_Roll);
  Out_Going_Data.Yaw   = (int)round(Last_Yaw);

  esp_now_send(Receiver_Address, (uint8_t *) &Out_Going_Data,
               sizeof(Out_Going_Data));
}

void Print_Data(float a1, float a2, float a3,
                float pitch, float roll, float yaw) {
  Sample_Count++;
  String estado = Paused ? "[ PAUSADO ]" : "[ ACTIVO  ]";

  Serial.printf("\n--- Muestra #%lu | %lu ms | %s | Boton: %dx ---\n",
                Sample_Count, millis(), estado.c_str(), Button_Press_Count);

  Serial.println("  Flex (angulo):");
  Serial.printf("    Sensor 1: %.1f°\n", a1);
  Serial.printf("    Sensor 2: %.1f°\n", a2);
  Serial.printf("    Sensor 3: %.1f°\n", a3);

  Serial.println("  Orientacion (grados):");
  Serial.printf("    Pitch: %+.1f°\n", pitch);
  Serial.printf("    Roll : %+.1f°\n", roll);
  Serial.printf("    Yaw  : %+.1f°\n", yaw);
}

void setup() {
  Serial.begin(115200);
  delay(1000);          

  Setup_Button();       
  Setup_ESPNOW();       

  Wire.begin();

  if (!mpu.begin()) {
    Serial.println("ERROR: MPU6050 no detectado.");
    while (1) delay(10);
  }

  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(100);
  Calibrate_Gyro();
  Last_Time = micros();
  Serial.println("Sistema listo. Presiona el boton para pausar/reanudar.");
}

void loop() {
  Check_Button();

  if (!Paused) {
    float raw1 = Read_Flex_Filtered(Flex_1_Pin, 0); delay(5);
    float raw2 = Read_Flex_Filtered(Flex_2_Pin, 1); delay(5);
    float raw3 = Read_Flex_Filtered(Flex_3_Pin, 2); delay(5);

    Last_Angle1 = To_Angle(raw1, FLEX_1_MIN, FLEX_1_MAX);
    Last_Angle2 = To_Angle(raw2, FLEX_2_MIN, FLEX_2_MAX);
    Last_Angle3 = To_Angle(raw3, FLEX_3_MIN, FLEX_3_MAX);

    Update_Orientation();
    Last_Pitch = Pitch;
    Last_Roll  = Roll;
    Last_Yaw   = Yaw;
  }

  Send_Data();          

  Print_Data(Last_Angle1, Last_Angle2, Last_Angle3,
             Last_Pitch, Last_Roll, Last_Yaw);

  delay(50);
}
                                

Explanation

Libraries

• #include <Wire.h>: Handled I2C communication with the MPU6050 sensor.
• #include <Adafruit_MPU6050.h> and #include <Adafruit_Sensor.h>: Adafruit libraries that abstracted the IMU reading and delivered the data in standard physical units (m/s² and rad/s).
• #include <WiFi.h> and #include <esp_now.h>: Enabled ESP-NOW communication over the Wi-Fi layer.
• #include "driver/gpio.h": Low-level ESP-IDF API, necessary for configuring the button pin, which was a strapping pin and didn't behave correctly with standard Arduino functions.

Pin Definitions

• Flex_1_Pin 2, Flex_2_Pin 3, Flex_3_Pin 4: Analog GPIOs connected to each flex sensor.
• Button_Pin GPIO_NUM_8: Button pin. The gpio_num_t constant from the ESP-IDF was used instead of the simple number because GPIO 8 is a strapping pin on the ESP32-C3.

Flex Sensors Calibration

• FLEX_X_MIN and FLEX_X_MAX: Defined the raw reading range (ADC) of each sensor between an extended finger and a flexed finger. These values were determined experimentally and varied per sensor due to manufacturing and assembly differences.

Filtering Parameters

• NUM_SAMPLES 9: Number of samples taken by the median filter for each reading.
• MIN_VALID 200: Minimum threshold to consider a sample valid. Readings below this were discarded as noise or ADC crosstalk.
• SMOOTH_FACTOR 0.3: Smoothing factor for the EMA filter. It determined how much weight each new reading had compared to the historical data (30% new, 70% accumulated).

IMU Parameters

• ALPHA 0.98: Complementary filter coefficient. 98% of the angle came from the gyroscope's integration, and 2% from the accelerometer.
• CALIB_COUNT 500: Number of samples averaged to calculate the gyroscope's bias (resting drift) at startup.
• DEBOUNCE_MS 250: Button debounce time in milliseconds, ensuring a single press wasn't registered multiple times.

Data Structure and Addressing

• Receiver_Address[]: Receiver's MAC address, to which the packets were sent. This had to match the MAC address the receiver printed at startup.
• Struct_Message: Defined the packet sent with six integers (Flex1-3, Pitch, Roll, Yaw). Its definition had to be identical in both the transmitter and receiver for proper deserialization.

Median Filter: Sort_Array() and Median_Filter()

• Sort_Array(): Implemented a simple bubble sort, which was suitable given the small number of samples.
• Median_Filter(): Took 9 ADC readings, discarded invalid ones (below MIN_VALID), sorted the valid ones, and returned the center value. The median was robust against spurious spikes: an isolated outlier wouldn't affect the result, unlike an average. If there were no valid readings, it returned the last known good value.

EMA Filter: EMA_Filter()

The exponential moving average filter smoothed the signal already processed by the median filter. Its formula was:

• EMA = EMA x (1 - 0.3) + median x 0.3
• This equated to keeping 70% of the previous value and adding 30% of the new reading, producing a smooth transition that eliminated residual jitter without introducing noticeable delay.
• Read_Flex_Filtered() Chained both filters: first median, then EMA.

Angle Conversion: To_Angle()

• Normalized the raw reading within its calibrated range: n = (raw - min) / (max - min), yielding a value between 0 and 1.
• constrain(n, 0.0, 1.0) Prevented readings outside the calibrated range from producing negative angles or exceeding the maximum.
• Multiplied by 135.0 to map the result to the usable 0° to 135° travel range expected by the receiver's servos.

Gyroscope Calibration: Calibrate_Gyro()

• Averaged 500 gyroscope readings while the sensor remained still to measure its resting drift (bias) on each axis.
• This bias was later subtracted from each reading. Without this correction, the gyroscope would continuously accumulate error, causing the angles to drift even without movement.

Complementary Filter: Update_Orientation()

This function fused the gyroscope and accelerometer to obtain a stable orientation. Each sensor by itself was deficient: the gyroscope was precise in the short term but drifted over time, while the accelerometer was stable in the long term but noisy during movement.

• dt: Calculated the actual elapsed time using micros(), which was necessary to correctly integrate the gyroscope's angular velocity.
• Gyroscope conversion: Subtracted the bias and converted from rad/s to °/s by multiplying by 180/PI.
• Accelerometer angles: Used atan2() on the acceleration components to calculate pitch and roll based on gravity's direction.
• Fusion: Pitch = 0.98 x (Pitch + gyro x dt) + 0.02 x acc_pitch. The gyroscope term provided a quick response, and the accelerometer slowly corrected the drift.
• Yaw: Was only integrated from the gyroscope (Yaw += gz x dt). Without a magnetometer, there was no absolute reference to correct it, so it drifted over time—which was why the receiver used Pitch and not Yaw for the base.

Button Handling (Strapping Pin)

• Button_ISR(): Interrupt routine marked with IRAM_ATTR so it ran from internal RAM (faster). It applied time-based debounce and only raised a flag, keeping the interrupt as brief as possible.
• Setup_Button(): Configured GPIO 8 using the low-level gpio_config() with GPIO_PULLDOWN_ENABLE. This was necessary because GPIO 8 was a strapping pin on the ESP32-C3, and standard Arduino functions didn't properly configure its internal resistance. The interrupt was triggered on a rising edge (RISING).
• Check_Button(): Toggled the pause state when detecting the flag. When paused, it froze the last flex and orientation values so the receiver held its position; upon resuming, it picked up live readings again.

ESP-NOW Configuration: Setup_ESPNOW()

• WiFi.mode(WIFI_STA): Set the Wi-Fi to station mode, required for ESP-NOW.
• WiFi.setTxPower(WIFI_POWER_2dBm): Deliberately reduced the transmission power. High power caused current spikes leading to battery voltage drops; lowering it stabilized the power supply at the expense of some range.
• Registered the send callback and added the receiver as a peer with the defined MAC address, without encryption and on channel 0.

Sending and Monitoring

• Send_Data(): Rounded floating values to integers, placed them in the structure, and transmitted them using esp_now_send().
• Print_Data(): Printed the complete system state via serial (angles, orientation, sample number, pause state, and press count) for debugging purposes.

Setup

• Initialized the button and ESP-NOW, started the I2C bus, and verified the presence of the MPU6050; if it didn't respond, the program halted in an infinite loop.
• Configured the IMU ranges: gyroscope to ±500°/s, accelerometer to ±8g, and the internal low-pass filter to 21Hz.
• Executed the gyroscope calibration and logged the initial time to calculate dt.

Main Loop

• Button: Check_Button() checked at the beginning of each cycle if there was a pending pause change.
• Active reading: If not paused, it read and filtered the three flex sensors, converted them to angles, and updated the IMU orientation.
• Continuous sending: Send_Data() transmitted on every iteration, even during a pause, keeping the receiver fed with the latest frozen values.
• delay(50): Set the transmission frequency at approximately 20Hz, which was enough for fluid control without saturating the channel.

#include <Arduino.h>
#include <WiFi.h>
#include <esp_now.h>
#include <Preferences.h>

const int PINES[4]      = {17, 19, 20, 18};     
const int ANGULO_MAX[4] = {270, 270, 270, 180};

const float SERVO_HOME[4] = {220, 135, 135, 90};
const int   SERVO_DIR[4]  = {1, -1, 1, 1};      
const int   LIM_MIN[4]    = {0,   0,   0,   0};
const int   LIM_MAX[4]    = {270, 270, 270, 180};

#define ENGAGE_THRESHOLD 8.0

const int  FREQ      = 50;
const int  RES_BITS  = 16;
const long PERIODO   = 20000;
const long MAX_DUTY  = (1 << RES_BITS) - 1;
const int  PULSO_MIN    = 500;
const int  PULSO_MAX270 = 2500;
const int  PULSO_MAX180 = 2400;

#define STEP_PIN    16
#define DIR_PIN     23
#define FLT_PIN     22

#define STEPS_PER_REV   200
#define MICROSTEPPING   16     
#define GEAR_RATIO      1.0

#define STEP_INTERVAL_US 75  
#define STEP_PULSE_US    3     

#define SERVO_SPEED     70.0
#define UPDATE_INTERVAL 20

const float STEPS_PER_DEGREE = 
(STEPS_PER_REV * MICROSTEPPING * GEAR_RATIO) / 360.0;
const char* SERVO_NAMES[4] = 
{"Roll (S1)", "Flex1 (S2)", "Flex2 (S3)", "Flex3 (S4)"};

float servo_zero[4]    = {220, 135, 135, 90};
float servo_current[4] = {220, 135, 135, 90};
float servo_target[4]  = {220, 135, 135, 90};
bool  servo_engaged[4] = {false, false, false, false};

long Stepper_Pos = 0;
long Stepper_Target = 0;
unsigned long Last_Step_Us = 0;

Preferences prefs;
bool Calibrating = false;
int  Cal_Index   = 0;

typedef struct Struct_Message {
  int Flex1, Flex2, Flex3, Pitch, Roll, Yaw;
} Struct_Message;

Struct_Message Incoming;
volatile bool New_Data = false;

long microsADuty(int us) {
  us = constrain(us, 500, 2500);
  return (long)us * MAX_DUTY / PERIODO;
}

int gradosAMicros(int idx, float grados) {
  int pulsoMax = (idx == 3) ? PULSO_MAX180 : PULSO_MAX270;
  grados = constrain(grados, 0, ANGULO_MAX[idx]);
  return map((long)(grados * 10), 0, (long)ANGULO_MAX[idx] 
  * 10, PULSO_MIN, pulsoMax);
}

void escribirServo(int idx, float grados) {
  ledcWrite(PINES[idx], microsADuty(gradosAMicros(idx, grados)));
}

void Update_Servos() {
  static unsigned long last = 0;
  unsigned long now = millis();
  if (now - last < UPDATE_INTERVAL) return;
  float dt = (now - last) / 1000.0;
  last = now;
  float max_step = SERVO_SPEED * dt;

  for (int i = 0; i < 4; i++) {
    float diff = servo_target[i] - servo_current[i];
    if (fabs(diff) <= max_step) servo_current[i] = servo_target[i];
    else servo_current[i] += (diff > 0 ? max_step : -max_step);
    escribirServo(i, servo_current[i]);
  }
}

void Stepper_Setup() {
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(FLT_PIN, INPUT_PULLUP);

  digitalWrite(DIR_PIN, HIGH);
  digitalWrite(STEP_PIN, LOW);
  delay(100);
}

void Stepper_Update() {
  if (Stepper_Pos == Stepper_Target) return;   
  unsigned long now = micros();
  if (now - Last_Step_Us < STEP_INTERVAL_US) return;
  Last_Step_Us = now;

  bool horario = (Stepper_Target > Stepper_Pos);
  digitalWrite(DIR_PIN, horario ? HIGH : LOW);
  delayMicroseconds(3);   

  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(STEP_PULSE_US);
  digitalWrite(STEP_PIN, LOW);

  Stepper_Pos += horario ? 1 : -1;
}

void Save_Calibration() {
  prefs.begin("arm", false);
  for (int i = 0; i < 4; i++)
    prefs.putFloat(("z" + String(i)).c_str(), servo_zero[i]);
  prefs.end();
}

void Load_Calibration() {
  prefs.begin("arm", true);
  for (int i = 0; i < 4; i++)
    servo_zero[i] = prefs.getFloat(("z" + String(i)).c_str(), servo_zero[i]);
  prefs.end();
}

void Print_Servo_Prompt() {
  Serial.printf("\n>> Calibrando %s (reposo). Comandos:\n", SERVO_NAMES[Cal_Index]);
  Serial.println("   numero / +N / -N   |   ok = guardar y siguiente");
}

void Print_Stepper_Prompt() {
  Serial.println("\n>> Calibrando BASE (NEMA). \
  +N / -N pasos  |  ok = fijar cero y terminar");
}

void Handle_Calibration(String cmd) {
  cmd.trim();

  if (Cal_Index < 4) {
    if (cmd == "ok") {
      servo_zero[Cal_Index] = servo_current[Cal_Index];
      Serial.printf("Reposo de %s = %.0f°\n", SERVO_NAMES[Cal_Index], 
      servo_zero[Cal_Index]);
      Cal_Index++;
      (Cal_Index < 4) ? Print_Servo_Prompt() : Print_Stepper_Prompt();
      return;
    }
    float val = (cmd.startsWith("+") || cmd.startsWith("-"))
    ? servo_current[Cal_Index] + cmd.toFloat()
    : cmd.toFloat();
    val = constrain(val, LIM_MIN[Cal_Index], LIM_MAX[Cal_Index]);
    servo_current[Cal_Index] = val;
    servo_target[Cal_Index]  = val;
    escribirServo(Cal_Index, val);
    Serial.printf("%s -> %.0f°\n", SERVO_NAMES[Cal_Index], val);

  } else {
    if (cmd == "ok") {
      Stepper_Pos = 0;
      Stepper_Target = 0;
      Save_Calibration();
      Calibrating = false;
      for (int i = 0; i < 4; i++) servo_engaged[i] = false;
      Serial.println("Cero de la base fijado.");
      Serial.println("== Calibracion completa y guardada ==\n");
      return;
    }
    Stepper_Target = (cmd.startsWith("+") || cmd.startsWith("-"))
    ? Stepper_Target + cmd.toInt()
    : cmd.toInt();
    Serial.printf("Base -> %ld pasos\n", Stepper_Target);
  }
}

String Serial_Line() {
  static String buf = "";
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (buf.length() > 0) { String r = buf; buf = ""; return r; }
    } else buf += c;
  }
  return "";
}

void On_Data_Received(const esp_now_recv_info_t* info,
                      const uint8_t* data, int dataLen) {
  memcpy(&Incoming, data, sizeof(Incoming));
  New_Data = true;
}

void Apply_Targets() {
  int valores[4] = {Incoming.Roll, Incoming.Flex1, Incoming.Flex2, Incoming.Flex3};

  for (int i = 0; i < 4; i++) {
    float raw = constrain(servo_zero[i] + SERVO_DIR[i] * valores[i],
    LIM_MIN[i], LIM_MAX[i]);
    if (!servo_engaged[i] && fabs(raw - servo_current[i]) <= ENGAGE_THRESHOLD) {
      servo_engaged[i] = true;
      Serial.printf(">> %s enganchado\n", SERVO_NAMES[i]);
    }
    servo_target[i] = servo_engaged[i] ? raw : servo_zero[i];
  }

  float pitch = constrain((float)Incoming.Pitch, -90, 90);
  float base_deg = (pitch + 90.0) * (270.0 / 180.0);
  long nueva_meta = (long)(base_deg * STEPS_PER_DEGREE);

  if (abs(nueva_meta - Stepper_Target) > 2) {
    Stepper_Target = nueva_meta;
  }
}

void setup() {
  Serial.begin(115200);
  delay(500);

  for (int i = 0; i < 4; i++) ledcAttach(PINES[i], FREQ, RES_BITS);

  Stepper_Setup();

  Load_Calibration();
  for (int i = 0; i < 4; i++) {
    servo_current[i] = servo_zero[i];
    servo_target[i]  = servo_zero[i];
    servo_engaged[i] = false;
    escribirServo(i, servo_zero[i]);
  }

  WiFi.mode(WIFI_STA);
  Serial.print("MAC Address de este receptor: ");
  Serial.println(WiFi.macAddress());

  if (esp_now_init() != ESP_OK) {
    Serial.println("Error al iniciar ESP-NOW");
    return;
  }
  esp_now_register_recv_cb(On_Data_Received);

  Serial.println("Receptor listo (servos en reposo).");
  Serial.println("Escribe 'c' + Enter para calibrar.\n");
}

void loop() {
  String cmd = Serial_Line();
  if (cmd.length() > 0) {
    if (!Calibrating && cmd == "c") {
      Calibrating = true;
      Cal_Index = 0;
      Serial.println("\n=== MODO CALIBRACION ===");
      Print_Servo_Prompt();
    } else if (Calibrating) {
      Handle_Calibration(cmd);
    }
  }

  if (!Calibrating && New_Data) {
    New_Data = false;
    Apply_Targets();
  }

  if (!Calibrating) Update_Servos();

  if (digitalRead(FLT_PIN) == HIGH) {
    Stepper_Update();
  } else {
    static unsigned long last_fault = 0;
    if (millis() - last_fault > 2000) {
      Serial.println("!! FALLA en DRV8825 - revisa corriente/temperatura");
      last_fault = millis();
    }
  }
}
                                

Explanation

Libraries

• #include <WiFi.h>: Necessary because ESP-NOW operated over the physical Wi-Fi layer of the ESP32, even though it didn't connect to any network.
• #include <esp_now.h>: Low-latency peer-to-peer communication protocol between two ESP32s without an intermediate router.
• #include <Preferences.h>: Saved the calibration in a non-volatile flash memory partition (NVS), so it survived reboots and power outages.

Servo Configuration

• PINES[4] = {17, 19, 20, 18}: GPIOs for each servo.
• ANGULO_MAX[4] = {270, 270, 270, 180}: Maximum physical range for each servo. The first three were 270° and the fourth was 180°.
• SERVO_HOME[4] = {220, 135, 135, 90}: Factory resting position for each servo, generally the midpoint of its range.
• SERVO_DIR[4] = {1, -1, 1, 1}: Rotation direction of each servo. A value of -1 inverted the direction via software, preventing the need to reverse wiring or mechanical assembly.
• LIM_MIN[4] and LIM_MAX[4]: Software limits that restricted the allowed travel of each servo, protecting the mechanical structure from collisions.
• ENGAGE_THRESHOLD 8.0: Degree threshold for the safety mechanism. The servo remained idle until the received value matched its current position within ±8°, avoiding sudden jumps when powering the system.

PWM Parameters (LEDC)

• FREQ = 50: The servomotors required a 50Hz signal, equating to a 20ms period.
• RES_BITS = 16: 16-bit resolution for the PWM, yielding 65536 levels (0 to 65535) for very fine position control.
• PERIODO = 20000: The 20ms period expressed in microseconds.
• MAX_DUTY = (1 << 16) - 1 = 65535: Maximum duty cycle value, calculated via bit shifting.
• PULSO_MIN = 500, PULSO_MAX270 = 2500, PULSO_MAX180 = 2400: Pulse widths in microseconds defining the travel extremes. These didn't come from a physical equation but from the industry standard for servos: 0.5ms defined one extreme and 2.4–2.5ms the other.

Stepper Motor Parameters

• STEP_PIN 16, DIR_PIN 23, FLT_PIN 22: Step, direction, and fault detection pins for the DRV8825.
• STEPS_PER_REV 200: Steps per revolution for a standard NEMA (1.8° per full step).
• MICROSTEPPING 16: Subdivision of each full step into 16 microsteps for smoother movement.
• STEP_INTERVAL_US 75: Minimum time between pulses in microseconds, which determined the maximum rotation speed.
• STEP_PULSE_US 3: Duration of the HIGH pulse on the STEP pin. The DRV8825 required a minimum pulse of a few microseconds to register a step.
• STEPS_PER_DEGREE = (200 x 16 x 1.0) / 360 = 8.89: Microsteps required to rotate the base one degree. This was the constant that converted angles into steps.

ESP-NOW Data Structure

• Struct_Message: Defined the packet received from the transmitter with six integer fields: Flex1, Flex2, Flex3 (glove sensors), and Pitch, Roll, Yaw (from the IMU MPU6050). The structure had to be identical in both transmitter and receiver.
• volatile bool New_Data: The keyword volatile was mandatory because this flag was modified inside an interrupt (the callback) and read in the main loop; it instructed the compiler not to optimize it or store it in a register.

Angle to Signal Conversion: microsADuty()

This function translated a pulse width in microseconds to the 16-bit duty cycle value needed by ledcWrite(). Since the full 20ms period was divided into 65536 steps, a direct proportion was used:

• 0.5ms was 2.5% of the 20ms cycle → 500 x 65535 / 20000 ≈ 1638
• 2.5ms was 12.5% of the cycle → 2500 x 65535 / 20000 ≈ 8192
• The constrain(us, 500, 2500) at the start ensured the pulse never went outside the servo's safe range.

Degrees to Microseconds Conversion: gradosAMicros()

• Selected the maximum pulse according to the servo: 2400µs for the 180° one (index 3) or 2500µs for the 270° ones.
• Used map() on grados x 10 instead of directly on degrees. This trick multiplied by 10 before mapping to preserve one decimal point of resolution, since map() only worked with integers and would round off degree fractions.

Smoothed Movement: Update_Servos()

• It only ran every UPDATE_INTERVAL = 20ms; if called earlier, it returned without doing anything.
• Calculated dt (actual elapsed time in seconds) and from that the maximum allowed displacement per frame: max_step = SERVO_SPEED x dt. With 70°/s and 20ms, it gave approximately 1.4° per update.
• For each servo, if the difference from the target was less than max_step, it jumped directly to the target; otherwise, it only advanced max_step in the right direction. This produced a controlled movement at a constant speed instead of instantaneous jumps.

Stepper Pulse Generation: Stepper_Update()

• Anti-oscillation safeguard: if (Stepper_Pos == Stepper_Target) return; was critical—without this early return, the motor would continuously oscillate ±1 step around the target.
• Non-blocking timing: Used micros() to respect the minimum interval between steps without halting the rest of the program. Manual pulse generation was chosen over AccelStepper because the latter provided irregular timing when sharing the loop with ESP-NOW callbacks and servo updates.
• Pulse sequence: set the direction according to the required way, waited 3µs for the driver to register the DIR change, generated the HIGH→LOW pulse on STEP, and incremented or decremented the accumulated position.

Persistent Calibration

• Save_Calibration() / Load_Calibration(): Saved and retrieved the resting angle of each servo in the flash memory under the "arm" namespace, using keys "z0" to "z3".
• Handle_Calibration(cmd): State machine for the calibration. While Cal_Index < 4, it calibrated servos; afterward, it calibrated the stepper's zero. It accepted an absolute number, a relative adjustment +N/-N, and ok to confirm and advance to the next.
• Serial_Line(): Read the serial character by character in a non-blocking way, accumulating in a buffer until finding a line break, and then returning the complete line.

Target Application: Apply_Targets()

• Servos: Combined the calibration offset, the direction, and the received value: servo_zero[i] + SERVO_DIR[i] x valores[i], bounded by the limits. The engagement mechanism only activated each servo when the incoming value approached its real position, keeping it idle until then.
• Base (stepper): Mapped the IMU's Pitch to the base's rotation. The formula (pitch + 90) x (270/180) converted the sensor's -90°...+90° range to the base's 0°...270° range, which was then translated into steps with STEPS_PER_DEGREE. Pitch was used instead of Yaw because Yaw drifted without a magnetometer to correct it.

Setup

• ledcAttach(PINES[i], FREQ, RES_BITS): Linked each servo GPIO to an ESP32 hardware timer configured at 50Hz and 16 bits.
• Loaded the saved calibration, positioned the servos at rest, and initialized ESP-NOW in WIFI_STA mode.
• Printed the receiver's MAC address via serial, data required for the transmitter to know where to send the packets.

Main Loop

• Serial commands: Typing c activated the calibration mode; any subsequent line was delegated to Handle_Calibration().
• New data: When the callback flagged New_Data, the targets were processed with Apply_Targets(). The processing occurred in the loop and not in the interrupt to keep the callback as short as possible.
• Continuous update: Update_Servos() ran on every iteration to sustain the smooth movement.
• Protected stepper: It only moved if FLT_PIN was HIGH. If the DRV8825 reported a fault (pin LOW due to overtemperature or overcurrent), it printed a warning limited to once every 2 seconds to avoid saturating the serial monitor.