diff --git a/src/main.cpp b/src/main.cpp index 56f0d18..6c07720 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,7 +4,7 @@ // ===================================================================== // ROBOT KOL - 4 SERVO KONTROL (ESP32 + Bluepad32) -// Best Practice: Hareket bitince servo detach edilir -> SIFIR titreme +// v2 - Optimize: Oransal hız, joystick smoothing, gelişmiş kalibrasyon // ===================================================================== ControllerPtr myControllers[BP32_MAX_GAMEPADS]; @@ -21,11 +21,16 @@ const int SHOULDER_PIN = 12; const int ELBOW_PIN = 14; const int GRIPPER_PIN = 27; +// --- SG90 SERVO PWM AYARLARI --- +// SG90 tipik aralık: 500-2400µs = 0-180° +const int SERVO_MIN_US = 500; +const int SERVO_MAX_US = 2400; + // --- AÇI LİMİTLERİ (her servo için ayrı ayrı) --- -const int BASE_MIN = 0; const int BASE_MAX = 150; +const int BASE_MIN = 0; const int BASE_MAX = 180; 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; +const int ELBOW_MIN = 0; const int ELBOW_MAX = 180; +const int GRIPPER_MIN = 0; const int GRIPPER_MAX = 180; // SG90 tam aralık - kıskaç tam kapansın // --- KALİBRASYON OFSETLERİ (montaj kayması düzeltme) --- const int BASE_OFFSET = 0; @@ -34,10 +39,16 @@ const int ELBOW_OFFSET = 0; const int GRIPPER_OFFSET = 0; // --- BAŞLANGIÇ AÇILARI --- -int baseAngle = (BASE_MIN + BASE_MAX) / 2; // 75 - orta konum +int baseAngle = 90; // Gövde ortada 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 +int elbowAngle = 90; // Dirsek ortada +int gripperAngle = GRIPPER_MAX; // Kıskaç AÇIK başlar (MAX = açık) + +// --- GERÇEK HEDEF AÇILAR (float - yumuşak hareket için) --- +float baseTarget = 90.0; +float shoulderTarget = 75.0; +float elbowTarget = 90.0; +float gripperTarget = (float)GRIPPER_MAX; // --- SON YAZILAN AÇILAR (değişim kontrolü için) --- int lastBaseAngle = -1; @@ -53,33 +64,44 @@ 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 +const unsigned long DETACH_DELAY = 400; // ms (artırıldı - dirsek titremesini önler) 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 +const int DEADZONE = 120; // Joystick ölü bölge (artırıldı - daha kararlı) + +// Oransal hız kontrol: joystick ne kadar itilirse o kadar hızlı +// MAX değerler = joystick tam itildiğinde derece/döngü +const float MAX_SPEED_BASE = 0.8; // Taban max hız (yavaşlatıldı) +const float MAX_SPEED_ARM = 1.5; // Omuz/Dirsek max hız (yavaşlatıldı) +const float MAX_SPEED_GRIP = 3.0; // Kıskaç max hız +const float MIN_SPEED = 0.3; // Minimum hareket eşiği + +// Joystick smoothing (exponential moving average) +// 0.0 = çok yumuşak (gecikmeli), 1.0 = ham veri (filtresiz) +const float SMOOTHING_FACTOR = 0.6; // 0.3'ten artırıldı - daha az gecikme +float smoothLX = 0, smoothLY = 0, smoothRY = 0; + +// Joystick max değeri (Bluepad32 tipik: ±512) +const float JOY_MAX = 512.0; // --- YÖN TERSLEME --- -const bool INVERT_BASE = false; +const bool INVERT_BASE = true; // DÜZELTİLDİ: Gövde ters dönüyordu const bool INVERT_SHOULDER = true; -const bool INVERT_ELBOW = true; +const bool INVERT_ELBOW = false; // DÜZELTİLDİ: Dirsek titremesi - ters kaldırıldı const bool INVERT_GRIPPER = false; // ===================================================================== -// SERVO YARDIMCI FONKSİYONLARI +// YARDIMCI FONKSİYONLAR // ===================================================================== // 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); + servo.attach(pin, SERVO_MIN_US, SERVO_MAX_US); attached = true; } servo.write(angle); @@ -93,6 +115,23 @@ void safeServoDetach(Servo &servo, bool &attached) { } } +// Joystick değerini oransal hıza çevir +// Deadzone içinde 0, deadzone dışında oransal hız döndürür +float joystickToSpeed(float axis, float maxSpeed) { + float absVal = fabs(axis); + if (absVal < DEADZONE) return 0.0; + + // Deadzone'dan sonraki kısmı 0-1 aralığına normalize et + float normalized = (absVal - DEADZONE) / (JOY_MAX - DEADZONE); + normalized = constrain(normalized, 0.0f, 1.0f); + + // Kuadratik eğri: Yavaş başla, hızlan (daha hassas kontrol) + float speed = normalized * normalized * maxSpeed; + + // Yön ekle + return (axis > 0) ? speed : -speed; +} + // ===================================================================== // BLUEPAD32 CALLBACK'LERİ // ===================================================================== @@ -101,7 +140,7 @@ void onConnectedController(ControllerPtr ctl) { for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { if (myControllers[i] == nullptr) { myControllers[i] = ctl; - Serial.println("Gamepad Baglandi"); + Serial.println("Gamepad Baglandi!"); break; } } @@ -122,77 +161,84 @@ void onDisconnectedController(ControllerPtr ctl) { // ===================================================================== void processGamepad(ControllerPtr ctl) { - int lx = ctl->axisX(); - int ly = ctl->axisY(); - int ry = ctl->axisRY(); + // Ham joystick değerlerini oku + float rawLX = (float)ctl->axisX(); + float rawLY = (float)ctl->axisY(); + float rawRY = (float)ctl->axisRY(); bool btnA = ctl->a(); bool btnB = ctl->b(); + // Exponential Moving Average ile yumuşatma + smoothLX = smoothLX + SMOOTHING_FACTOR * (rawLX - smoothLX); + smoothLY = smoothLY + SMOOTHING_FACTOR * (rawLY - smoothLY); + smoothRY = smoothRY + SMOOTHING_FACTOR * (rawRY - smoothRY); + unsigned long now = millis(); - // --- 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; + // --- TABAN / GÖVDE (Sol Analog X) - Oransal hız --- + float baseSpeed = joystickToSpeed(smoothLX, MAX_SPEED_BASE); + if (baseSpeed != 0.0f) { + baseTarget += INVERT_BASE ? -baseSpeed : baseSpeed; } - // --- 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; + // --- OMUZ (Sol Analog Y) - Oransal hız --- + float shoulderSpeed = joystickToSpeed(smoothLY, MAX_SPEED_ARM); + if (shoulderSpeed != 0.0f) { + shoulderTarget += INVERT_SHOULDER ? -shoulderSpeed : shoulderSpeed; } - // --- 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; + // --- DİRSEK (Sağ Analog Y) - Oransal hız --- + float elbowSpeed = joystickToSpeed(smoothRY, MAX_SPEED_ARM); + if (elbowSpeed != 0.0f) { + elbowTarget += INVERT_ELBOW ? -elbowSpeed : elbowSpeed; } // --- KISKAÇ (A=Aç, B=Kapat) --- if (btnA) { - gripperAngle += INVERT_GRIPPER ? -STEP_SPEED_GRIP : STEP_SPEED_GRIP; + gripperTarget += INVERT_GRIPPER ? -MAX_SPEED_GRIP : MAX_SPEED_GRIP; } else if (btnB) { - gripperAngle += INVERT_GRIPPER ? STEP_SPEED_GRIP : -STEP_SPEED_GRIP; + gripperTarget += INVERT_GRIPPER ? MAX_SPEED_GRIP : -MAX_SPEED_GRIP; } - // --- 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); + // --- HEDEF AÇILARI SINIRLA --- + baseTarget = constrain(baseTarget, (float)BASE_MIN, (float)BASE_MAX); + shoulderTarget = constrain(shoulderTarget, (float)SHOULDER_MIN, (float)SHOULDER_MAX); + elbowTarget = constrain(elbowTarget, (float)ELBOW_MIN, (float)ELBOW_MAX); + gripperTarget = constrain(gripperTarget, (float)GRIPPER_MIN, (float)GRIPPER_MAX); + + // --- Float'tan int'e çevir --- + baseAngle = (int)round(baseTarget); + shoulderAngle = (int)round(shoulderTarget); + elbowAngle = (int)round(elbowTarget); + gripperAngle = (int)round(gripperTarget); // --- 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); + Serial.printf("[BASE] %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); + Serial.printf("[SHLDR] %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); + Serial.printf("[ELBOW] %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); + Serial.printf("[GRIP] %d°\n", gripperAngle); } // --- 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); } @@ -226,10 +272,10 @@ void setup() { elbowServo.setPeriodHertz(50); gripperServo.setPeriodHertz(50); - baseServo.attach(BASE_PIN, 500, 2400); - shoulderServo.attach(SHOULDER_PIN, 500, 2400); - elbowServo.attach(ELBOW_PIN, 500, 2400); - gripperServo.attach(GRIPPER_PIN, 500, 2400); + baseServo.attach(BASE_PIN, SERVO_MIN_US, SERVO_MAX_US); + shoulderServo.attach(SHOULDER_PIN, SERVO_MIN_US, SERVO_MAX_US); + elbowServo.attach(ELBOW_PIN, SERVO_MIN_US, SERVO_MAX_US); + gripperServo.attach(GRIPPER_PIN, SERVO_MIN_US, SERVO_MAX_US); baseServo.write(baseAngle + BASE_OFFSET); shoulderServo.write(shoulderAngle + SHOULDER_OFFSET); @@ -246,15 +292,18 @@ void setup() { elbowAttached = true; gripperAttached = true; + Serial.printf("Baslangic: Base=%d Shoulder=%d Elbow=%d Gripper=%d\n", + baseAngle, shoulderAngle, elbowAngle, gripperAngle); + // Başlangıç konumuna ulaşması için bekle, sonra detach et - delay(500); + delay(800); safeServoDetach(baseServo, baseAttached); safeServoDetach(shoulderServo, shoulderAttached); safeServoDetach(elbowServo, elbowAttached); safeServoDetach(gripperServo, gripperAttached); BP32.setup(&onConnectedController, &onDisconnectedController); - Serial.println("Sistem Hazir - Tum servolar detach edildi."); + Serial.println("Sistem Hazir - Tum servolar detach."); } // =====================================================================