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

// ==========================================
// 1. 目標 MAC 位址（請替換成飛機端印出來的 MAC）
// ==========================================
uint8_t receiverMacAddress[] = {0x24, 0x0A, 0xC4, 0x00, 0x11, 0x22}; 

// ==========================================
// 2. 腳位定義 (對應 ESP32-C3 Super Mini)
// ==========================================
const int PIN_THROTTLE = 0;  // 左手 Y 軸 (油門)
const int PIN_RUDDER   = 1;  // 左手 X 軸 (方向)
const int PIN_ELEVATOR = 2;  // 右手 Y 軸 (升降)
const int PIN_AILERON  = 3;  // 右手 X 軸 (副翼)
const int PIN_MODE_SW  = 4;  // 搖桿按鈕   (模式切換)

// ==========================================
// 3. 通訊資料結構
// ==========================================
struct DataPacket {
  int throttle;    // 油門：0 ~ 100
  int rudder;      // 方向：0 ~ 180 (中點 90)
  int elevator;    // 升降：0 ~ 180 (中點 90)
  int aileron;     // 副翼：0 ~ 180 (中點 90)
  int flightMode;  // 模式：0 手動, 1 自穩
};

DataPacket txData;

// 按鈕狀態控制
bool lastButtonState = HIGH;
int currentMode = 0; // 預設為手動模式

// ==========================================
// 4. ESP-NOW 發送狀態回報 (相容 Core 3.x 新版寫法)
// ==========================================
void OnDataSent(const esp_now_send_info_t *tx_info, esp_now_send_status_t status) {
  if (status != ESP_NOW_SEND_SUCCESS) {
    Serial.println("【警告】訊號發射失敗！飛機未開機或 MAC 錯誤！");
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(PIN_MODE_SW, INPUT_PULLUP);

  // 初始化 Wi-Fi 與 ESP-NOW
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW 初始化失敗！");
    return;
  }

  // 註冊發送回呼函數 (強制轉型解決編譯錯誤)
  esp_now_register_send_cb((esp_now_send_cb_t)OnDataSent);

  // 註冊配對飛機
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, receiverMacAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("配對飛機失敗！");
    return;
  }
  Serial.println("遙控發射端就緒！");
}

void loop() {
  // A. 處理模式切換按鈕 (按一下反轉)
  bool currentButtonState = digitalRead(PIN_MODE_SW);
  if (lastButtonState == HIGH && currentButtonState == LOW) {
    delay(15); // 去抖動
    if (digitalRead(PIN_MODE_SW) == LOW) {
      currentMode = !currentMode;
      Serial.print("切換模式至: ");
      Serial.println(currentMode == 1 ? "【MPU6050 自穩】" : "【純手動】");
    }
  }
  lastButtonState = currentButtonState;

  // B. 讀取並轉換搖桿數值
  txData.throttle   = map(analogRead(PIN_THROTTLE), 0, 4095, 0, 100);
  txData.rudder     = map(analogRead(PIN_RUDDER),   0, 4095, 0, 180);
  txData.elevator   = map(analogRead(PIN_ELEVATOR), 0, 4095, 0, 180);
  txData.aileron    = map(analogRead(PIN_AILERON),  0, 4095, 0, 180);
  txData.flightMode = currentMode;

  // C. 發送資料
  esp_now_send(receiverMacAddress, (uint8_t *) &txData, sizeof(txData));

  // D. 維持 50Hz 更新率
  delay(20); 
}
#include <WiFi.h>
#include <esp_now.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <MPU6050_light.h>

// ==========================================
// 1. 腳位定義 (對應 ESP32-C3 Super Mini)
// ==========================================
const int PIN_ESC       = 2;  // 電變
const int PIN_SERVO_AR  = 3;  // 右副翼
const int PIN_SERVO_AL  = 4;  // 左副翼
const int PIN_SERVO_ELE = 5;  // 升降舵
const int PIN_SERVO_RUD = 6;  // 方向舵

const int PIN_I2C_SDA   = 8;  // 陀螺儀 SDA
const int PIN_I2C_SCL   = 9;  // 陀螺儀 SCL

// ==========================================
// 2. 宣告元件實體與結構
// ==========================================
Servo escMotor;
Servo servoAileronR;
Servo servoAileronL;
Servo servoElevator;
Servo servoRudder;

MPU6050 mpu(Wire);

struct DataPacket {
  int throttle;
  int rudder;
  int elevator;
  int aileron;
  int flightMode;
};

DataPacket rxData;

// 自穩 PID 參數
float Kp_Roll = 0.8;
float Kp_Pitch = 0.8;

// ==========================================
// 3. ESP-NOW 接收回呼函數 (相容 Core 3.x 新版寫法)
// ==========================================
void OnDataRecv(const esp_now_recv_info_t * recv_info, const uint8_t *incomingData, int len) {
  memcpy(&rxData, incomingData, sizeof(rxData));
}

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

  // 初始化 Wi-Fi 與 ESP-NOW
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW 初始化失敗！");
    return;
  }
  
  // 註冊接收回呼函數 (強制轉型解決編譯錯誤)
  esp_now_register_recv_cb((esp_now_recv_cb_t)OnDataRecv);

  Serial.print("【系統】本機 MAC 位址為: ");
  Serial.println(WiFi.macAddress());

  // 初始化 I2C 與 MPU6050
  Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);
  byte status = mpu.begin();
  if(status != 0){ 
    Serial.println("找不到 MPU6050，請檢查接線！");
    while(1){} // 防呆卡死
  }
  
  Serial.println("正在校準陀螺儀，請保持飛機靜止...");
  delay(1000);
  mpu.calcOffsets(true); 
  Serial.println("校準完成！");

  // 初始化舵機與電變
  escMotor.attach(PIN_ESC, 1000, 2000); 
  servoAileronR.attach(PIN_SERVO_AR);
  servoAileronL.attach(PIN_SERVO_AL);
  servoElevator.attach(PIN_SERVO_ELE);
  servoRudder.attach(PIN_SERVO_RUD);

  // 開機安全歸零
  escMotor.writeMicroseconds(1000); 
  servoAileronR.write(90);
  servoAileronL.write(90);
  servoElevator.write(90);
  servoRudder.write(90);
}

void loop() {
  // 更新陀螺儀數值
  mpu.update(); 
  float currentRoll = mpu.getAngleX();
  float currentPitch = mpu.getAngleY();

  // 變數宣告
  int finalThrottle;
  int finalAileronR, finalAileronL, finalElevator, finalRudder;

  // A. 輸出油門 (0~100 轉為 1000us~2000us)
  finalThrottle = map(rxData.throttle, 0, 100, 1000, 2000);
  escMotor.writeMicroseconds(finalThrottle);

  // B. 混控邏輯
  if (rxData.flightMode == 1) { // 自穩模式
    float rollCorrection = currentRoll * Kp_Roll;
    float pitchCorrection = currentPitch * Kp_Pitch;

    finalAileronR = rxData.aileron - rollCorrection;
    finalAileronL = (180 - rxData.aileron) - rollCorrection;
    finalElevator = rxData.elevator + pitchCorrection; 
    finalRudder   = rxData.rudder;
  } 
  else { // 手動模式
    finalAileronR = rxData.aileron;
    finalAileronL = 180 - rxData.aileron; 
    finalElevator = rxData.elevator;
    finalRudder   = rxData.rudder;
  }

  // C. 安全限幅 (防止扯壞機翼)
  finalAileronR = constrain(finalAileronR, 0, 180);
  finalAileronL = constrain(finalAileronL, 0, 180);
  finalElevator = constrain(finalElevator, 0, 180);
  finalRudder   = constrain(finalRudder,   0, 180);

  // D. 輸出給實體舵機
  servoAileronR.write(finalAileronR);
  servoAileronL.write(finalAileronL);
  servoElevator.write(finalElevator);
  servoRudder.write(finalRudder);

  delay(10); 
}