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
+200 -159
View File
@@ -2,42 +2,101 @@
#include <Bluepad32.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];
// 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ı
// --- PIN TANIMLARI ---
const int BASE_PIN = 13;
const int SHOULDER_PIN = 12;
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);
// --- 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);
}
prevLx = lx;
prevLy = ly;
prevRy = ry;
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);
}
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);
}
}
// 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;
}
// 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;
}
// 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;
}
}
// =====================================================================
// SETUP
// =====================================================================
void setup() {
// Seri haberleşmeyi başlat (115200 baud)
Serial.begin(115200);
while (!Serial) {
delay(1); // Seri portun hazır olmasını bekle
}
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);
}