/*
ESP32 Bluetooth HID RC ALICI
Version 1.2.0
Bluepad32 uyumlu Bluetooth HID kontrol cihazları için.

NOT:
- Kontrol cihazınızı belirledikten sonra BTN_* değerlerini güncelleyin.
- 5-8 numaralı kanallar, butonlar ile kontrol edilen servo kanallarıdır.
*/
21
#include <Bluepad32.h>
#include <ESP32Servo.h>

//================ KULLANICI AYARLARI ====================

#define CHANNEL_COUNT 8

// Servo Pins
const uint8_t servoPins[CHANNEL_COUNT] = {
  19, // CH1 Roll
  18, // CH2 Throttle
  32, // CH3 Yaw1
  33, // CH4 Pitch
  25, // CH5 L1/L2
  26, // CH6 R1/R2
  27, // CH7 Kare1/Kare2
  14  // CH8 Kare3/Kare4
};

// Buton haritası (P09 için - Gerekirse değiştir)
#define BTN_L1      64
#define BTN_L2      16
#define BTN_R1      128
#define BTN_R2      32

#define BTN_KARE1   1
#define BTN_KARE2   2
#define BTN_KARE3   4
#define BTN_KARE4   8

#define DEAD_BAND 15

bool reverseCh[CHANNEL_COUNT]={
  false,false,false,false,false,false,false,false
};

int centerUs[4]={
  1500,1500,1500,1500
};

int minUs[CHANNEL_COUNT]={
 1000,1000,1000,1000,1000,1000,1000,1000
};

int maxUs[CHANNEL_COUNT]={
 2000,2000,2000,2000,2000,2000,2000,2000
};

int failsafeUs[CHANNEL_COUNT]={
 1500,1500,1500,1500,1500,1500,1500,1500
};

//===================================================

ControllerPtr controller=nullptr;
Servo servo[CHANNEL_COUNT];

uint32_t lastPrint=0;

void onConnectedController(ControllerPtr ctl){
  controller=ctl;
  Serial.println("Controller Connected");
}

void onDisconnectedController(ControllerPtr ctl){
  controller=nullptr;
  Serial.println("Controller Disconnected");
}

int calcAxis(int value,uint8_t ch)
{
  value=constrain(value,-512,512);

  if(reverseCh[ch]) value=-value;

  if(abs(value)<DEAD_BAND) value=0;

  int us=map(value,-512,512,minUs[ch],maxUs[ch]);

  int delta=centerUs[ch]-1500;
  us+=delta;

  return constrain(us,minUs[ch],maxUs[ch]);
}

void applyFailsafe()
{
  for(int i=0;i<CHANNEL_COUNT;i++)
    servo[i].writeMicroseconds(failsafeUs[i]);
}

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

  for(int i=0;i<CHANNEL_COUNT;i++){
    servo[i].setPeriodHertz(50);
    servo[i].attach(servoPins[i],1000,2000);
  }

  applyFailsafe();

  BP32.setup(&onConnectedController,&onDisconnectedController);

  Serial.println();
  Serial.println("ESP32 Bluetooth HID RC Receiver");
  Serial.println("Version 1.1.0");
  Serial.println("Waiting controller...");
}

void processButtons(uint16_t b)
{
  // CH5
  if(b & BTN_L1) servo[4].writeMicroseconds(maxUs[4]);
  else if(b & BTN_L2) servo[4].writeMicroseconds(minUs[4]);
  else servo[4].writeMicroseconds(centerUs[0]);

  // CH6
  if(b & BTN_R1) servo[5].writeMicroseconds(maxUs[5]);
  else if(b & BTN_R2) servo[5].writeMicroseconds(minUs[5]);
  else servo[5].writeMicroseconds(1500);

  // CH7
  if(BTN_KARE1 && (b & BTN_KARE1)) servo[6].writeMicroseconds(maxUs[6]);
  else if(BTN_KARE2 && (b & BTN_KARE2)) servo[6].writeMicroseconds(minUs[6]);
  else servo[6].writeMicroseconds(1500);

  // CH8
  if(BTN_KARE3 && (b & BTN_KARE3)) servo[7].writeMicroseconds(maxUs[7]);
  else if(BTN_KARE4 && (b & BTN_KARE4)) servo[7].writeMicroseconds(minUs[7]);
  else servo[7].writeMicroseconds(1500);
}

void loop()
{
  BP32.update();

  if(!controller || !controller->isConnected()){
    applyFailsafe();
    return;
  }

  int ch[4];
  ch[0]=calcAxis(controller->axisX(),0);
  ch[1]=calcAxis(controller->axisY(),1);
  ch[2]=calcAxis(controller->axisRX(),2);
  ch[3]=calcAxis(controller->axisRY(),3);

  for(int i=0;i<4;i++)
    servo[i].writeMicroseconds(ch[i]);

  processButtons(controller->buttons());

  if(millis()-lastPrint>200){
    lastPrint=millis();
    Serial.printf("CH1=%d CH2=%d CH3=%d CH4=%d BTN=%u\n",
      ch[0],ch[1],ch[2],ch[3],controller->buttons());
  }
}