truncgit/RobotKolProjesi
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Ümit Tunç
2026-05-26 14:33:38 +03:00
parent f035a88692
commit 1a2422e2aa
1 changed files with 104 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cpp
+104 -55
View File
@@ -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.");
}
// =====================================================================