feat: implement Bluepad32-based 4-servo robot arm control with auto-detach jitter reduction
This commit is contained in:
Ümit Tunç
+199
-158
@@ -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);
|
||||
}
|
||||
|
||||
// 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;
|
||||
if (shoulderAttached && (now - shoulderLastMoveTime > DETACH_DELAY)) {
|
||||
safeServoDetach(shoulderServo, shoulderAttached);
|
||||
}
|
||||
|
||||
// 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;
|
||||
if (elbowAttached && (now - elbowLastMoveTime > DETACH_DELAY)) {
|
||||
safeServoDetach(elbowServo, elbowAttached);
|
||||
}
|
||||
|
||||
// 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;
|
||||
if (gripperAttached && (now - gripperLastMoveTime > DETACH_DELAY)) {
|
||||
safeServoDetach(gripperServo, gripperAttached);
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
// Seri haberleşmeyi başlat (115200 baud)
|
||||
Serial.begin(115200);
|
||||
while (!Serial) {
|
||||
delay(1); // Seri portun hazır olmasını bekle
|
||||
}
|
||||
// =====================================================================
|
||||
// SETUP
|
||||
// =====================================================================
|
||||
|
||||
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(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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user