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

feat: implement Bluepad32-based 4-servo robot arm control with auto-detach jitter reduction

Browse Source
This commit is contained in:
Ümit Tunç
2026-05-26 14:10:08 +03:00
parent 13f38be075
commit f035a88692
1 changed files with 203 additions and 162 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cpp
+203 -162
View File
@@ -2,42 +2,101 @@
#include <Bluepad32.h> #include <Bluepad32.h>
#include <ESP32Servo.h> #include <ESP32Servo.h>
// 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]; ControllerPtr myControllers[BP32_MAX_GAMEPADS];
// Servo motor nesnelerimiz // --- SERVO NESNELERİ ---
Servo baseServo; // Taban (GPIO 13) Servo baseServo;
Servo shoulderServo; // Omuz (GPIO 12) Servo shoulderServo;
Servo elbowServo; // Dirsek (GPIO 14) Servo elbowServo;
Servo gripperServo; // Kıskaç (GPIO 27) Servo gripperServo;
// Pin tanımlamaları // --- PIN TANIMLARI ---
const int BASE_PIN = 13; const int BASE_PIN = 13;
const int SHOULDER_PIN = 12; const int SHOULDER_PIN = 12;
const int ELBOW_PIN = 14; const int ELBOW_PIN = 14;
const int GRIPPER_PIN = 27; const int GRIPPER_PIN = 27;
// Başlangıç açıları (Genel olarak orta konumlarda başlatıyoruz) // --- AÇI LİMİTLERİ (her servo için ayrı ayrı) ---
float baseAngle = 90.0; const int BASE_MIN = 0; const int BASE_MAX = 150;
float shoulderAngle = 90.0; const int SHOULDER_MIN = 0; const int SHOULDER_MAX = 150;
float elbowAngle = 90.0; const int ELBOW_MIN = 0; const int ELBOW_MAX = 150;
float gripperAngle = 90.0; // Kıskaç da yumuşak hareket için 90'dan başlasın 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 // --- KALİBRASYON OFSETLERİ (montaj kayması düzeltme) ---
int lastBaseUs = -1; const int BASE_OFFSET = 0;
int lastShoulderUs = -1; const int SHOULDER_OFFSET = 0;
int lastElbowUs = -1; const int ELBOW_OFFSET = 0;
int lastGripperUs = -1; const int GRIPPER_OFFSET = 0;
// Joystick deadzone (ölü bölge) eşiği // --- BAŞLANGIÇ AÇILARI ---
const int DEADZONE = 50; 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ü) // --- SON YAZILAN AÇILAR (değişim kontrolü için) ---
const float MAX_SPEED_BASE = 2.2; // Taban motoru için maksimum dönüş hızı int lastBaseAngle = -1;
const float MAX_SPEED_ARM = 1.8; // Omuz ve dirsek motorları için maksimum hız int lastShoulderAngle = -1;
const float GRIPPER_SPEED = 2.0; // Kıskacın kapanma/açılma hızı (Daha seri) 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) { void onConnectedController(ControllerPtr ctl) {
for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
if (myControllers[i] == nullptr) { 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) { void onDisconnectedController(ControllerPtr ctl) {
for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
if (myControllers[i] == ctl) { if (myControllers[i] == ctl) {
@@ -59,187 +117,170 @@ void onDisconnectedController(ControllerPtr ctl) {
} }
} }
// Gamepad verilerini işleyen fonksiyon // =====================================================================
// GAMEPAD İŞLEME
// =====================================================================
void processGamepad(ControllerPtr ctl) { void processGamepad(ControllerPtr ctl) {
// Sol ve Sağ analog çubuk değerlerini alıyoruz
int lx = ctl->axisX(); int lx = ctl->axisX();
int ly = ctl->axisY(); int ly = ctl->axisY();
int ry = ctl->axisRY(); 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) unsigned long now = millis();
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;
bool curA = ctl->a(); // --- TABAN (Sol Analog X) ---
bool curB = ctl->b(); if (lx < -DEADZONE) {
bool curX = ctl->x(); baseAngle += INVERT_BASE ? STEP_SPEED_BASE : -STEP_SPEED_BASE;
bool curY = ctl->y(); } else if (lx > DEADZONE) {
baseAngle += INVERT_BASE ? -STEP_SPEED_BASE : STEP_SPEED_BASE;
// 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;
} }
// Buton B Loglaması // --- OMUZ (Sol Analog Y) ---
if (curB != prevB) { if (ly < -DEADZONE) {
if (curB) { shoulderAngle += INVERT_SHOULDER ? -STEP_SPEED_ARM : STEP_SPEED_ARM;
Serial.println("[INPUT] B Butonuna Basildi -> Kiskac Kapatiliyor (Yumusak)"); } else if (ly > DEADZONE) {
} else { shoulderAngle += INVERT_SHOULDER ? STEP_SPEED_ARM : -STEP_SPEED_ARM;
Serial.println("[INPUT] B Butonu Birakildi");
}
prevB = curB;
} }
// Buton X Loglaması // --- DİRSEK (Sağ Analog Y) ---
if (curX != prevX) { if (ry < -DEADZONE) {
if (curX) { elbowAngle += INVERT_ELBOW ? -STEP_SPEED_ARM : STEP_SPEED_ARM;
Serial.println("[INPUT] X Butonuna Basildi"); } else if (ry > DEADZONE) {
} else { elbowAngle += INVERT_ELBOW ? STEP_SPEED_ARM : -STEP_SPEED_ARM;
Serial.println("[INPUT] X Butonu Birakildi");
}
prevX = curX;
} }
// Buton Y Loglaması // --- KISKAÇ (A=Aç, B=Kapat) ---
if (curY != prevY) { if (btnA) {
if (curY) { gripperAngle += INVERT_GRIPPER ? -STEP_SPEED_GRIP : STEP_SPEED_GRIP;
Serial.println("[INPUT] Y Butonuna Basildi"); } else if (btnB) {
} else { gripperAngle += INVERT_GRIPPER ? STEP_SPEED_GRIP : -STEP_SPEED_GRIP;
Serial.println("[INPUT] Y Butonu Birakildi");
}
prevY = curY;
} }
// Joystick hareketlerini loglama // --- AÇI SINIRLARINI UYGULA ---
if (abs(lx - prevLx) > 30 || abs(ly - prevLy) > 30 || abs(ry - prevRy) > 30) { baseAngle = constrain(baseAngle, BASE_MIN, BASE_MAX);
if (abs(lx) > DEADZONE || abs(ly) > DEADZONE || abs(ry) > DEADZONE) { shoulderAngle = constrain(shoulderAngle, SHOULDER_MIN, SHOULDER_MAX);
Serial.printf("[JOYSTICK] lx: %d, ly: %d, ry: %d | Aci -> Taban: %.1f, Omuz: %.1f, Dirsek: %.1f\n", elbowAngle = constrain(elbowAngle, ELBOW_MIN, ELBOW_MAX);
lx, ly, ry, baseAngle, shoulderAngle, elbowAngle); gripperAngle = constrain(gripperAngle, GRIPPER_MIN, GRIPPER_MAX);
}
prevLx = lx; // --- SADECE DEĞİŞEN SERVOYA YAZ ---
prevLy = ly; if (baseAngle != lastBaseAngle) {
prevRy = ry; 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) // --- DETACH: Hareket bittikten sonra PWM sinyalini kes ---
if (abs(lx) > DEADZONE) { // Servo hedefe ulaştı ve DETACH_DELAY süresi geçti -> detach et
float normalized = lx / 512.0; // Bu sayede motorlar boşta iken ASLA titremez/hareket etmez
baseAngle += (normalized * abs(normalized)) * MAX_SPEED_BASE; if (baseAttached && (now - baseLastMoveTime > DETACH_DELAY)) {
safeServoDetach(baseServo, baseAttached);
} }
if (shoulderAttached && (now - shoulderLastMoveTime > DETACH_DELAY)) {
// 2. OMUZ - Sol Analog Y (Karesel Tepki Eğrisi) safeServoDetach(shoulderServo, shoulderAttached);
if (abs(ly) > DEADZONE) {
float normalized = ly / 512.0;
shoulderAngle -= (normalized * abs(normalized)) * MAX_SPEED_ARM;
} }
if (elbowAttached && (now - elbowLastMoveTime > DETACH_DELAY)) {
// 3. DİRSEK - Sağ Analog Y (Karesel Tepki Eğrisi) safeServoDetach(elbowServo, elbowAttached);
if (abs(ry) > DEADZONE) {
float normalized = ry / 512.0;
elbowAngle -= (normalized * abs(normalized)) * MAX_SPEED_ARM;
} }
if (gripperAttached && (now - gripperLastMoveTime > DETACH_DELAY)) {
// 4. KISKAÇ - A ve B Butonları ile YUMUŞAK kontrol safeServoDetach(gripperServo, gripperAttached);
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;
} }
} }
void setup() { // =====================================================================
// Seri haberleşmeyi başlat (115200 baud) // SETUP
Serial.begin(115200); // =====================================================================
while (!Serial) {
delay(1); // Seri portun hazır olmasını bekle 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(0);
ESP32PWM::allocateTimer(1); ESP32PWM::allocateTimer(1);
ESP32PWM::allocateTimer(2); ESP32PWM::allocateTimer(2);
ESP32PWM::allocateTimer(3); 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); baseServo.setPeriodHertz(50);
shoulderServo.setPeriodHertz(50); shoulderServo.setPeriodHertz(50);
elbowServo.setPeriodHertz(50); elbowServo.setPeriodHertz(50);
gripperServo.setPeriodHertz(50); gripperServo.setPeriodHertz(50);
// Servoları ilgili GPIO pinlerine ata (Standart pulse aralıkları: 500-2400 mikrosaniye)
baseServo.attach(BASE_PIN, 500, 2400); baseServo.attach(BASE_PIN, 500, 2400);
shoulderServo.attach(SHOULDER_PIN, 500, 2400); shoulderServo.attach(SHOULDER_PIN, 500, 2400);
elbowServo.attach(ELBOW_PIN, 500, 2400); elbowServo.attach(ELBOW_PIN, 500, 2400);
gripperServo.attach(GRIPPER_PIN, 500, 2400); gripperServo.attach(GRIPPER_PIN, 500, 2400);
// İlk konumları yaz baseServo.write(baseAngle + BASE_OFFSET);
lastBaseUs = round(500 + (baseAngle / 180.0) * 1900.0); shoulderServo.write(shoulderAngle + SHOULDER_OFFSET);
lastShoulderUs = round(500 + (shoulderAngle / 180.0) * 1900.0); elbowServo.write(elbowAngle + ELBOW_OFFSET);
lastElbowUs = round(500 + (elbowAngle / 180.0) * 1900.0); gripperServo.write(gripperAngle + GRIPPER_OFFSET);
lastGripperUs = round(500 + (gripperAngle / 180.0) * 1900.0);
baseServo.writeMicroseconds(lastBaseUs); lastBaseAngle = baseAngle;
shoulderServo.writeMicroseconds(lastShoulderUs); lastShoulderAngle = shoulderAngle;
elbowServo.writeMicroseconds(lastElbowUs); lastElbowAngle = elbowAngle;
gripperServo.writeMicroseconds(lastGripperUs); 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); BP32.setup(&onConnectedController, &onDisconnectedController);
Serial.println("Sistem Hazir."); Serial.println("Sistem Hazir - Tum servolar detach edildi.");
} }
// =====================================================================
// LOOP
// =====================================================================
void loop() { void loop() {
// Bluetooth bağlantı durumunu ve gamepad verilerini günceller (Zorunlu)
BP32.update(); BP32.update();
// Bağlı olan tüm gamepad'leri işle
for (int i = 0; i < BP32_MAX_GAMEPADS; i++) { for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
ControllerPtr myController = myControllers[i]; ControllerPtr ctl = myControllers[i];
if (myController && myController->isConnected()) { if (ctl && ctl->isConnected()) {
processGamepad(myController); 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);
}