feat: implement optimized robot arm control with proportional joystick speed, signal smoothing, and dynamic servo detach

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 ELBOW_MIN    = 0;   const int ELBOW_MAX    = 180;
-const int GRIPPER_MIN  = 0;   const int GRIPPER_MAX  = 150;
+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 elbowAngle    = 90;                          // Dirsek ortada
-int gripperAngle  = GRIPPER_MIN;                        // 0 - KISKAÇ KAPALI BAŞLAR
+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 = 400; // ms (artırıldı - dirsek titremesini önler)
-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 DEADZONE = 120;          // Joystick ölü bölge (artırıldı - daha kararlı)
-const int STEP_SPEED_BASE = 2;    // Taban adım (derece/döngü)
+
-const int STEP_SPEED_ARM  = 2;    // Omuz/Dirsek adım
+// Oransal hız kontrol: joystick ne kadar itilirse o kadar hızlı
-const int STEP_SPEED_GRIP = 3;    // Kıskaç adım
+// 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();
+    // Ham joystick değerlerini oku
-    int ly = ctl->axisY();
+    float rawLX = (float)ctl->axisX();
-    int ry = ctl->axisRY();
+    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) ---
+    // --- TABAN / GÖVDE (Sol Analog X) - Oransal hız ---
-    if (lx < -DEADZONE) {
+    float baseSpeed = joystickToSpeed(smoothLX, MAX_SPEED_BASE);
-        baseAngle += INVERT_BASE ? STEP_SPEED_BASE : -STEP_SPEED_BASE;
+    if (baseSpeed != 0.0f) {
-    } else if (lx > DEADZONE) {
+        baseTarget += INVERT_BASE ? -baseSpeed : baseSpeed;
-        baseAngle += INVERT_BASE ? -STEP_SPEED_BASE : STEP_SPEED_BASE;
     }
 
-    // --- OMUZ (Sol Analog Y) ---
+    // --- OMUZ (Sol Analog Y) - Oransal hız ---
-    if (ly < -DEADZONE) {
+    float shoulderSpeed = joystickToSpeed(smoothLY, MAX_SPEED_ARM);
-        shoulderAngle += INVERT_SHOULDER ? -STEP_SPEED_ARM : STEP_SPEED_ARM;
+    if (shoulderSpeed != 0.0f) {
-    } else if (ly > DEADZONE) {
+        shoulderTarget += INVERT_SHOULDER ? -shoulderSpeed : shoulderSpeed;
-        shoulderAngle += INVERT_SHOULDER ? STEP_SPEED_ARM : -STEP_SPEED_ARM;
     }
 
-    // --- DİRSEK (Sağ Analog Y) ---
+    // --- DİRSEK (Sağ Analog Y) - Oransal hız ---
-    if (ry < -DEADZONE) {
+    float elbowSpeed = joystickToSpeed(smoothRY, MAX_SPEED_ARM);
-        elbowAngle += INVERT_ELBOW ? -STEP_SPEED_ARM : STEP_SPEED_ARM;
+    if (elbowSpeed != 0.0f) {
-    } else if (ry > DEADZONE) {
+        elbowTarget += INVERT_ELBOW ? -elbowSpeed : elbowSpeed;
-        elbowAngle += INVERT_ELBOW ? STEP_SPEED_ARM : -STEP_SPEED_ARM;
     }
 
     // --- 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 ---
+    // --- HEDEF AÇILARI SINIRLA ---
-    baseAngle     = constrain(baseAngle, BASE_MIN, BASE_MAX);
+    baseTarget     = constrain(baseTarget, (float)BASE_MIN, (float)BASE_MAX);
-    shoulderAngle = constrain(shoulderAngle, SHOULDER_MIN, SHOULDER_MAX);
+    shoulderTarget = constrain(shoulderTarget, (float)SHOULDER_MIN, (float)SHOULDER_MAX);
-    elbowAngle    = constrain(elbowAngle, ELBOW_MIN, ELBOW_MAX);
+    elbowTarget    = constrain(elbowTarget, (float)ELBOW_MIN, (float)ELBOW_MAX);
-    gripperAngle  = constrain(gripperAngle, GRIPPER_MIN, GRIPPER_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);
+    baseServo.attach(BASE_PIN, SERVO_MIN_US, SERVO_MAX_US);
-    shoulderServo.attach(SHOULDER_PIN, 500, 2400);
+    shoulderServo.attach(SHOULDER_PIN, SERVO_MIN_US, SERVO_MAX_US);
-    elbowServo.attach(ELBOW_PIN, 500, 2400);
+    elbowServo.attach(ELBOW_PIN, SERVO_MIN_US, SERVO_MAX_US);
-    gripperServo.attach(GRIPPER_PIN, 500, 2400);
+    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.");
 }
 
 // =====================================================================