Untitled

#include <SoftwareSerial.h>
#include <Servo.h>

// --- PIN DEFINITIONS ---
#define IBUS_RX_PIN     4
#define IBUS_TX_PIN     5
#define SERVO_LEFT_PIN  9
#define SERVO_RIGHT_PIN 10

// --- FLIGHT SETTINGS ---
#define WING_CENTER       90  
#define TARGET_AMPLITUDE  70   // Reduced to 70 for a safe 140-degree total sweep
#define STEER_MAX_OFFSET  45   // Max steering offset
#define SERVO_MIN_ANGLE   20   // Pulled in to prevent bottom-end dwelling
#define SERVO_MAX_ANGLE   160  // Pulled in to prevent top-end dwelling
#define SPEED_MIN_MS      450  // FASTEST speed (restored for snappier flapping)
#define SPEED_MAX_MS      1500 // SLOWEST speed

// --- HARDWARE OBJECTS ---
SoftwareSerial ibus_serial(IBUS_RX_PIN, IBUS_TX_PIN);
Servo servoLeft;
Servo servoRight;

// --- GLOBAL FLIGHT STATE ---
int channels[10];
bool isSignalValid = false;

float currentSpeed = 0.0;       
int currentSteerOffset = 0;     
float phase = 0.0;
unsigned long lastUpdate = 0;

void setup() {
  Serial.begin(115200);
  ibus_serial.begin(115200);

  // Electrical calibration bounds
  servoLeft.attach(SERVO_LEFT_PIN, 600, 2400);
  servoRight.attach(SERVO_RIGHT_PIN, 600, 2400);

  // Initialize at 90 degrees
  servoLeft.write(WING_CENTER);
  servoRight.write(WING_CENTER);
  
  Serial.println("🦋 Bionic Butterfly - Glide & Trim Fixed");
}

void loop() {
  fetchRadioSignals();
  
  if (!isSignalValid) {
    engageFailsafe();
    return;
  }

  calculateFlightDynamics();
  updateWingServos();
  
  delay(2);
}

void fetchRadioSignals() {
  if (ibus_serial.available() >= 4 && ibus_serial.read() == 0x20 && ibus_serial.read() == 0x40) {
    uint8_t buf[28];
    for (int i = 0; i < 28; i++) {
      unsigned long t = millis();
      while (!ibus_serial.available()) {
        if (millis() - t > 10) return; 
      }
      buf[i] = ibus_serial.read();
    }
    
    for (int i = 0; i < 10; i++) {
      channels[i] = buf[i * 2] | (buf[i * 2 + 1] << 8);
    }
  }

  int rawThrottle = channels[2];
  if (rawThrottle > 900 && rawThrottle < 2100) {
    isSignalValid = true;
  } else {
    isSignalValid = false;
  }
}

void engageFailsafe() {
  servoLeft.write(WING_CENTER);
  servoRight.write(WING_CENTER);
  phase = 0; 
}

void calculateFlightDynamics() {
  int rawThrottle = channels[2]; // Left stick Up/Down
  int rawSteer    = channels[0]; // Right stick Left/Right

  // 1. Calculate Flapping Speed
  if (rawThrottle > 1100) {
    currentSpeed = (float)(rawThrottle - 1100) / 800.0;
    currentSpeed = constrain(currentSpeed, 0.0, 1.0);
  } else {
    currentSpeed = 0.0; 
  }

  // 2. Calculate Steering Offset
  currentSteerOffset = 0;
  if (rawSteer < 1450 || rawSteer > 1550) { // Deadzone
    float steerNorm = (float)(rawSteer - 1500) / 500.0;
    steerNorm = constrain(steerNorm, -1.0, 1.0);
    currentSteerOffset = (int)(steerNorm * STEER_MAX_OFFSET);
  }
}

void updateWingServos() {
  // Determine where the wings should be pointing based on steering
  int leftCenter  = WING_CENTER - currentSteerOffset;
  int rightCenter = WING_CENTER + currentSteerOffset;

  // --- GLIDE MODE (Throttle is 0) ---
  if (currentSpeed <= 0.01) {
    // Write steering offsets but do not flap
    servoLeft.write(constrain(leftCenter, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE));
    servoRight.write(constrain(rightCenter, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE));
    phase = 0; // Reset wave for next flap
    return;
  }

  // --- FLAPPING MODE ---
  unsigned long now = millis();
  unsigned long dt = now - lastUpdate;
  lastUpdate = now;

  float cycleDuration = map(currentSpeed * 1000, 1, 1000, SPEED_MAX_MS, SPEED_MIN_MS);
  float phaseStep = (TWO_PI / cycleDuration) * dt;
  
  phase += phaseStep;
  if (phase > TWO_PI) phase -= TWO_PI;

  float sineVal = sin(phase);

  // Dynamic Amplitude Calculation
  int maxLeftAmplitude  = min(SERVO_MAX_ANGLE - leftCenter, leftCenter - SERVO_MIN_ANGLE);
  int maxRightAmplitude = min(SERVO_MAX_ANGLE - rightCenter, rightCenter - SERVO_MIN_ANGLE);
  int safeAmplitude = min(maxLeftAmplitude, maxRightAmplitude);
  int actualAmplitude = min(TARGET_AMPLITUDE, safeAmplitude);

  // Calculate final moving angles
  int leftAngle  = leftCenter  + (int)(sineVal * actualAmplitude);
  int rightAngle = rightCenter - (int)(sineVal * actualAmplitude); 

  // Hard clamp safety net
  leftAngle = constrain(leftAngle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
  rightAngle = constrain(rightAngle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);

  servoLeft.write(leftAngle);
  servoRight.write(rightAngle);
}