diff --git a/src/main.cpp b/src/main.cpp index d437e0d..56f0d18 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,42 +2,101 @@ #include #include -// Gamepad'lerin listesini tutmak için dizi (Bluepad32 birden fazla cihaz destekler) +// ===================================================================== +// ROBOT KOL - 4 SERVO KONTROL (ESP32 + Bluepad32) +// Best Practice: Hareket bitince servo detach edilir -> SIFIR titreme +// ===================================================================== + ControllerPtr myControllers[BP32_MAX_GAMEPADS]; -// Servo motor nesnelerimiz -Servo baseServo; // Taban (GPIO 13) -Servo shoulderServo; // Omuz (GPIO 12) -Servo elbowServo; // Dirsek (GPIO 14) -Servo gripperServo; // Kıskaç (GPIO 27) +// --- SERVO NESNELERİ --- +Servo baseServo; +Servo shoulderServo; +Servo elbowServo; +Servo gripperServo; -// Pin tanımlamaları -const int BASE_PIN = 13; +// --- PIN TANIMLARI --- +const int BASE_PIN = 13; const int SHOULDER_PIN = 12; -const int ELBOW_PIN = 14; -const int GRIPPER_PIN = 27; +const int ELBOW_PIN = 14; +const int GRIPPER_PIN = 27; -// Başlangıç açıları (Genel olarak orta konumlarda başlatıyoruz) -float baseAngle = 90.0; -float shoulderAngle = 90.0; -float elbowAngle = 90.0; -float gripperAngle = 90.0; // Kıskaç da yumuşak hareket için 90'dan başlasın +// --- AÇI LİMİTLERİ (her servo için ayrı ayrı) --- +const int BASE_MIN = 0; const int BASE_MAX = 150; +const int SHOULDER_MIN = 0; const int SHOULDER_MAX = 150; +const int ELBOW_MIN = 0; const int ELBOW_MAX = 150; +const int GRIPPER_MIN = 0; const int GRIPPER_MAX = 150; -// Eski mikrosaniye (us) değerlerini hafızada tutarak sadece değişim olduğunda yazacağız -int lastBaseUs = -1; -int lastShoulderUs = -1; -int lastElbowUs = -1; -int lastGripperUs = -1; +// --- KALİBRASYON OFSETLERİ (montaj kayması düzeltme) --- +const int BASE_OFFSET = 0; +const int SHOULDER_OFFSET = 0; +const int ELBOW_OFFSET = 0; +const int GRIPPER_OFFSET = 0; -// Joystick deadzone (ölü bölge) eşiği -const int DEADZONE = 50; +// --- BAŞLANGIÇ AÇILARI --- +int baseAngle = (BASE_MIN + BASE_MAX) / 2; // 75 - orta konum +int shoulderAngle = (SHOULDER_MIN + SHOULDER_MAX) / 2; // 75 - orta konum +int elbowAngle = (ELBOW_MIN + ELBOW_MAX) / 2; // 75 - orta konum +int gripperAngle = GRIPPER_MIN; // 0 - KISKAÇ KAPALI BAŞLAR -// Yumuşak ve kontrollü hareket için maksimum hız limitleri (Derece / Döngü) -const float MAX_SPEED_BASE = 2.2; // Taban motoru için maksimum dönüş hızı -const float MAX_SPEED_ARM = 1.8; // Omuz ve dirsek motorları için maksimum hız -const float GRIPPER_SPEED = 2.0; // Kıskacın kapanma/açılma hızı (Daha seri) +// --- SON YAZILAN AÇILAR (değişim kontrolü için) --- +int lastBaseAngle = -1; +int lastShoulderAngle = -1; +int lastElbowAngle = -1; +int lastGripperAngle = -1; + +// --- SERVO ATTACH DURUMU --- +bool baseAttached = false; +bool shoulderAttached = false; +bool elbowAttached = false; +bool gripperAttached = false; + +// --- DETACH ZAMANLAYICILARI --- +// Servo hedefe ulaştıktan sonra bu süre (ms) geçince detach edilir +// Detach = PWM sinyali kesilir = SIFIR titreme +const unsigned long DETACH_DELAY = 250; // ms +unsigned long baseLastMoveTime = 0; +unsigned long shoulderLastMoveTime = 0; +unsigned long elbowLastMoveTime = 0; +unsigned long gripperLastMoveTime = 0; + +// --- KONTROL AYARLARI --- +const int DEADZONE = 100; // Joystick ölü bölge (yüksek = daha kararlı) +const int STEP_SPEED_BASE = 2; // Taban adım (derece/döngü) +const int STEP_SPEED_ARM = 2; // Omuz/Dirsek adım +const int STEP_SPEED_GRIP = 3; // Kıskaç adım + +// --- YÖN TERSLEME --- +const bool INVERT_BASE = false; +const bool INVERT_SHOULDER = true; +const bool INVERT_ELBOW = true; +const bool INVERT_GRIPPER = false; + +// ===================================================================== +// SERVO YARDIMCI FONKSİYONLARI +// ===================================================================== + +// Servo'yu attach et (eğer değilse) ve açı yaz +void safeServoWrite(Servo &servo, int pin, bool &attached, int angle) { + if (!attached) { + servo.attach(pin, 500, 2400); + attached = true; + } + servo.write(angle); +} + +// Servo'yu detach et (PWM sinyalini tamamen kes) +void safeServoDetach(Servo &servo, bool &attached) { + if (attached) { + servo.detach(); + attached = false; + } +} + +// ===================================================================== +// BLUEPAD32 CALLBACK'LERİ +// ===================================================================== -// Gamepad bağlandığında tetiklenecek callback fonksiyonu void onConnectedController(ControllerPtr ctl) { for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { if (myControllers[i] == nullptr) { @@ -48,7 +107,6 @@ void onConnectedController(ControllerPtr ctl) { } } -// Gamepad bağlantısı koptuğunda tetiklenecek callback fonksiyonu void onDisconnectedController(ControllerPtr ctl) { for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { if (myControllers[i] == ctl) { @@ -59,187 +117,170 @@ void onDisconnectedController(ControllerPtr ctl) { } } -// Gamepad verilerini işleyen fonksiyon +// ===================================================================== +// GAMEPAD İŞLEME +// ===================================================================== + void processGamepad(ControllerPtr ctl) { - // Sol ve Sağ analog çubuk değerlerini alıyoruz int lx = ctl->axisX(); int ly = ctl->axisY(); int ry = ctl->axisRY(); + bool btnA = ctl->a(); + bool btnB = ctl->b(); - // Buton basış durumlarını takip etmek için static değişkenler (Loglama için) - static bool prevA = false; - static bool prevB = false; - static bool prevX = false; - static bool prevY = false; - static int prevLx = 0; - static int prevLy = 0; - static int prevRy = 0; + unsigned long now = millis(); - bool curA = ctl->a(); - bool curB = ctl->b(); - bool curX = ctl->x(); - bool curY = ctl->y(); - - // Buton A Loglaması - if (curA != prevA) { - if (curA) { - Serial.println("[INPUT] A Butonuna Basildi -> Kiskac Aciliyor (Yumusak)"); - } else { - Serial.println("[INPUT] A Butonu Birakildi"); - } - prevA = curA; + // --- TABAN (Sol Analog X) --- + if (lx < -DEADZONE) { + baseAngle += INVERT_BASE ? STEP_SPEED_BASE : -STEP_SPEED_BASE; + } else if (lx > DEADZONE) { + baseAngle += INVERT_BASE ? -STEP_SPEED_BASE : STEP_SPEED_BASE; } - // Buton B Loglaması - if (curB != prevB) { - if (curB) { - Serial.println("[INPUT] B Butonuna Basildi -> Kiskac Kapatiliyor (Yumusak)"); - } else { - Serial.println("[INPUT] B Butonu Birakildi"); - } - prevB = curB; + // --- OMUZ (Sol Analog Y) --- + if (ly < -DEADZONE) { + shoulderAngle += INVERT_SHOULDER ? -STEP_SPEED_ARM : STEP_SPEED_ARM; + } else if (ly > DEADZONE) { + shoulderAngle += INVERT_SHOULDER ? STEP_SPEED_ARM : -STEP_SPEED_ARM; } - // Buton X Loglaması - if (curX != prevX) { - if (curX) { - Serial.println("[INPUT] X Butonuna Basildi"); - } else { - Serial.println("[INPUT] X Butonu Birakildi"); - } - prevX = curX; + // --- DİRSEK (Sağ Analog Y) --- + if (ry < -DEADZONE) { + elbowAngle += INVERT_ELBOW ? -STEP_SPEED_ARM : STEP_SPEED_ARM; + } else if (ry > DEADZONE) { + elbowAngle += INVERT_ELBOW ? STEP_SPEED_ARM : -STEP_SPEED_ARM; } - // Buton Y Loglaması - if (curY != prevY) { - if (curY) { - Serial.println("[INPUT] Y Butonuna Basildi"); - } else { - Serial.println("[INPUT] Y Butonu Birakildi"); - } - prevY = curY; + // --- KISKAÇ (A=Aç, B=Kapat) --- + if (btnA) { + gripperAngle += INVERT_GRIPPER ? -STEP_SPEED_GRIP : STEP_SPEED_GRIP; + } else if (btnB) { + gripperAngle += INVERT_GRIPPER ? STEP_SPEED_GRIP : -STEP_SPEED_GRIP; } - // Joystick hareketlerini loglama - if (abs(lx - prevLx) > 30 || abs(ly - prevLy) > 30 || abs(ry - prevRy) > 30) { - if (abs(lx) > DEADZONE || abs(ly) > DEADZONE || abs(ry) > DEADZONE) { - Serial.printf("[JOYSTICK] lx: %d, ly: %d, ry: %d | Aci -> Taban: %.1f, Omuz: %.1f, Dirsek: %.1f\n", - lx, ly, ry, baseAngle, shoulderAngle, elbowAngle); - } - prevLx = lx; - prevLy = ly; - prevRy = ry; + // --- AÇI SINIRLARINI UYGULA --- + baseAngle = constrain(baseAngle, BASE_MIN, BASE_MAX); + shoulderAngle = constrain(shoulderAngle, SHOULDER_MIN, SHOULDER_MAX); + elbowAngle = constrain(elbowAngle, ELBOW_MIN, ELBOW_MAX); + gripperAngle = constrain(gripperAngle, GRIPPER_MIN, GRIPPER_MAX); + + // --- SADECE DEĞİŞEN SERVOYA YAZ --- + if (baseAngle != lastBaseAngle) { + safeServoWrite(baseServo, BASE_PIN, baseAttached, baseAngle + BASE_OFFSET); + lastBaseAngle = baseAngle; + baseLastMoveTime = now; + Serial.printf("[MOVE] Taban: %d\n", baseAngle); + } + if (shoulderAngle != lastShoulderAngle) { + safeServoWrite(shoulderServo, SHOULDER_PIN, shoulderAttached, shoulderAngle + SHOULDER_OFFSET); + lastShoulderAngle = shoulderAngle; + shoulderLastMoveTime = now; + Serial.printf("[MOVE] Omuz: %d\n", shoulderAngle); + } + if (elbowAngle != lastElbowAngle) { + safeServoWrite(elbowServo, ELBOW_PIN, elbowAttached, elbowAngle + ELBOW_OFFSET); + lastElbowAngle = elbowAngle; + elbowLastMoveTime = now; + Serial.printf("[MOVE] Dirsek: %d\n", elbowAngle); + } + if (gripperAngle != lastGripperAngle) { + safeServoWrite(gripperServo, GRIPPER_PIN, gripperAttached, gripperAngle + GRIPPER_OFFSET); + lastGripperAngle = gripperAngle; + gripperLastMoveTime = now; + Serial.printf("[MOVE] Kiskac: %d\n", gripperAngle); } - // 1. TABAN - Sol Analog X (Karesel Tepki Eğrisi) - if (abs(lx) > DEADZONE) { - float normalized = lx / 512.0; - baseAngle += (normalized * abs(normalized)) * MAX_SPEED_BASE; + // --- DETACH: Hareket bittikten sonra PWM sinyalini kes --- + // Servo hedefe ulaştı ve DETACH_DELAY süresi geçti -> detach et + // Bu sayede motorlar boşta iken ASLA titremez/hareket etmez + if (baseAttached && (now - baseLastMoveTime > DETACH_DELAY)) { + safeServoDetach(baseServo, baseAttached); } - - // 2. OMUZ - Sol Analog Y (Karesel Tepki Eğrisi) - if (abs(ly) > DEADZONE) { - float normalized = ly / 512.0; - shoulderAngle -= (normalized * abs(normalized)) * MAX_SPEED_ARM; + if (shoulderAttached && (now - shoulderLastMoveTime > DETACH_DELAY)) { + safeServoDetach(shoulderServo, shoulderAttached); } - - // 3. DİRSEK - Sağ Analog Y (Karesel Tepki Eğrisi) - if (abs(ry) > DEADZONE) { - float normalized = ry / 512.0; - elbowAngle -= (normalized * abs(normalized)) * MAX_SPEED_ARM; + if (elbowAttached && (now - elbowLastMoveTime > DETACH_DELAY)) { + safeServoDetach(elbowServo, elbowAttached); } - - // 4. KISKAÇ - A ve B Butonları ile YUMUŞAK kontrol - if (ctl->a()) { - gripperAngle += GRIPPER_SPEED; // Kademeli açılma - } else if (ctl->b()) { - gripperAngle -= GRIPPER_SPEED; // Kademeli kapanma - } - - // Servoların fiziksel sınırlarını (0 - 180 derece arası) constrain ile garanti altına alıyoruz - baseAngle = constrain(baseAngle, 0.0, 180.0); - shoulderAngle = constrain(shoulderAngle, 0.0, 180.0); - elbowAngle = constrain(elbowAngle, 0.0, 180.0); - gripperAngle = constrain(gripperAngle, 0.0, 180.0); - - // Float açıları mikrosaniyeye dönüştürüyoruz (0° -> 500us, 180° -> 2400us) - int currentBaseUs = round(500 + (baseAngle / 180.0) * 1900.0); - int currentShoulderUs = round(500 + (shoulderAngle / 180.0) * 1900.0); - int currentElbowUs = round(500 + (elbowAngle / 180.0) * 1900.0); - int currentGripperUs = round(500 + (gripperAngle / 180.0) * 1900.0); - - // KRİTİK FİLTRE: Sadece mikrosaniye değeri değiştiyse servo motorlara yaz - // Çok ufak elektriksel gürültüleri filtrelemek için en az 2 us değişim şartı koşuyoruz - if (abs(currentBaseUs - lastBaseUs) >= 2) { - baseServo.writeMicroseconds(currentBaseUs); - lastBaseUs = currentBaseUs; - } - if (abs(currentShoulderUs - lastShoulderUs) >= 2) { - shoulderServo.writeMicroseconds(currentShoulderUs); - lastShoulderUs = currentShoulderUs; - } - if (abs(currentElbowUs - lastElbowUs) >= 2) { - elbowServo.writeMicroseconds(currentElbowUs); - lastElbowUs = currentElbowUs; - } - if (abs(currentGripperUs - lastGripperUs) >= 2) { - gripperServo.writeMicroseconds(currentGripperUs); - lastGripperUs = currentGripperUs; + if (gripperAttached && (now - gripperLastMoveTime > DETACH_DELAY)) { + safeServoDetach(gripperServo, gripperAttached); } } -void setup() { - // Seri haberleşmeyi başlat (115200 baud) - Serial.begin(115200); - while (!Serial) { - delay(1); // Seri portun hazır olmasını bekle - } +// ===================================================================== +// SETUP +// ===================================================================== + +void setup() { + Serial.begin(115200); + while (!Serial) { delay(1); } - // ESP32'nin donanımsal PWM zamanlayıcılarını tahsis et ESP32PWM::allocateTimer(0); ESP32PWM::allocateTimer(1); ESP32PWM::allocateTimer(2); ESP32PWM::allocateTimer(3); - // Servoların frekansını ayarla (Standart servo motorlar 50 Hz'de çalışır) + // Servoları başlangıç konumuna götür baseServo.setPeriodHertz(50); shoulderServo.setPeriodHertz(50); elbowServo.setPeriodHertz(50); gripperServo.setPeriodHertz(50); - // Servoları ilgili GPIO pinlerine ata (Standart pulse aralıkları: 500-2400 mikrosaniye) baseServo.attach(BASE_PIN, 500, 2400); shoulderServo.attach(SHOULDER_PIN, 500, 2400); elbowServo.attach(ELBOW_PIN, 500, 2400); gripperServo.attach(GRIPPER_PIN, 500, 2400); - // İlk konumları yaz - lastBaseUs = round(500 + (baseAngle / 180.0) * 1900.0); - lastShoulderUs = round(500 + (shoulderAngle / 180.0) * 1900.0); - lastElbowUs = round(500 + (elbowAngle / 180.0) * 1900.0); - lastGripperUs = round(500 + (gripperAngle / 180.0) * 1900.0); + baseServo.write(baseAngle + BASE_OFFSET); + shoulderServo.write(shoulderAngle + SHOULDER_OFFSET); + elbowServo.write(elbowAngle + ELBOW_OFFSET); + gripperServo.write(gripperAngle + GRIPPER_OFFSET); - baseServo.writeMicroseconds(lastBaseUs); - shoulderServo.writeMicroseconds(lastShoulderUs); - elbowServo.writeMicroseconds(lastElbowUs); - gripperServo.writeMicroseconds(lastGripperUs); + lastBaseAngle = baseAngle; + lastShoulderAngle = shoulderAngle; + lastElbowAngle = elbowAngle; + lastGripperAngle = gripperAngle; + + baseAttached = true; + shoulderAttached = true; + elbowAttached = true; + gripperAttached = true; + + // Başlangıç konumuna ulaşması için bekle, sonra detach et + delay(500); + safeServoDetach(baseServo, baseAttached); + safeServoDetach(shoulderServo, shoulderAttached); + safeServoDetach(elbowServo, elbowAttached); + safeServoDetach(gripperServo, gripperAttached); - // Bluepad32 kütüphanesini başlat ve callback fonksiyonlarını bağla BP32.setup(&onConnectedController, &onDisconnectedController); - Serial.println("Sistem Hazir."); + Serial.println("Sistem Hazir - Tum servolar detach edildi."); } +// ===================================================================== +// LOOP +// ===================================================================== + void loop() { - // Bluetooth bağlantı durumunu ve gamepad verilerini günceller (Zorunlu) BP32.update(); - // Bağlı olan tüm gamepad'leri işle for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { - ControllerPtr myController = myControllers[i]; - if (myController && myController->isConnected()) { - processGamepad(myController); + ControllerPtr ctl = myControllers[i]; + if (ctl && ctl->isConnected()) { + processGamepad(ctl); } } - delay(15); -} + // Gamepad bağlı değilken de detach kontrolü yap + unsigned long now = millis(); + if (baseAttached && (now - baseLastMoveTime > DETACH_DELAY)) + safeServoDetach(baseServo, baseAttached); + if (shoulderAttached && (now - shoulderLastMoveTime > DETACH_DELAY)) + safeServoDetach(shoulderServo, shoulderAttached); + if (elbowAttached && (now - elbowLastMoveTime > DETACH_DELAY)) + safeServoDetach(elbowServo, elbowAttached); + if (gripperAttached && (now - gripperLastMoveTime > DETACH_DELAY)) + safeServoDetach(gripperServo, gripperAttached); + + delay(20); +}